/**
* 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);
}
}
}
void AltitudeHoldSettingsThrustExpGet(uint8_t *NewThrustExp)
{
UAVObjGetDataField(AltitudeHoldSettingsHandle(), (void *)NewThrustExp, offsetof(AltitudeHoldSettingsData, ThrustExp), sizeof(uint8_t));
}
void AltitudeHoldSettingsCutThrustWhenZeroGet(uint8_t *NewCutThrustWhenZero)
{
UAVObjGetDataField(AltitudeHoldSettingsHandle(), (void *)NewCutThrustWhenZero, offsetof(AltitudeHoldSettingsData, CutThrustWhenZero), sizeof(uint8_t));
}
void AltitudeHoldSettingsThrustRateGet(float *NewThrustRate)
{
UAVObjGetDataField(AltitudeHoldSettingsHandle(), (void *)NewThrustRate, offsetof(AltitudeHoldSettingsData, ThrustRate), sizeof(float));
}
void AltitudeHoldSettingsVelocityPIArrayGet( float *NewVelocityPI )
{
UAVObjGetDataField(AltitudeHoldSettingsHandle(), (void *)NewVelocityPI, offsetof(AltitudeHoldSettingsData, VelocityPI), 3*sizeof(float));
}
void AltitudeHoldSettingsAltitudePIArraySet( float *NewAltitudePI )
{
UAVObjSetDataField(AltitudeHoldSettingsHandle(), (void *)NewAltitudePI, offsetof(AltitudeHoldSettingsData, AltitudePI), 3*sizeof(float));
}
