/** * 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 }