From f7dd5cc8ada8a1761e18ac463c80fd85130077bf Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 28 Oct 2012 17:45:40 +0100 Subject: [PATCH] Refactored Airspeed and FixedWingPathFollowermodule to use AirspeedActual, which hitl writes to, too. Cleaned up both PathFollower and Airspeed a bit. --- flight/Modules/Airspeed/revolution/airspeed.c | 36 ++++++ ...baro_airspeed.c => baro_airspeed_analog.c} | 105 ++-------------- .../revolution/baro_airspeed_etasv3.c | 109 ++++++++++++++++ .../revolution/inc/baro_airspeed_analog.h | 43 +++++++ .../revolution/inc/baro_airspeed_etasv3.h | 43 +++++++ .../fixedwingpathfollower.c | 118 +++++++----------- flight/Revolution/UAVObjects.inc | 1 + flight/SimPosix/UAVObjects.inc | 2 + 8 files changed, 289 insertions(+), 168 deletions(-) rename flight/Modules/Airspeed/revolution/{baro_airspeed.c => baro_airspeed_analog.c} (53%) create mode 100644 flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c create mode 100644 flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h create mode 100644 flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c index 80ddaf281..dcd3db0f6 100644 --- a/flight/Modules/Airspeed/revolution/airspeed.c +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -44,8 +44,11 @@ #include "gps_airspeed.h" #include "baroaltitude.h" #include "baroairspeed.h" // object that will be updated by the module +#include "airspeedactual.h" // object that will be updated by the module #include "attitudeactual.h" #include "CoordinateConversions.h" +#include "baro_airspeed_etasv3.h" +#include "baro_airspeed_analog.h" // Private constants #if defined (PIOS_INCLUDE_GPS) @@ -164,6 +167,7 @@ int32_t AirspeedInitialize() #endif BaroAirspeedInitialize(); + AirspeedActualInitialize(); AirspeedSettingsInitialize(); AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); @@ -181,7 +185,9 @@ static void airspeedTask(void *parameters) AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); BaroAirspeedData airspeedData; + AirspeedActualData airspeedActualData; BaroAirspeedGet(&airspeedData); + AirspeedActualGet(&airspeedActualData); airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; @@ -205,6 +211,7 @@ static void airspeedTask(void *parameters) { // Update the airspeed object BaroAirspeedGet(&airspeedData); + AirspeedActualGet(&airspeedActualData); #ifdef BARO_AIRSPEED_PRESENT float airspeed_tas_baro=0; @@ -308,7 +315,10 @@ static void airspeedTask(void *parameters) #endif #endif //Set the UAVO + airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed; + airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed; BaroAirspeedSet(&airspeedData); + AirspeedActualSet(&airspeedActualData); } } @@ -321,6 +331,32 @@ static void GPSVelocityUpdatedCb(UAVObjEvent * ev) } #endif +void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){ + + //Find out which sensor we're using. + switch (airspeedSensorType) { +#if defined(PIOS_INCLUDE_MPXV7002) + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: +#endif +#if defined(PIOS_INCLUDE_MPXV5004) + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: +#endif +#if defined(PIOS_INCLUDE_MPXV7002) || defined(PIOS_INCLUDE_MPXV5004) + //MPXV5004 and MPXV7002 sensors + baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin); + break; +#endif +#if defined(PIOS_INCLUDE_ETASV3) + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: + //Eagletree Airspeed v3 + baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin); + break; +#endif + default: + baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; + } +} + static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev) { diff --git a/flight/Modules/Airspeed/revolution/baro_airspeed.c b/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c similarity index 53% rename from flight/Modules/Airspeed/revolution/baro_airspeed.c rename to flight/Modules/Airspeed/revolution/baro_airspeed_analog.c index 70b8654f8..c5bbac3d6 100644 --- a/flight/Modules/Airspeed/revolution/baro_airspeed.c +++ b/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c @@ -36,18 +36,15 @@ * */ -#define AIRSPEED_STORED_IN_A_PROPER_UAVO FALSE //AIRSPEED SHOULD NOT GO INTO BaroAirspeed. IT IS A STATE VARIABLE, AND NOT A MEASUREMENT - #include "openpilot.h" #include "hwsettings.h" #include "airspeed.h" #include "airspeedsettings.h" #include "baroairspeed.h" // object that will be updated by the module +#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004) -#define SAMPLING_DELAY_MS_ETASV3 50 //Update at 20Hz #define SAMPLING_DELAY_MS_MPXV 10 //Update at 100Hz -#define ETS_AIRSPEED_SCALE 1.0f //??? #define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms] #define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms] #define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100.0f //Needs to be settable in a UAVO @@ -58,29 +55,14 @@ // Private functions -#if defined(PIOS_INCLUDE_ETASV3) || defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004) static uint16_t calibrationCount=0; -#endif -void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){ - -#if defined(PIOS_INCLUDE_ETASV3) - static uint32_t calibrationSum = 0; -#endif - - //Find out which sensor we're using. - if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002 || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){ //MPXV5004 and MPXV7002 sensors - -#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004) + +void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){ + //Ensure that the ADC pin is properly configured if(airspeedADCPin <0){ //It's not, so revert to former sensor type baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; - AirspeedSettingsData airspeedSettingsData; - AirspeedSettingsGet(&airspeedSettingsData); - - airspeedSettingsData.AirspeedSensorType=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY; - - AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT? return; } @@ -93,20 +75,15 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT return; } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV) { //Then compute the average. calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */ - AirspeedSettingsData airspeedSettingsData; - AirspeedSettingsGet(&airspeedSettingsData); - - + uint16_t sensorCalibration; if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){ - uint16_t sensorCalibration=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV); - airspeedSettingsData.ZeroPoint=sensorCalibration; - 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. + sensorCalibration=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV); + PIOS_MPXV7002_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot. } else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){ - uint16_t sensorCalibration=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV); - airspeedSettingsData.ZeroPoint=sensorCalibration; - 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. + sensorCalibration=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV); + PIOS_MPXV5004_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot. } @@ -114,7 +91,7 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT //Set settings UAVO. The airspeed UAVO is set elsewhere in the function. if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV) - AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT? + AirspeedSettingsZeroPointSet(&sensorCalibration); return; } @@ -155,69 +132,11 @@ void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysT //Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one baroAirspeedData->CalibratedAirspeed = filteredAirspeed; -#else - //Whoops, no sensor defined in pios_config.h. Revert to GPS. - baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; - AirspeedSettingsData airspeedSettingsData; - AirspeedSettingsGet(&airspeedSettingsData); - airspeedSettingsData.AirspeedSensorType=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY; - - AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT? - - return; -#endif - } - else if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3){ //Eagletree Airspeed v3 -#if defined(PIOS_INCLUDE_ETASV3) - AirspeedSettingsData airspeedSettingsData; - AirspeedSettingsGet(&airspeedSettingsData); - //Wait until our turn. //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE - vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_ETASV3 / portTICK_RATE_MS); - - //Check to see if airspeed sensor is returning baroAirspeedData - baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed(); - if (baroAirspeedData->SensorValue==-1) { - baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; - baroAirspeedData->CalibratedAirspeed = 0; - BaroAirspeedSet(&baroAirspeedData); - return; - } - - //Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS? - if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) { - calibrationCount++; - return; - } else if (calibrationCount < (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { - calibrationCount++; - calibrationSum += baroAirspeedData->SensorValue; - if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { - - airspeedSettingsData.ZeroPoint=(uint16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f); - AirspeedSettingsSet(&airspeedSettingsData); - } else { - return; - } - } - - //Compute airspeed - float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed? - - baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE; - baroAirspeedData->CalibratedAirspeed = calibratedAirspeed; -#else - //Whoops, no EagleTree sensor defined in pios_config.h. Declare it unconnected... - baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; - - //...and revert to GPS. - if (airspeedSensorType!=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) { - uint8_t tmpVal=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY; - AirspeedSettingsAirspeedSensorTypeSet(&tmpVal); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT? - } -#endif - } } +#endif + /** * @} * @} diff --git a/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c b/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c new file mode 100644 index 000000000..07c58dc85 --- /dev/null +++ b/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c @@ -0,0 +1,109 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Communicate with airspeed sensors and return values + * @{ + * + * @file baro_airspeed.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Airspeed module, handles temperature and pressure readings from BMP085 + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/** + * Output object: BaroAirspeed + * + * This module will periodically update the value of the BaroAirspeed object. + * + */ + +#include "openpilot.h" +#include "hwsettings.h" +#include "airspeed.h" +#include "airspeedsettings.h" +#include "baroairspeed.h" // object that will be updated by the module + +#if defined(PIOS_INCLUDE_ETASV3) + +#define SAMPLING_DELAY_MS_ETASV3 50 //Update at 20Hz +#define ETS_AIRSPEED_SCALE 1.0f //??? +#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms] +#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms] +#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100.0f //Needs to be settable in a UAVO + +// Private types + +// Private variables + +// Private functions + +static uint16_t calibrationCount=0; + + +void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){ + + static uint32_t calibrationSum = 0; + AirspeedSettingsData airspeedSettingsData; + AirspeedSettingsGet(&airspeedSettingsData); + + //Wait until our turn. //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE + vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_ETASV3 / portTICK_RATE_MS); + + //Check to see if airspeed sensor is returning baroAirspeedData + baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed(); + if (baroAirspeedData->SensorValue==-1) { + baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; + baroAirspeedData->CalibratedAirspeed = 0; + BaroAirspeedSet(&baroAirspeedData); + return; + } + + //Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS? + if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) { + calibrationCount++; + return; + } else if (calibrationCount < (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { + calibrationCount++; + calibrationSum += baroAirspeedData->SensorValue; + if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { + + airspeedSettingsData.ZeroPoint = (int16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f); + AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint ); + } else { + return; + } + } + + //Compute airspeed + float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed? + + baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE; + baroAirspeedData->CalibratedAirspeed = calibratedAirspeed; +} + + +#endif + +/** + * @} + * @} + */ diff --git a/flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h b/flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h new file mode 100644 index 000000000..229196235 --- /dev/null +++ b/flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements + * @{ + * + * @file baro_airspeed_analog.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Airspeed module, reads temperature and pressure from BMP085 + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef ANALOG_AIRSPEED_H +#define ANALOG_AIRSPEED_H +#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004) + +void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin); + +#endif +#endif // ANALOG_AIRSPEED_H + +/** + * @} + * @} + */ diff --git a/flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h b/flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h new file mode 100644 index 000000000..2dd5e57b5 --- /dev/null +++ b/flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup AirspeedModule Airspeed Module + * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements + * @{ + * + * @file baro_airspeed_etasv3.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Airspeed module, reads temperature and pressure from BMP085 + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef ETASV3_AIRSPEED_H +#define ETASV3_AIRSPEED_H +#if defined(PIOS_INCLUDE_ETASV3) + +void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin); + +#endif +#endif // ETASV3_AIRSPEED_H + +/** + * @} + * @} + */ diff --git a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c index 64b6b832c..7439c57ec 100644 --- a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -54,7 +54,7 @@ #include "manualcontrol.h" #include "flightstatus.h" #include "pathstatus.h" -#include "baroairspeed.h" +#include "airspeedactual.h" #include "gpsvelocity.h" #include "gpsposition.h" #include "fixedwingpathfollowersettings.h" @@ -91,7 +91,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev); static void updatePathVelocity(); static uint8_t updateFixedDesiredAttitude(); static void updateFixedAttitude(); -static void baroAirspeedUpdatedCb(UAVObjEvent * ev); +static void airspeedActualUpdatedCb(UAVObjEvent * ev); static float bound(float val, float min, float max); /** @@ -125,7 +125,7 @@ int32_t FixedWingPathFollowerInitialize() PathDesiredInitialize(); PathStatusInitialize(); VelocityDesiredInitialize(); - BaroAirspeedInitialize(); + AirspeedActualInitialize(); } else { followerEnabled = false; } @@ -144,7 +144,7 @@ static float powerIntegral = 0; static float airspeedErrorInt=0; // correct speed by measured airspeed -static float baroAirspeedBias = 0; +static float indicatedAirspeedActualBias = 0; /** * Module thread, should not return. @@ -156,7 +156,7 @@ static void pathfollowerTask(void *parameters) portTickType lastUpdateTime; - BaroAirspeedConnectCallback(baroAirspeedUpdatedCb); + AirspeedActualConnectCallback(airspeedActualUpdatedCb); FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); @@ -359,12 +359,12 @@ static uint8_t updateFixedDesiredAttitude() FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; StabilizationSettingsData stabSettings; FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; - BaroAirspeedData baroAirspeed; + AirspeedActualData airspeedActual; -// float groundspeedActual; + float groundspeedActual; float groundspeedDesired; - float calibratedAirspeedActual; - float airspeedDesired; + float indicatedAirspeedActual; + float indicatedAirspeedDesired; float airspeedError; float accelActual; float accelDesired; @@ -392,7 +392,7 @@ static uint8_t updateFixedDesiredAttitude() AttitudeActualGet(&attitudeActual); AccelsGet(&accels); StabilizationSettingsGet(&stabSettings); - BaroAirspeedGet(&baroAirspeed); + AirspeedActualGet(&airspeedActual); /** @@ -400,17 +400,20 @@ static uint8_t updateFixedDesiredAttitude() */ // Current ground speed -// groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); - calibratedAirspeedActual = baroAirspeed.CalibratedAirspeed; + groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); + // note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement, + // but thanks to accelerometers, groundspeed reacts faster to changes in direction + // than airspeed and gps sensors alone + indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias; // Desired ground speed groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East); - airspeedDesired = bound( groundspeedDesired + baroAirspeedBias, + indicatedAirspeedDesired = bound( groundspeedDesired + indicatedAirspeedActualBias, fixedwingpathfollowerSettings.BestClimbRateSpeed, fixedwingpathfollowerSettings.CruiseSpeed); // Airspeed error - airspeedError = airspeedDesired - calibratedAirspeedActual; + airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual; // Vertical speed error descentspeedDesired = bound ( @@ -421,35 +424,35 @@ static uint8_t updateFixedDesiredAttitude() // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; - if (groundspeedDesired - baroAirspeedBias <= 0 ) { + if (groundspeedDesired - indicatedAirspeedActualBias <= 0 ) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; result = 0; } // Error condition: plane too slow or too fast fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; - if ( calibratedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) { + if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; result = 0; } - if ( calibratedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { + if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; result = 0; } - if (calibratedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; result = 0; } - if (calibratedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; result = 0; } - if (calibratedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { + if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; result = 0; } - if (calibratedAirspeedActual<1e-6) { + if (indicatedAirspeedActual<1e-6) { // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // also we cannot handle planes flying backwards, lets just wait until the nose drops fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; @@ -473,47 +476,14 @@ static uint8_t updateFixedDesiredAttitude() -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] )*(1.0f-1.0f/(1.0f+30.0f/dT)); - } + } else powerIntegral = 0; // Compute final throttle response - powerCommand = (powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + - powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL]; - - if (0) { - //Saturate command, and reduce integral as a way of further avoiding integral windup - if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) { - if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) { - powerIntegral = bound( - powerIntegral - - ( powerCommand - - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] - )/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], - -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT], - fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]); - } - - powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]; - } - if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) { - if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) { - powerIntegral = bound( - powerIntegral + - ( powerCommand - - fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] - )/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], - -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT], - fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]); - } - } - } - - //Saturate throttle command. - if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) { - powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]; - } - if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) { - powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]; - } + powerCommand = bound( + (powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + + powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL], + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN], + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]); //Output internal state to telemetry fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = powerError; @@ -554,7 +524,7 @@ static uint8_t updateFixedDesiredAttitude() */ // compute desired acceleration if(0){ - accelDesired = bound( (airspeedError/calibratedAirspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP], + accelDesired = bound( (airspeedError/indicatedAirspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP], -fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX], fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]); fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError; @@ -578,7 +548,7 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = accelIntegral; fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = accelCommand; - pitchCommand= -accelCommand + bound ( (-descentspeedError/calibratedAirspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], + pitchCommand= -accelCommand + bound ( (-descentspeedError/indicatedAirspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX], fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] ); @@ -605,7 +575,7 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError; fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt; - fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = airspeedDesired; + fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = indicatedAirspeedDesired; fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = -123; fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = -123; @@ -665,7 +635,7 @@ static uint8_t updateFixedDesiredAttitude() /** * Compute desired yaw command */ - // TODO implement raw control mode for yaw and base on Accels.X + // TODO implement raw control mode for yaw and base on Accels.Y stabDesired.Yaw = 0; @@ -700,21 +670,19 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) PathDesiredGet(&pathDesired); } -static void baroAirspeedUpdatedCb(UAVObjEvent * ev) +static void airspeedActualUpdatedCb(UAVObjEvent * ev) { - BaroAirspeedData baroAirspeed; + AirspeedActualData airspeedActual; VelocityActualData velocityActual; - BaroAirspeedGet(&baroAirspeed); - if (baroAirspeed.BaroConnected != BAROAIRSPEED_BAROCONNECTED_TRUE && BaroAirspeedReadOnly()) { - baroAirspeedBias = 0; - } else { - VelocityActualGet(&velocityActual); - float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); + AirspeedActualGet(&airspeedActual); + VelocityActualGet(&velocityActual); + float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); - - baroAirspeedBias = baroAirspeed.TrueAirspeed - groundspeed; - } + + indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed; + // note - we do fly by Indicated Airspeed (== calibrated airspeed) + // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement. } diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index 72b89d200..46f604ec2 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -38,6 +38,7 @@ UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroairspeed UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedactual UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate diff --git a/flight/SimPosix/UAVObjects.inc b/flight/SimPosix/UAVObjects.inc index baf683f76..de6f503a8 100644 --- a/flight/SimPosix/UAVObjects.inc +++ b/flight/SimPosix/UAVObjects.inc @@ -37,6 +37,8 @@ UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroairspeed +UAVOBJSRCFILENAMES += airspeedsettings +UAVOBJSRCFILENAMES += airspeedactual UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate