1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +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 2012-07-30 20:55:43 -05:00
parent e01c5d5f87
commit 44e72d0a70

View File

@ -97,7 +97,7 @@ static float sin_lookup[360];
// Private functions // Private functions
static void stabilizationTask(void* parameters); static void stabilizationTask(void* parameters);
static float ApplyPid(pid_type * pid, const float err, float dT); 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 ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent * ev); static void SettingsUpdatedCb(UAVObjEvent * ev);
@ -116,10 +116,10 @@ int32_t StabilizationStart()
// Listen for updates. // Listen for updates.
// AttitudeActualConnectQueue(queue); // AttitudeActualConnectQueue(queue);
GyrosConnectQueue(queue); GyrosConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb); StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle()); SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task // Start main task
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_STABILIZATION, taskHandle);
@ -140,7 +140,7 @@ int32_t StabilizationInitialize()
#if defined(DIAGNOSTICS) #if defined(DIAGNOSTICS)
RateDesiredInitialize(); RateDesiredInitialize();
#endif #endif
return 0; return 0;
} }
@ -152,73 +152,73 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart)
static void stabilizationTask(void* parameters) static void stabilizationTask(void* parameters)
{ {
UAVObjEvent ev; UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw(); uint32_t timeval = PIOS_DELAY_GetRaw();
ActuatorDesiredData actuatorDesired; ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
RateDesiredData rateDesired; RateDesiredData rateDesired;
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
GyrosData gyrosData; GyrosData gyrosData;
FlightStatusData flightStatus; FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL); SettingsUpdatedCb((UAVObjEvent *) NULL);
// Main task loop // Main task loop
ZeroPids(); ZeroPids();
while(1) { while(1) {
float dT; float dT;
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe // 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 ) if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{ {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue; continue;
} }
dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw(); timeval = PIOS_DELAY_GetRaw();
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
GyrosGet(&gyrosData); GyrosGet(&gyrosData);
#if defined(DIAGNOSTICS) #if defined(DIAGNOSTICS)
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
#endif #endif
#if defined(PIOS_QUATERNION_STABILIZATION) #if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory. // Quaternion calculation of error in each axis. Uses more memory.
float rpy_desired[3]; float rpy_desired[3];
float q_desired[4]; float q_desired[4];
float q_error[4]; float q_error[4];
float local_error[3]; float local_error[3];
// Essentially zero errors for anything in rate or none // Essentially zero errors for anything in rate or none
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[0] = stabDesired.Roll; rpy_desired[0] = stabDesired.Roll;
else else
rpy_desired[0] = attitudeActual.Roll; rpy_desired[0] = attitudeActual.Roll;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[1] = stabDesired.Pitch; rpy_desired[1] = stabDesired.Pitch;
else else
rpy_desired[1] = attitudeActual.Pitch; rpy_desired[1] = attitudeActual.Pitch;
if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
rpy_desired[2] = stabDesired.Yaw; rpy_desired[2] = stabDesired.Yaw;
else else
rpy_desired[2] = attitudeActual.Yaw; rpy_desired[2] = attitudeActual.Yaw;
RPY2Quaternion(rpy_desired, q_desired); RPY2Quaternion(rpy_desired, q_desired);
quat_inverse(q_desired); quat_inverse(q_desired);
quat_mult(q_desired, &attitudeActual.q1, q_error); quat_mult(q_desired, &attitudeActual.q1, q_error);
quat_inverse(q_error); quat_inverse(q_error);
Quaternion2RPY(q_error, local_error); Quaternion2RPY(q_error, local_error);
#else #else
// Simpler algorithm for CC, less memory // Simpler algorithm for CC, less memory
float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, 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; local_error[2] = fmodf(local_error[2] + 180, 360) - 180;
#endif #endif
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); 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[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (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 *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.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++) 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]) switch(stabDesired.StabilizationMode[i])
{ {
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAY:
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: 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: case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
{
if(reinit)
vbar_integral[i] = 0;
rateDesiredAxis[i] = attitudeDesiredAxis[i]; rateDesiredAxis[i] = attitudeDesiredAxis[i];
// Zero attitude and axis lock accumulators // Track the angle of the virtual flybar which includes a slow decay
pids[PID_ROLL + i].iAccumulator = 0; vbar_integral[i] = vbar_integral[i] * vbar_decay + gyro_filtered[i] * dT;
axis_lock_accum[i] = 0; vbar_integral[i] = bound(vbar_integral[i], settings.VbarMaxAngle);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: // Command signal can indicate how much to disregard the gyro feedback (fast flips)
{ float gyro_gain = 1.0f;
float weak_leveling = local_error[i] * weak_leveling_kp; 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) // Command signal is composed of stick input added to the gyro and virtual flybar
weak_leveling = weak_leveling_max; actuatorDesiredAxis[i] = rateDesiredAxis[i] * vbar_sensitivity[i] -
if(weak_leveling < -weak_leveling_max) gyro_gain * (vbar_integral[i] * pids[PID_VBAR_ROLL + i].i +
weak_leveling = -weak_leveling_max; 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; break;
} }
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i], dT); {
if (reinit)
if(rateDesiredAxis[i] > settings.MaximumRate[i]) pids[PID_RATE_ROLL + i].iAccumulator = 0;
rateDesiredAxis[i] = settings.MaximumRate[i];
else if(rateDesiredAxis[i] < -settings.MaximumRate[i]) float weak_leveling = local_error[i] * weak_leveling_kp;
rateDesiredAxis[i] = -settings.MaximumRate[i]; weak_leveling = bound(weak_leveling, weak_leveling_max);
// Compute desired rate as input biased towards leveling
axis_lock_accum[i] = 0; 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; break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit)
pids[PID_RATE_ROLL + i].iAccumulator = 0;
if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) { if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode // While getting strong commands act like rate mode
rateDesiredAxis[i] = attitudeDesiredAxis[i]; rateDesiredAxis[i] = attitudeDesiredAxis[i];
@ -287,59 +334,46 @@ static void stabilizationTask(void* parameters)
} else { } else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change // For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT;
if(axis_lock_accum[i] > max_axis_lock) axis_lock_accum[i] = bound(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;
rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT); rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
} }
if(rateDesiredAxis[i] > settings.MaximumRate[i]) rateDesiredAxis[i] = bound(rateDesiredAxis[i], settings.MaximumRate[i]);
rateDesiredAxis[i] = settings.MaximumRate[i];
else if(rateDesiredAxis[i] < -settings.MaximumRate[i]) actuatorDesiredAxis[i] = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT);
rateDesiredAxis[i] = -settings.MaximumRate[i]; actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i],1.0f);
break; break;
}
}
// Piro compensation rotates the virtual flybar by yaw step to keep it case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE:
// 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:
{ {
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; RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings); RelayTuningSettingsGet(&relaySettings);
float error = rateDesiredAxis[i] - gyro_filtered[i]; float error = rateDesiredAxis[i] - gyro_filtered[i];
float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude; float command = error > 0 ? relaySettings.Amplitude : -relaySettings.Amplitude;
actuatorDesiredAxis[i] = bound(command); actuatorDesiredAxis[i] = bound(command,1.0f);
RelayTuningData relay;
RelayTuningGet(&relay);
static bool high = false; static bool high = false;
static portTickType lastHighTime; static portTickType lastHighTime;
@ -371,6 +405,8 @@ static void stabilizationTask(void* parameters)
bool hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; bool hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME;
if ( !high && hysteresis && error > 0 ){ /* RISE DETECTED */ if ( !high && hysteresis && error > 0 ){ /* RISE DETECTED */
float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated; float this_amplitude = 2 * sqrtf(accum_sin*accum_sin + accum_cos*accum_cos) / accumulated;
float this_gain = this_amplitude / relaySettings.Amplitude;
accumulated = 0; accumulated = 0;
accum_sin = 0; accum_sin = 0;
accum_cos = 0; accum_cos = 0;
@ -378,10 +414,10 @@ static void stabilizationTask(void* parameters)
if(rateRelayRunning[i] == false) { if(rateRelayRunning[i] == false) {
rateRelayRunning[i] = true; rateRelayRunning[i] = true;
relay.Period[i] = 200; relay.Period[i] = 200;
relay.Amplitude[i] = 0; relay.Gain[i] = 0;
} else { } else {
// Low pass filter each amplitude and period // 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); relay.Period[i] = relay.Period[i] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA);
} }
lastHighTime = thisTime; lastHighTime = thisTime;
@ -394,110 +430,165 @@ static void stabilizationTask(void* parameters)
break; break;
} }
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{ {
rateRelayRunning[i] = false; RelayTuningData relay;
float command = ApplyPid(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i] - gyro_filtered[i], dT); RelayTuningGet(&relay);
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;
// Command signal is composed of stick input added to the gyro and virtual flybar static bool rateRelayRunning[MAX_AXES];
float gyro_gain = 1.0f;
if (vbar_gyros_suppress > 0) { // On first run initialize estimates to something reasonable
gyro_gain = (1.0f - fabs(rateDesiredAxis[i]) * vbar_gyros_suppress / 100.0f); if(reinit) {
gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain; 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); // Replace the rate PID with a relay to measure the critical properties of this axis
break; // 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: case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
rateRelayRunning[i] = false; actuatorDesiredAxis[i] = bound(attitudeDesiredAxis[i],1.0f);
switch (i) break;
{ default:
case ROLL: error = true;
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;
}
break; 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 // Save dT
actuatorDesired.UpdateTime = dT * 1000; actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Throttle = stabDesired.Throttle;
if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
shouldUpdate = 0;
if(shouldUpdate)
{
actuatorDesired.Throttle = stabDesired.Throttle;
if(dT > 15)
actuatorDesired.NumLongUpdates++;
ActuatorDesiredSet(&actuatorDesired); 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 || if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0) || (lowThrottleZeroIntegral && stabDesired.Throttle < 0))
!shouldUpdate)
{ {
ZeroPids(); // Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
} }
// Clear or set alarms. Done like this to prevent toggline each cycle
// Clear alarms // and hammering system alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); 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 ApplyPid(pid_type * pid, const float err, float dT)
{ {
float diff = (err - pid->lastErr); float diff = (err - pid->lastErr);
pid->lastErr = err; pid->lastErr = err;
// Scale up accumulator by 1000 while computing to avoid losing precision // Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f); pid->iAccumulator += err * (pid->i * dT * 1000.0f);
if(pid->iAccumulator > (pid->iLim * 1000.0f)) { pid->iAccumulator = bound(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;
}
return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT)); 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) static void ZeroPids(void)
{ {
for(int8_t ct = 0; ct < PID_MAX; ct++) { for(int8_t ct = 0; ct < PID_MAX; ct++) {
@ -512,12 +603,12 @@ static void ZeroPids(void)
/** /**
* Bound input value between limits * Bound input value between limits
*/ */
static float bound(float val) static float bound(float val, float range)
{ {
if(val < -1.0f) { if(val < -range) {
val = -1.0f; val = -range;
} else if(val > 1.0f) { } else if(val > range) {
val = 1.0f; val = range;
} }
return val; return val;
} }
@ -525,40 +616,40 @@ static float bound(float val)
static void SettingsUpdatedCb(UAVObjEvent * ev) static void SettingsUpdatedCb(UAVObjEvent * ev)
{ {
StabilizationSettingsGet(&settings); StabilizationSettingsGet(&settings);
// Set the roll rate PID constants // Set the roll rate PID constants
pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP]; pids[PID_RATE_ROLL].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP];
pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI]; pids[PID_RATE_ROLL].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI];
pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD]; pids[PID_RATE_ROLL].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD];
pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]; pids[PID_RATE_ROLL].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT];
// Set the pitch rate PID constants // Set the pitch rate PID constants
pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP]; pids[PID_RATE_PITCH].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP];
pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI]; pids[PID_RATE_PITCH].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI];
pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD]; pids[PID_RATE_PITCH].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD];
pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]; pids[PID_RATE_PITCH].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT];
// Set the yaw rate PID constants // Set the yaw rate PID constants
pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP]; pids[PID_RATE_YAW].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP];
pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI]; pids[PID_RATE_YAW].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI];
pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD]; pids[PID_RATE_YAW].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD];
pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]; pids[PID_RATE_YAW].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT];
// Set the roll attitude PI constants // Set the roll attitude PI constants
pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP]; pids[PID_ROLL].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP];
pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI]; pids[PID_ROLL].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI];
pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]; pids[PID_ROLL].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT];
// Set the pitch attitude PI constants // Set the pitch attitude PI constants
pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP]; pids[PID_PITCH].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP];
pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI]; pids[PID_PITCH].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI];
pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]; pids[PID_PITCH].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT];
// Set the yaw attitude PI constants // Set the yaw attitude PI constants
pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP]; pids[PID_YAW].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP];
pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI]; pids[PID_YAW].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]; pids[PID_YAW].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
// Set the roll attitude PI constants // Set the roll attitude PI constants
pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP]; pids[PID_VBAR_ROLL].p = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KP];
pids[PID_VBAR_ROLL].i = settings.VbarRollPI[STABILIZATIONSETTINGS_VBARROLLPI_KI]; 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 // Set the yaw attitude PI constants
pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP]; pids[PID_VBAR_YAW].p = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KP];
pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI]; pids[PID_VBAR_YAW].i = settings.VbarYawPI[STABILIZATIONSETTINGS_VBARYAWPI_KI];
// Need to store the vbar sensitivity // Need to store the vbar sensitivity
vbar_sensitivity[0] = settings.VbarSensitivity[0]; vbar_sensitivity[0] = settings.VbarSensitivity[0];
vbar_sensitivity[1] = settings.VbarSensitivity[1]; vbar_sensitivity[1] = settings.VbarSensitivity[1];
vbar_sensitivity[2] = settings.VbarSensitivity[2]; vbar_sensitivity[2] = settings.VbarSensitivity[2];
// Maximum deviation to accumulate for axis lock // Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock; max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate; max_axislock_rate = settings.MaxAxisLockRate;
// Settings for weak leveling // Settings for weak leveling
weak_leveling_kp = settings.WeakLevelingKp; weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate; weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while throttle is low // Whether to zero the PID integrals while throttle is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// The dT has some jitter iteration to iteration that we don't want to // 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 // 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 // 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 gyro_alpha = 0; // not trusting this to resolve to 0
else else
gyro_alpha = expf(-fakeDt / settings.GyroTau); gyro_alpha = expf(-fakeDt / settings.GyroTau);
// Compute time constant for vbar decay term based on a tau // Compute time constant for vbar decay term based on a tau
vbar_decay = expf(-fakeDt / settings.VbarTau); vbar_decay = expf(-fakeDt / settings.VbarTau);
vbar_gyros_suppress = settings.VbarGyroSuppress; vbar_gyros_suppress = settings.VbarGyroSuppress;
@ -609,3 +700,4 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
* @} * @}
* @} * @}
*/ */