mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-30 08:24:11 +01:00
A big refactoring of stabilization.c to get rid of the two separate loops and
move them into one big structure. This makes it easier to implement other modes.
This commit is contained in:
James Cotton
parent
e01c5d5f87
commit
44e72d0a70
@ -97,7 +97,7 @@ static float sin_lookup[360];
|
||||
// Private functions
|
||||
static void stabilizationTask(void* parameters);
|
||||
static float ApplyPid(pid_type * pid, const float err, float dT);
|
||||
static float bound(float val);
|
||||
static float bound(float val, float range);
|
||||
static void ZeroPids(void);
|
||||
static void SettingsUpdatedCb(UAVObjEvent * ev);
|
||||
|
||||
@ -116,10 +116,10 @@ int32_t StabilizationStart()
|
||||
// Listen for updates.
|
||||
// AttitudeActualConnectQueue(queue);
|
||||
GyrosConnectQueue(queue);
|
||||
|
||||
|
||||
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
|
||||
SettingsUpdatedCb(StabilizationSettingsHandle());
|
||||
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle);
|
||||
@ -140,7 +140,7 @@ int32_t StabilizationInitialize()
|
||||
#if defined(DIAGNOSTICS)
|
||||
RateDesiredInitialize();
|
||||
#endif
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -152,73 +152,73 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
|
||||
static void stabilizationTask(void* parameters)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
|
||||
uint32_t timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
|
||||
ActuatorDesiredData actuatorDesired;
|
||||
StabilizationDesiredData stabDesired;
|
||||
RateDesiredData rateDesired;
|
||||
AttitudeActualData attitudeActual;
|
||||
GyrosData gyrosData;
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
|
||||
SettingsUpdatedCb((UAVObjEvent *) NULL);
|
||||
|
||||
|
||||
// Main task loop
|
||||
ZeroPids();
|
||||
while(1) {
|
||||
float dT;
|
||||
|
||||
|
||||
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
|
||||
|
||||
|
||||
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
|
||||
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
|
||||
FlightStatusGet(&flightStatus);
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
GyrosGet(&gyrosData);
|
||||
|
||||
|
||||
#if defined(DIAGNOSTICS)
|
||||
RateDesiredGet(&rateDesired);
|
||||
#endif
|
||||
|
||||
|
||||
#if defined(PIOS_QUATERNION_STABILIZATION)
|
||||
// Quaternion calculation of error in each axis. Uses more memory.
|
||||
float rpy_desired[3];
|
||||
float q_desired[4];
|
||||
float q_error[4];
|
||||
float local_error[3];
|
||||
|
||||
|
||||
// Essentially zero errors for anything in rate or none
|
||||
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
|
||||
rpy_desired[0] = stabDesired.Roll;
|
||||
else
|
||||
rpy_desired[0] = attitudeActual.Roll;
|
||||
|
||||
|
||||
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
|
||||
rpy_desired[1] = stabDesired.Pitch;
|
||||
else
|
||||
rpy_desired[1] = attitudeActual.Pitch;
|
||||
|
||||
|
||||
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
|
||||
rpy_desired[2] = stabDesired.Yaw;
|
||||
else
|
||||
rpy_desired[2] = attitudeActual.Yaw;
|
||||
|
||||
|
||||
RPY2Quaternion(rpy_desired, q_desired);
|
||||
quat_inverse(q_desired);
|
||||
quat_mult(q_desired, &attitudeActual.q1, q_error);
|
||||
quat_inverse(q_error);
|
||||
Quaternion2RPY(q_error, local_error);
|
||||
|
||||
|
||||
#else
|
||||
// Simpler algorithm for CC, less memory
|
||||
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll,
|
||||
@ -227,7 +227,6 @@ static void stabilizationTask(void* parameters)
|
||||
local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
|
||||
#endif
|
||||
|
||||
|
||||
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
|
||||
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
|
||||
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
|
||||
@ -236,50 +235,98 @@ static void stabilizationTask(void* parameters)
|
||||
float *actuatorDesiredAxis = &actuatorDesired.Roll;
|
||||
float *rateDesiredAxis = &rateDesired.Roll;
|
||||
|
||||
//Calculate desired rate
|
||||
ActuatorDesiredGet(&actuatorDesired);
|
||||
|
||||
// A flag to track which stabilization mode each axis is in
|
||||
static uint8_t previous_mode[MAX_AXES] = {255,255,255};
|
||||
bool error = false;
|
||||
|
||||
//Run the selected stabilization algorithm on each axis:
|
||||
for(uint8_t i=0; i< MAX_AXES; i++)
|
||||
{
|
||||
// Check whether this axis mode needs to be reinitialized
|
||||
bool reinit = (stabDesired.StabilizationMode[i] == previous_mode[i]);
|
||||
previous_mode[i] = stabDesired.StabilizationMode[i];
|
||||
|
||||
// Apply the selected control law
|
||||
switch(stabDesired.StabilizationMode[i])
|
||||
{
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
|
||||
if(reinit)
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
|
||||
// Store to rate desired variable for storing to UAVO
|
||||
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
|
||||
|
||||
// Compute the inner loop
|
||||
actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
|
||||
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
if(reinit) {
|
||||
pids[PID_ROLL + i].iAccumulator = 0;
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
}
|
||||
|
||||
// Compute the outer loop
|
||||
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
|
||||
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
|
||||
|
||||
// Compute the inner loop
|
||||
actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
|
||||
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
||||
{
|
||||
if(reinit)
|
||||
vbar_integral[i] = 0;
|
||||
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i];
|
||||
|
||||
// Zero attitude and axis lock accumulators
|
||||
pids[PID_ROLL + i].iAccumulator = 0;
|
||||
axis_lock_accum[i] = 0;
|
||||
break;
|
||||
// Track the angle of the virtual flybar which includes a slow decay
|
||||
vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT;
|
||||
vbar_integral[i] = bound(vbar_integral[i], settings.VbarMaxAngle);
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
|
||||
{
|
||||
float weak_leveling = local_error[i] * weak_leveling_kp;
|
||||
// Command signal can indicate how much to disregard the gyro feedback (fast flips)
|
||||
float gyro_gain = 1.0f;
|
||||
if (vbar_gyros_suppress > 0) {
|
||||
gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f);
|
||||
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
|
||||
}
|
||||
|
||||
if(weak_leveling > weak_leveling_max)
|
||||
weak_leveling = weak_leveling_max;
|
||||
if(weak_leveling < -weak_leveling_max)
|
||||
weak_leveling = -weak_leveling_max;
|
||||
// Command signal is composed of stick input added to the gyro and virtual flybar
|
||||
actuatorDesiredAxis[i] = rateDesiredAxis[i] * vbar_sensitivity[i] -
|
||||
gyro_gain * (vbar_integral[i] * pids[PID_VBAR_ROLL + i].i +
|
||||
gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p);
|
||||
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
|
||||
|
||||
// Zero attitude and axis lock accumulators
|
||||
pids[PID_ROLL + i].iAccumulator = 0;
|
||||
axis_lock_accum[i] = 0;
|
||||
break;
|
||||
}
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
|
||||
|
||||
if(rateDesiredAxis[i] > settings.MaximumRate[i])
|
||||
rateDesiredAxis[i] = settings.MaximumRate[i];
|
||||
else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
|
||||
rateDesiredAxis[i] = -settings.MaximumRate[i];
|
||||
|
||||
|
||||
axis_lock_accum[i] = 0;
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
|
||||
{
|
||||
if (reinit)
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
|
||||
float weak_leveling = local_error[i] * weak_leveling_kp;
|
||||
weak_leveling = bound(weak_leveling, weak_leveling_max);
|
||||
|
||||
// Compute desired rate as input biased towards leveling
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling;
|
||||
actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
|
||||
if (reinit)
|
||||
pids[PID_RATE_ROLL + i].iAccumulator = 0;
|
||||
|
||||
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
|
||||
// While getting strong commands act like rate mode
|
||||
rateDesiredAxis[i] = attitudeDesiredAxis[i];
|
||||
@ -287,59 +334,46 @@ static void stabilizationTask(void* parameters)
|
||||
} else {
|
||||
// For weaker commands or no command simply attitude lock (almost) on no gyro change
|
||||
axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
|
||||
if(axis_lock_accum[i] > max_axis_lock)
|
||||
axis_lock_accum[i] = max_axis_lock;
|
||||
else if(axis_lock_accum[i] < -max_axis_lock)
|
||||
axis_lock_accum[i] = -max_axis_lock;
|
||||
|
||||
axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
|
||||
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
|
||||
}
|
||||
|
||||
if(rateDesiredAxis[i] > settings.MaximumRate[i])
|
||||
rateDesiredAxis[i] = settings.MaximumRate[i];
|
||||
else if(rateDesiredAxis[i] < -settings.MaximumRate[i])
|
||||
rateDesiredAxis[i] = -settings.MaximumRate[i];
|
||||
|
||||
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
|
||||
|
||||
actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Piro compensation rotates the virtual flybar by yaw step to keep it
|
||||
// rotated to external world
|
||||
if (vbar_piro_comp) {
|
||||
const float F_PI = 3.14159f;
|
||||
float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT);
|
||||
float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT);
|
||||
|
||||
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
|
||||
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
|
||||
|
||||
vbar_integral[1] = vbar_pitch;
|
||||
vbar_integral[0] = vbar_roll;
|
||||
}
|
||||
|
||||
uint8_t shouldUpdate = 1;
|
||||
#if defined(DIAGNOSTICS)
|
||||
RateDesiredSet(&rateDesired);
|
||||
#endif
|
||||
|
||||
static bool rateRelayRunning[3] = {false, false, false};
|
||||
ActuatorDesiredGet(&actuatorDesired);
|
||||
//Calculate desired command
|
||||
for(uint32_t i = 0; i < MAX_AXES; i++)
|
||||
{
|
||||
switch(stabDesired.StabilizationMode[i])
|
||||
{
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
|
||||
{
|
||||
RelayTuningData relay;
|
||||
RelayTuningGet(&relay);
|
||||
|
||||
static bool rateRelayRunning[MAX_AXES];
|
||||
|
||||
// On first run initialize estimates to something reasonable
|
||||
if(reinit) {
|
||||
pids[PID_ROLL + i].iAccumulator = 0;
|
||||
rateRelayRunning[i] = false;
|
||||
relay.Period[i] = 200;
|
||||
relay.Gain[i] = 0;
|
||||
}
|
||||
// Replace the rate PID with a relay to measure the critical properties of this axis
|
||||
// i.e. period and gain
|
||||
|
||||
// Compute the outer loop
|
||||
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT);
|
||||
rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
|
||||
|
||||
// Store to rate desired variable for storing to UAVO
|
||||
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
|
||||
|
||||
RelayTuningSettingsData relaySettings;
|
||||
RelayTuningSettingsGet(&relaySettings);
|
||||
float error = rateDesiredAxis[i] - gyro_filtered[i];
|
||||
float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude;
|
||||
actuatorDesiredAxis[i] = bound(command);
|
||||
|
||||
RelayTuningData relay;
|
||||
RelayTuningGet(&relay);
|
||||
actuatorDesiredAxis[i] = bound(command,1.0f);
|
||||
|
||||
static bool high = false;
|
||||
static portTickType lastHighTime;
|
||||
@ -371,6 +405,8 @@ static void stabilizationTask(void* parameters)
|
||||
bool hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
|
||||
if ( !high && hysteresis && error > 0 ){ /* RISE DETECTED */
|
||||
float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated;
|
||||
float this_gain = this_amplitude / relaySettings.Amplitude;
|
||||
|
||||
accumulated = 0;
|
||||
accum_sin = 0;
|
||||
accum_cos = 0;
|
||||
@ -378,10 +414,10 @@ static void stabilizationTask(void* parameters)
|
||||
if(rateRelayRunning[i] == false) {
|
||||
rateRelayRunning[i] = true;
|
||||
relay.Period[i] = 200;
|
||||
relay.Amplitude[i] = 0;
|
||||
relay.Gain[i] = 0;
|
||||
} else {
|
||||
// Low pass filter each amplitude and period
|
||||
relay.Amplitude[i] = relay.Amplitude[i] * AMPLITUDE_ALPHA + this_amplitude * (1 - AMPLITUDE_ALPHA);
|
||||
relay.Gain[i] = relay.Gain[i] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
|
||||
relay.Period[i] = relay.Period[i] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
|
||||
}
|
||||
lastHighTime = thisTime;
|
||||
@ -394,110 +430,165 @@ static void stabilizationTask(void* parameters)
|
||||
|
||||
break;
|
||||
}
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
|
||||
{
|
||||
rateRelayRunning[i] = false;
|
||||
float command = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
|
||||
actuatorDesiredAxis[i] = bound(command);
|
||||
break;
|
||||
}
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
||||
{
|
||||
rateRelayRunning[i] = false;
|
||||
// Track the angle of the virtual flybar which includes a slow decay
|
||||
vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT;
|
||||
if (vbar_integral[i] > settings.VbarMaxAngle)
|
||||
vbar_integral[i] = settings.VbarMaxAngle;
|
||||
if (-vbar_integral[i] < -settings.VbarMaxAngle)
|
||||
vbar_integral[i] = -settings.VbarMaxAngle;
|
||||
RelayTuningData relay;
|
||||
RelayTuningGet(&relay);
|
||||
|
||||
// Command signal is composed of stick input added to the gyro and virtual flybar
|
||||
float gyro_gain = 1.0f;
|
||||
if (vbar_gyros_suppress > 0) {
|
||||
gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f);
|
||||
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
|
||||
static bool rateRelayRunning[MAX_AXES];
|
||||
|
||||
// On first run initialize estimates to something reasonable
|
||||
if(reinit) {
|
||||
pids[PID_ROLL + i].iAccumulator = 0;
|
||||
rateRelayRunning[i] = false;
|
||||
relay.Period[i] = 200;
|
||||
relay.Gain[i] = 0;
|
||||
}
|
||||
float command = rateDesiredAxis[i] * vbar_sensitivity[i] - gyro_gain * (
|
||||
vbar_integral[i] * pids[PID_VBAR_ROLL + i].i +
|
||||
gyro_filtered[i] * pids[PID_VBAR_ROLL + i].p);
|
||||
|
||||
actuatorDesiredAxis[i] = bound(command);
|
||||
break;
|
||||
// Replace the rate PID with a relay to measure the critical properties of this axis
|
||||
// i.e. period and gain
|
||||
|
||||
// Store to rate desired variable for storing to UAVO
|
||||
rateDesiredAxis[i] = bound(attitudeDesiredAxis[i], settings.ManualRate[i]);
|
||||
|
||||
RelayTuningSettingsData relaySettings;
|
||||
RelayTuningSettingsGet(&relaySettings);
|
||||
float error = rateDesiredAxis[i] - gyro_filtered[i];
|
||||
float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude;
|
||||
actuatorDesiredAxis[i] = bound(command,1.0);
|
||||
|
||||
static bool high = false;
|
||||
static portTickType lastHighTime;
|
||||
static portTickType lastLowTime;
|
||||
portTickType thisTime = xTaskGetTickCount();
|
||||
|
||||
static float accum_sin, accum_cos;
|
||||
static uint32_t accumulated = 0;
|
||||
|
||||
const uint16_t DEGLITCH_TIME = 20; // ms
|
||||
const float AMPLITUDE_ALPHA = 0.95;
|
||||
const float PERIOD_ALPHA = 0.95;
|
||||
|
||||
// Make sure the period can't go below limit
|
||||
if (relay.Period[i] < DEGLITCH_TIME)
|
||||
relay.Period[i] = DEGLITCH_TIME;
|
||||
|
||||
// Project the error onto a sine and cosine of the same frequency
|
||||
// to accumulate the average amplitude
|
||||
float dT = thisTime - lastHighTime;
|
||||
uint32_t phase = 360 * dT / relay.Period[i];
|
||||
if(phase >= 360)
|
||||
phase = 1;
|
||||
accum_sin += sin_lookup[phase] * error;
|
||||
accum_cos += sin_lookup[(phase + 90) % 360] * error;
|
||||
accumulated ++;
|
||||
|
||||
// Make susre we've had enough time since last transition then check for a change in the output
|
||||
bool hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
|
||||
if ( !high && hysteresis && error > 0 ){ /* RISE DETECTED */
|
||||
float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated;
|
||||
float this_gain = this_amplitude / relaySettings.Amplitude;
|
||||
|
||||
accumulated = 0;
|
||||
accum_sin = 0;
|
||||
accum_cos = 0;
|
||||
|
||||
if(rateRelayRunning[i] == false) {
|
||||
rateRelayRunning[i] = true;
|
||||
relay.Period[i] = 200;
|
||||
relay.Gain[i] = 0;
|
||||
} else {
|
||||
// Low pass filter each amplitude and period
|
||||
relay.Gain[i] = relay.Gain[i] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA);
|
||||
relay.Period[i] = relay.Period[i] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
|
||||
}
|
||||
lastHighTime = thisTime;
|
||||
high = true;
|
||||
RelayTuningSet(&relay);
|
||||
} else if ( high && hysteresis && error < 0 ) { /* FALL DETECTED */
|
||||
lastLowTime = thisTime;
|
||||
high = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
|
||||
rateRelayRunning[i] = false;
|
||||
switch (i)
|
||||
{
|
||||
case ROLL:
|
||||
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
|
||||
shouldUpdate = 1;
|
||||
pids[PID_RATE_ROLL].iAccumulator = 0;
|
||||
pids[PID_ROLL].iAccumulator = 0;
|
||||
break;
|
||||
case PITCH:
|
||||
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
|
||||
shouldUpdate = 1;
|
||||
pids[PID_RATE_PITCH].iAccumulator = 0;
|
||||
pids[PID_PITCH].iAccumulator = 0;
|
||||
break;
|
||||
case YAW:
|
||||
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i]);
|
||||
shouldUpdate = 1;
|
||||
pids[PID_RATE_YAW].iAccumulator = 0;
|
||||
pids[PID_YAW].iAccumulator = 0;
|
||||
break;
|
||||
}
|
||||
actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f);
|
||||
break;
|
||||
default:
|
||||
error = true;
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// Piro compensation rotates the virtual flybar by yaw step to keep it
|
||||
// rotated to external world
|
||||
if (vbar_piro_comp) {
|
||||
const float F_PI = 3.14159f;
|
||||
float cy = cosf(gyro_filtered[2] / 180.0f * F_PI * dT);
|
||||
float sy = sinf(gyro_filtered[2] / 180.0f * F_PI * dT);
|
||||
|
||||
float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
|
||||
float vbar_roll = sy * vbar_integral[1] + cy * vbar_integral[0];
|
||||
|
||||
vbar_integral[1] = vbar_pitch;
|
||||
vbar_integral[0] = vbar_roll;
|
||||
}
|
||||
|
||||
#if defined(DIAGNOSTICS)
|
||||
RateDesiredSet(&rateDesired);
|
||||
#endif
|
||||
|
||||
// Save dT
|
||||
actuatorDesired.UpdateTime = dT * 1000;
|
||||
actuatorDesired.Throttle = stabDesired.Throttle;
|
||||
|
||||
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL)
|
||||
shouldUpdate = 0;
|
||||
|
||||
if(shouldUpdate)
|
||||
{
|
||||
actuatorDesired.Throttle = stabDesired.Throttle;
|
||||
if(dT > 15)
|
||||
actuatorDesired.NumLongUpdates++;
|
||||
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
|
||||
ActuatorDesiredSet(&actuatorDesired);
|
||||
} else {
|
||||
// Force all axes to reinitialize when engaged
|
||||
for(uint8_t i=0; i< MAX_AXES; i++)
|
||||
previous_mode[i] = 255;
|
||||
}
|
||||
|
||||
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
|
||||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0) ||
|
||||
!shouldUpdate)
|
||||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0))
|
||||
{
|
||||
ZeroPids();
|
||||
// Force all axes to reinitialize when engaged
|
||||
for(uint8_t i=0; i< MAX_AXES; i++)
|
||||
previous_mode[i] = 255;
|
||||
}
|
||||
|
||||
|
||||
// Clear alarms
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
|
||||
// Clear or set alarms. Done like this to prevent toggline each cycle
|
||||
// and hammering system alarms
|
||||
if (error)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR);
|
||||
else
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update one of the PID structures with the input error and timestep
|
||||
* @param pid Pointer to the PID structure
|
||||
* @param[in] err The error on for this controller
|
||||
* @param[in] dT The time step since the last update
|
||||
*/
|
||||
float ApplyPid(pid_type * pid, const float err, float dT)
|
||||
{
|
||||
float diff = (err - pid->lastErr);
|
||||
pid->lastErr = err;
|
||||
|
||||
|
||||
// Scale up accumulator by 1000 while computing to avoid losing precision
|
||||
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
|
||||
if(pid->iAccumulator > (pid->iLim * 1000.0f)) {
|
||||
pid->iAccumulator = pid->iLim * 1000.0f;
|
||||
} else if (pid->iAccumulator < -(pid->iLim * 1000.0f)) {
|
||||
pid->iAccumulator = -pid->iLim * 1000.0f;
|
||||
}
|
||||
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
|
||||
return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Clear the accumulators and derivatives for all the axes
|
||||
*/
|
||||
static void ZeroPids(void)
|
||||
{
|
||||
for(int8_t ct = 0; ct < PID_MAX; ct++) {
|
||||
@ -512,12 +603,12 @@ static void ZeroPids(void)
|
||||
/**
|
||||
* Bound input value between limits
|
||||
*/
|
||||
static float bound(float val)
|
||||
static float bound(float val, float range)
|
||||
{
|
||||
if(val < -1.0f) {
|
||||
val = -1.0f;
|
||||
} else if(val > 1.0f) {
|
||||
val = 1.0f;
|
||||
if(val < -range) {
|
||||
val = -range;
|
||||
} else if(val > range) {
|
||||
val = range;
|
||||
}
|
||||
return val;
|
||||
}
|
||||
@ -525,40 +616,40 @@ static float bound(float val)
|
||||
static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
StabilizationSettingsGet(&settings);
|
||||
|
||||
|
||||
// Set the roll rate PID constants
|
||||
pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP];
|
||||
pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI];
|
||||
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD];
|
||||
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT];
|
||||
|
||||
|
||||
// Set the pitch rate PID constants
|
||||
pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP];
|
||||
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI];
|
||||
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD];
|
||||
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT];
|
||||
|
||||
|
||||
// Set the yaw rate PID constants
|
||||
pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP];
|
||||
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI];
|
||||
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD];
|
||||
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT];
|
||||
|
||||
|
||||
// Set the roll attitude PI constants
|
||||
pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP];
|
||||
pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI];
|
||||
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT];
|
||||
|
||||
|
||||
// Set the pitch attitude PI constants
|
||||
pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP];
|
||||
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI];
|
||||
pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT];
|
||||
|
||||
|
||||
// Set the yaw attitude PI constants
|
||||
pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP];
|
||||
pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
|
||||
pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
|
||||
|
||||
|
||||
// Set the roll attitude PI constants
|
||||
pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
|
||||
pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI];
|
||||
@ -570,23 +661,23 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
// Set the yaw attitude PI constants
|
||||
pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP];
|
||||
pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI];
|
||||
|
||||
|
||||
// Need to store the vbar sensitivity
|
||||
vbar_sensitivity[0] = settings.VbarSensitivity[0];
|
||||
vbar_sensitivity[1] = settings.VbarSensitivity[1];
|
||||
vbar_sensitivity[2] = settings.VbarSensitivity[2];
|
||||
|
||||
|
||||
// Maximum deviation to accumulate for axis lock
|
||||
max_axis_lock = settings.MaxAxisLock;
|
||||
max_axislock_rate = settings.MaxAxisLockRate;
|
||||
|
||||
|
||||
// Settings for weak leveling
|
||||
weak_leveling_kp = settings.WeakLevelingKp;
|
||||
weak_leveling_max = settings.MaxWeakLevelingRate;
|
||||
|
||||
|
||||
// Whether to zero the PID integrals while throttle is low
|
||||
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
|
||||
|
||||
|
||||
// The dT has some jitter iteration to iteration that we don't want to
|
||||
// make thie result unpredictable. Still, it's nicer to specify the constant
|
||||
// based on a time (in ms) rather than a fixed multiplier. The error between
|
||||
@ -597,7 +688,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
gyro_alpha = 0; // not trusting this to resolve to 0
|
||||
else
|
||||
gyro_alpha = expf(-fakeDt / settings.GyroTau);
|
||||
|
||||
|
||||
// Compute time constant for vbar decay term based on a tau
|
||||
vbar_decay = expf(-fakeDt / settings.VbarTau);
|
||||
vbar_gyros_suppress = settings.VbarGyroSuppress;
|
||||
@ -609,3 +700,4 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user