/**
* 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);
}
}
}
/**
* Module thread, should not return.
*/
static void airspeedTask(void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
BaroAirspeedData airspeedData;
AirspeedActualData airspeedActualData;
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
#ifdef BARO_AIRSPEED_PRESENT
portTickType lastGPSTime = xTaskGetTickCount(); //Time since last GPS-derived airspeed calculation
portTickType lastLoopTime= xTaskGetTickCount(); //Time since last loop
float airspeedErrInt=0;
#endif
//GPS airspeed calculation variables
#ifdef GPS_AIRSPEED_PRESENT
GPSVelocityConnectCallback(GPSVelocityUpdatedCb);
gps_airspeedInitialize();
#endif
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1)
{
// Update the airspeed object
BaroAirspeedGet(&airspeedData);
#ifdef BARO_AIRSPEED_PRESENT
float airspeed_tas_baro=0;
if(airspeedSensorType != AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) {
//Fetch calibrated airspeed from sensors
baro_airspeedGet(&airspeedData, &lastSysTime, airspeedSensorType, airspeedADCPin);
//Calculate true airspeed, not taking into account compressibility effects
int16_t groundTemperature_10;
float groundTemperature;
float positionActual_Down;
PositionActualDownGet(&positionActual_Down);
HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C
groundTemperature = groundTemperature_10/10; // Convert into degrees C
groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
struct AirParameters air_STP = initialize_air_structure();
air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN;
#ifdef GPS_AIRSPEED_PRESENT
//GPS present, so use baro sensor to filter TAS
airspeed_tas_baro = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#else
//No GPS, so TAS comes only from baro sensor
airspeedData.TrueAirspeed = cas2tas(airspeedData.CalibratedAirspeed, -positionActual_Down, &air_STP) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#endif
}
else
#endif
{ //Have to catch the fallthrough, or else this loop will monopolize the processor!
airspeedData.BaroConnected=BAROAIRSPEED_BAROCONNECTED_FALSE;
airspeedData.SensorValue=12345;
//Likely, we have a GPS, so let's configure the fallthrough at close to GPS refresh rates
vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH));
}
#ifdef GPS_AIRSPEED_PRESENT
float v_air_GPS=-1.0f;
//Check if sufficient time has passed. This will depend on whether we have a pitot tube
//sensor or not. In the case we do, shoot for about once per second. Otherwise, consume GPS
//as quickly as possible.
#ifdef BARO_AIRSPEED_PRESENT
float delT = TICKS2MS(lastSysTime - lastLoopTime) / 1000.0f;
lastLoopTime=lastSysTime;
if ( (TICKS2MS(lastSysTime - lastGPSTime) > 1000 || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY)
&& gpsNew) {
lastGPSTime=lastSysTime;
#else
if (gpsNew) {
#endif
gpsNew=false; //Do this first
//Calculate airspeed as a function of GPS groundspeed and vehicle attitude. From "IMU Wind Estimation (Theory)", by William Premerlani
gps_airspeedGet(&v_air_GPS);
}
//Use the GPS error to correct the airspeed estimate.
if (v_air_GPS > 0) //We have valid GPS estimate...
{
airspeedData.GPSAirspeed=v_air_GPS;
#ifdef BARO_AIRSPEED_PRESENT
if(airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){ //Check if there is an airspeed sensors present...
//Calculate error and error integral
float airspeedErr=v_air_GPS - airspeed_tas_baro;
airspeedErrInt+=airspeedErr * delT;
//Saturate integral component at 5 m/s
airspeedErrInt = airspeedErrInt > (5.0f / GPS_AIRSPEED_BIAS_KI) ? (5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
airspeedErrInt = airspeedErrInt < -(5.0f / GPS_AIRSPEED_BIAS_KI) ? -(5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
//There's already an airspeed sensor, so instead correct it for bias with P correction. The I correction happened earlier in the function.
airspeedData.TrueAirspeed = airspeed_tas_baro + airspeedErr * GPS_AIRSPEED_BIAS_KP;
/* Note:
This would be a good place to change the airspeed calibration, so that it matches the GPS computed values. However,
this might be a bad idea, as their are two degrees of freedom here: temperature and sensor calibration. I don't
know how to control for temperature bias.
*/
}
else
#endif
{
//...there's no airspeed sensor, so everything comes from GPS. In this
//case, filter the airspeed for smoother output
float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
airspeedData.TrueAirspeed=v_air_GPS*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
//Calculate calibrated airspeed from GPS, since we're not getting it from a discrete airspeed sensor
int16_t groundTemperature_10;
float groundTemperature;
float positionActual_Down;
PositionActualDownGet(&positionActual_Down);
HomeLocationGroundTemperatureGet(&groundTemperature_10); // Gets tenths of degrees C
groundTemperature = groundTemperature_10/10; // Convert into degrees C
groundTemperature -= BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
struct AirParameters air_STP = initialize_air_structure();
air_STP.air_temperature_at_surface = groundTemperature + CELSIUS2KELVIN;
airspeedData.CalibratedAirspeed = tas2cas(airspeedData.TrueAirspeed, -positionActual_Down, &air_STP);
}
}
#ifdef BARO_AIRSPEED_PRESENT
else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){
//No GPS velocity estimate this loop, so filter true airspeed data with baro airspeed
float alpha=delT/(delT + BARO_TRUEAIRSPEED_TIME_CONSTANT_S); //Low pass filter.
airspeedData.TrueAirspeed=airspeed_tas_baro*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
}
#endif
#endif
//Set the UAVO
airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed;
airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
BaroAirspeedSet(&airspeedData);
AirspeedActualSet(&airspeedActualData);
}
}
#ifdef GPS_AIRSPEED_PRESENT
static void GPSVelocityUpdatedCb(UAVObjEvent * ev)
{
gpsNew=true;
}
#endif
#ifdef BARO_AIRSPEED_PRESENT
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin_dummy)
{
//Find out which sensor we're using.
switch (airspeedSensorType) {
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
//MPXV5004 and MPXV7002 sensors
baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
//Eagletree Airspeed v3
baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
default:
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
vTaskDelayUntil(lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_FALLTHROUGH));
break;
}
}
#endif
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
{
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
airspeedSensorType=airspeedSettingsData.AirspeedSensorType;
gpsSamplePeriod_ms=airspeedSettingsData.GPSSamplePeriod_ms;
#if defined(PIOS_INCLUDE_MPXV7002)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
}
