/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void *parameters)
{
bool engaged = false;
AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired;
AltitudeHoldSettingsData altitudeHoldSettings;
UAVObjEvent ev;
struct pid velocity_pid;
// Listen for object updates.
AltitudeHoldDesiredConnectQueue(queue);
AltitudeHoldSettingsConnectQueue(queue);
FlightStatusConnectQueue(queue);
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK);
// Main task loop
const uint32_t dt_ms = 5;
const float dt_s = dt_ms * 0.001f;
uint32_t timeout = dt_ms;
while (1) {
if (PIOS_Queue_Receive(queue, &ev, timeout) != true) {
} else if (ev.obj == FlightStatusHandle()) {
uint8_t flight_mode;
FlightStatusFlightModeGet(&flight_mode);
if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) {
// Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator);
velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000
engaged = true;
} else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
engaged = false;
// Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged
timeout = engaged ? dt_ms : 100;
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
AltitudeHoldDesiredGet(&altitudeHoldDesired);
} else if (ev.obj == AltitudeHoldSettingsHandle()) {
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
}
bool landing = altitudeHoldDesired.Land == ALTITUDEHOLDDESIRED_LAND_TRUE;
// For landing mode allow throttle to go negative to allow the integrals
// to stop winding up
const float min_throttle = landing ? -0.1f : 0.0f;
// When engaged compute altitude controller output
if (engaged) {
float position_z, velocity_z, altitude_error;
PositionActualDownGet(&position_z);
VelocityActualDownGet(&velocity_z);
position_z = -position_z; // Use positive up convention
velocity_z = -velocity_z; // Use positive up convention
// Compute the altitude error
altitude_error = altitudeHoldDesired.Altitude - position_z;
// Velocity desired is from the outer controller plus the set point
float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate;
float throttle_desired = pid_apply_antiwindup(&velocity_pid,
velocity_desired - velocity_z,
min_throttle, 1.0f, // positive limits since this is throttle
dt_s);
if (altitudeHoldSettings.AttitudeComp > 0) {
// Throttle desired is at this point the mount desired in the up direction, we can
// account for the attitude if desired
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
// Project a unit vector pointing up into the body frame and
// get the z component
float fraction = attitudeActual.q1 * attitudeActual.q1 -
attitudeActual.q2 * attitudeActual.q2 -
attitudeActual.q3 * attitudeActual.q3 +
attitudeActual.q4 * attitudeActual.q4;
// Add ability to scale up the amount of compensation to achieve
// level forward flight
fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f);
// Dividing by the fraction remaining in the vertical projection will
// attempt to compensate for tilt. This acts like the thrust is linear
// with the output which isn't really true. If the fraction is starting
// to go negative we are inverted and should shut off throttle
throttle_desired = (fraction > 0.1f) ? (throttle_desired / fraction) : 0.0f;
}
StabilizationDesiredGet(&stabilizationDesired);
stabilizationDesired.Throttle = bound_min_max(throttle_desired, min_throttle, 1.0f);
if (landing) {
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = 0;
stabilizationDesired.Pitch = 0;
stabilizationDesired.Yaw = 0;
} else {
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
}
StabilizationDesiredSet(&stabilizationDesired);
}
}
}
static void calculate_pids()
{
// This scale will be calculated and allows suppressing the PID
// controller gain
float roll_scale = 1.0f;
float pitch_scale = 1.0f;
float yaw_scale = 1.0f;
// Fetch the current throttle settings
float throttle;
StabilizationDesiredThrottleGet(&throttle);
// Calculate the desired PID suppression based on throttle settings. This is
// similar to an algorithm used by MultiWii and empirically works well. It
// creates a piecewise linear suppression of PIDs versus throttle.
for (uint32_t i = 0; i < 3; i++) {
float attenuation;
float threshold;
float scale = 1.0f;
switch(i) {
case 0:
attenuation = settings.RollRateTPA[STABILIZATIONSETTINGS_ROLLRATETPA_ATTENUATION] / 100.0f;
threshold = settings.RollRateTPA[STABILIZATIONSETTINGS_ROLLRATETPA_THRESHOLD] / 100.0f;
break;
case 1:
attenuation = settings.RollRateTPA[STABILIZATIONSETTINGS_PITCHRATETPA_ATTENUATION] / 100.0f;
threshold = settings.RollRateTPA[STABILIZATIONSETTINGS_PITCHRATETPA_THRESHOLD] / 100.0f;
break;
case 2:
attenuation = settings.RollRateTPA[STABILIZATIONSETTINGS_YAWRATETPA_ATTENUATION] / 100.0f;
threshold = settings.RollRateTPA[STABILIZATIONSETTINGS_YAWRATETPA_THRESHOLD] / 100.0f;
break;
}
// Ensure everything is in a valid range to keep scale well behaved
if (throttle > 0 && throttle < 1.0f &&
attenuation > 0 && attenuation < 0.9f &&
threshold > 0 && threshold < 1) {
if (throttle > threshold)
scale = 1.0f - attenuation * (throttle - threshold) / (1.0f - threshold);
}
switch(i) {
case 0:
roll_scale = scale;
break;
case 1:
pitch_scale = scale;
break;
case 2:
yaw_scale = scale;
break;
}
}
// Set the roll rate PID constants
pid_configure(&pids[PID_RATE_ROLL],
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] * roll_scale,
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI],
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD] * roll_scale,
settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]);
// Set the pitch rate PID constants
pid_configure(&pids[PID_RATE_PITCH],
settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP] * pitch_scale,
settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI],
settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD] * pitch_scale,
settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]);
// Set the yaw rate PID constants
pid_configure(&pids[PID_RATE_YAW],
settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP] * yaw_scale,
settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI],
settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD] * yaw_scale,
settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]);
// Set the roll attitude PI constants
pid_configure(&pids[PID_ATT_ROLL],
settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP],
settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0,
settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]);
// Set the pitch attitude PI constants
pid_configure(&pids[PID_ATT_PITCH],
settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP],
settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0,
settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]);
// Set the yaw attitude PI constants
pid_configure(&pids[PID_ATT_YAW],
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP],
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0,
settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]);
// Set the vbar roll settings
pid_configure(&pids[PID_VBAR_ROLL],
settings.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP] * roll_scale,
settings.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI],
settings.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD] * roll_scale,
0);
// Set the vbar pitch settings
pid_configure(&pids[PID_VBAR_PITCH],
settings.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP] * pitch_scale,
settings.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI],
settings.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD] * pitch_scale,
0);
// Set the vbar yaw settings
pid_configure(&pids[PID_VBAR_YAW],
settings.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KP] * yaw_scale,
settings.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KI],
settings.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KD] * yaw_scale,
0);
// Set the coordinated flight settings
pid_configure(&pids[PID_COORDINATED_FLIGHT_YAW],
settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KP],
settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KI],
0, /* No derivative term */
settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_ILIMIT]);
// Set the mwrate roll settings
pid_configure(&pids[PID_MWR_ROLL],
mwrate_settings.RollRatePID[MWRATESETTINGS_ROLLRATEPID_KP] * roll_scale,
mwrate_settings.RollRatePID[MWRATESETTINGS_ROLLRATEPID_KI],
mwrate_settings.RollRatePID[MWRATESETTINGS_ROLLRATEPID_KD] * roll_scale,
mwrate_settings.RollRatePID[MWRATESETTINGS_ROLLRATEPID_ILIMIT]);
// Set the mwrate pitch settings
pid_configure(&pids[PID_MWR_PITCH],
mwrate_settings.PitchRatePID[MWRATESETTINGS_PITCHRATEPID_KP] * pitch_scale,
mwrate_settings.PitchRatePID[MWRATESETTINGS_PITCHRATEPID_KI],
mwrate_settings.PitchRatePID[MWRATESETTINGS_PITCHRATEPID_KD] * pitch_scale,
mwrate_settings.PitchRatePID[MWRATESETTINGS_PITCHRATEPID_ILIMIT]);
// Set the mwrate yaw settings
pid_configure(&pids[PID_MWR_YAW],
mwrate_settings.YawRatePID[MWRATESETTINGS_YAWRATEPID_KP] * yaw_scale,
mwrate_settings.YawRatePID[MWRATESETTINGS_YAWRATEPID_KI],
mwrate_settings.YawRatePID[MWRATESETTINGS_YAWRATEPID_KD] * yaw_scale,
mwrate_settings.YawRatePID[MWRATESETTINGS_YAWRATEPID_ILIMIT]);
// Set the coordinated flight settings
pid_configure(&pids[PID_COORDINATED_FLIGHT_YAW],
settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KP],
settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KI],
0, /* No derivative term */
settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_ILIMIT]);
// Set up the derivative term
pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);
}