diff --git a/flight/Modules/Airspeed/revolution/airspeed.c b/flight/Modules/Airspeed/revolution/airspeed.c index dcd3db0f6..fd1479d0f 100644 --- a/flight/Modules/Airspeed/revolution/airspeed.c +++ b/flight/Modules/Airspeed/revolution/airspeed.c @@ -30,84 +30,40 @@ */ /** - * Output object: BaroAirspeed + * Output object: AirspeedSensor * - * This module will periodically update the value of the BaroAirspeed object. + * This module will periodically update the value of the AirspeedSensor object. * */ #include "openpilot.h" #include "hwsettings.h" -#include "airspeed.h" -#include "gpsvelocity.h" #include "airspeedsettings.h" -#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 "airspeedsensor.h" // object that will be updated by the module #include "baro_airspeed_etasv3.h" -#include "baro_airspeed_analog.h" +#include "baro_airspeed_mpxv.h" // Private constants -#if defined (PIOS_INCLUDE_GPS) - #define GPS_AIRSPEED_PRESENT -#endif -#if defined (PIOS_INCLUDE_MPXV5004) || defined (PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_ETASV3) - #define BARO_AIRSPEED_PRESENT -#endif - -#if defined (GPS_AIRSPEED_PRESENT) && defined (BARO_AIRSPEED_PRESENT) - #define STACK_SIZE_BYTES 700 -#elif defined (GPS_AIRSPEED_PRESENT) - #define STACK_SIZE_BYTES 600 -#elif defined (BARO_AIRSPEED_PRESENT) - #define STACK_SIZE_BYTES 550 -#else - #define STACK_SIZE_BYTES 0 - #define NO_AIRSPEED_SENSOR_PRESENT -#endif +#define STACK_SIZE_BYTES 500 #define TASK_PRIORITY (tskIDLE_PRIORITY+1) -#define SAMPLING_DELAY_MS_FALLTHROUGH 50 //Fallthrough update at 20Hz. The fallthrough runs faster than the GPS to ensure that we don't miss GPS updates because we're slightly out of sync - -#define GPS_AIRSPEED_BIAS_KP 0.01f //Needs to be settable in a UAVO -#define GPS_AIRSPEED_BIAS_KI 0.01f //Needs to be settable in a UAVO -#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO -#define BARO_TRUEAIRSPEED_TIME_CONSTANT_S 1.0f //Needs to be settable in a UAVO - -#define F_PI 3.141526535897932f -#define DEG2RAD (F_PI/180.0f) -#define T0 288.15f -#define BARO_TEMPERATURE_OFFSET 5 - // Private types // Private variables static xTaskHandle taskHandle; static bool airspeedEnabled = false; -volatile bool gpsNew = false; -static uint8_t airspeedSensorType; -static uint16_t gpsSamplePeriod_ms; +static AirspeedSettingsData airspeedSettings; -#ifdef BARO_AIRSPEED_PRESENT static int8_t airspeedADCPin=-1; -#endif // Private functions static void airspeedTask(void *parameters); -void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin); static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev); -#ifdef GPS_AIRSPEED_PRESENT -static void GPSVelocityUpdatedCb(UAVObjEvent * ev); -#endif - /** * Initialise the module, called on startup @@ -115,9 +71,6 @@ static void GPSVelocityUpdatedCb(UAVObjEvent * ev); */ int32_t AirspeedStart() { -#if defined (NO_AIRSPEED_SENSOR_PRESENT) - return -1; -#endif //Check if module is enabled or not if (airspeedEnabled == false) { @@ -153,7 +106,6 @@ int32_t AirspeedInitialize() } #endif -#ifdef BARO_AIRSPEED_PRESENT uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; HwSettingsADCRoutingGet(adcRouting); @@ -164,10 +116,7 @@ int32_t AirspeedInitialize() } } -#endif - - BaroAirspeedInitialize(); - AirspeedActualInitialize(); + AirspeedSensorInitialize(); AirspeedSettingsInitialize(); AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); @@ -184,198 +133,54 @@ static void airspeedTask(void *parameters) { AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); - BaroAirspeedData airspeedData; - AirspeedActualData airspeedActualData; - BaroAirspeedGet(&airspeedData); - AirspeedActualGet(&airspeedActualData); + AirspeedSensorData airspeedData; + AirspeedSensorGet(&airspeedData); + + AirspeedSettingsUpdatedCb(NULL); - 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 + airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; // Main task loop portTickType lastSysTime = xTaskGetTickCount(); while (1) { + vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS); + // Update the airspeed object - BaroAirspeedGet(&airspeedData); - AirspeedActualGet(&airspeedActualData); - -#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 - float baroTemperature; - - BaroAltitudeTemperatureGet(&baroTemperature); - baroTemperature-=BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm) - #ifdef GPS_AIRSPEED_PRESENT - //GPS present, so use baro sensor to filter TAS - airspeed_tas_baro=airspeedData.CalibratedAirspeed * sqrtf((baroTemperature+273.15)/T0) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI; - #else - //No GPS, so TAS comes only from baro sensor - airspeedData.TrueAirspeed=airspeedData.CalibratedAirspeed * sqrtf((baroTemperature+273.15)/T0) + 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, SAMPLING_DELAY_MS_FALLTHROUGH / portTICK_RATE_MS); - } - -#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 = (lastSysTime - lastLoopTime)/(portTICK_RATE_MS*1000.0f); - lastLoopTime=lastSysTime; - if ( ((lastSysTime - lastGPSTime) > 1000*portTICK_RATE_MS || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) - && gpsNew) { - lastGPSTime=lastSysTime; - #else - if (gpsNew) { - #endif - gpsNew=false; //Do this first + AirspeedSensorGet(&airspeedData); - //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 - float baroTemperature; - BaroAltitudeTemperatureGet(&baroTemperature); - baroTemperature-=BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm) - airspeedData.CalibratedAirspeed =airspeedData.TrueAirspeed / sqrtf((baroTemperature+273.15)/T0); - } - } - #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 - -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; + switch (airspeedSettings.AirspeedSensorType) { +#if defined(PIOS_INCLUDE_MPXV) + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: + //MPXV5004 and MPXV7002 sensors + baro_airspeedGetMPXV(&airspeedData, &airspeedSettings,airspeedADCPin); + break; #endif #if defined(PIOS_INCLUDE_ETASV3) - case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: - //Eagletree Airspeed v3 - baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin); - break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: + //Eagletree Airspeed v3 + baro_airspeedGetETASV3(&airspeedData, &airspeedSettings); + break; #endif - default: - baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; + default: + airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + } + + //Set the UAVO + AirspeedSensorSet(&airspeedData); + } } + + static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev) { - AirspeedSettingsData airspeedSettingsData; - AirspeedSettingsGet(&airspeedSettingsData); + AirspeedSettingsGet(&airspeedSettings); - 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 } diff --git a/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c b/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c deleted file mode 100644 index c5bbac3d6..000000000 --- a/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c +++ /dev/null @@ -1,143 +0,0 @@ -/** - ****************************************************************************** - * @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_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004) - -#define SAMPLING_DELAY_MS_MPXV 10 //Update at 100Hz -#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_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; - - return; - } - //Wait until our turn //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE - vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_MPXV / portTICK_RATE_MS); - - //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_MPXV) { //First let sensor warm up and stabilize. - calibrationCount++; - 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 */ - - uint16_t sensorCalibration; - if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){ - 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){ - 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. - } - - - baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE; - - //Set settings UAVO. The airspeed UAVO is set elsewhere in the function. - if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV) - AirspeedSettingsZeroPointSet(&sensorCalibration); - - return; - } - - //Get CAS - float calibratedAirspeed=0; - if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){ - calibratedAirspeed = PIOS_MPXV7002_ReadAirspeed(airspeedADCPin); - if (calibratedAirspeed < 0) //This only occurs when there's a bad ADC reading. - return; - - //Get sensor value, just for telemetry purposes. - //This is a silly waste of resources, and should probably be removed at some point in the future. - //At this time, PIOS_MPXV7002_Measure() should become a static function and be removed from the header file. - // - //Moreover, due to the way the ADC driver is currently written, this code will return 0 more often than - //not. This is something that will have to change on the ADC side of things. - baroAirspeedData->SensorValue=PIOS_MPXV7002_Measure(airspeedADCPin); - - } - else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){ - calibratedAirspeed = PIOS_MPXV5004_ReadAirspeed(airspeedADCPin); - if (calibratedAirspeed < 0) //This only occurs when there's a bad ADC reading. - return; - - //Get sensor value, just for telemetry purposes. - //This is a silly waste of resources, and should probably be removed at some point in the future. - //At this time, PIOS_MPXV7002_Measure() should become a static function and be removed from the header file. - // - //Moreover, due to the way the ADC driver is currently written, this code will return 0 more often than - //not. This is something that will have to change on the ADC side of things. - baroAirspeedData->SensorValue=PIOS_MPXV5004_Measure(airspeedADCPin); - - } - //Filter CAS - float alpha=SAMPLING_DELAY_MS_MPXV/(SAMPLING_DELAY_MS_MPXV + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter. - float filteredAirspeed = calibratedAirspeed*(alpha) + baroAirspeedData->CalibratedAirspeed*(1.0f-alpha); - - //Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one - baroAirspeedData->CalibratedAirspeed = filteredAirspeed; - -} - -#endif - -/** - * @} - * @} - */ diff --git a/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c b/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c index 07c58dc85..5da2e2810 100644 --- a/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c +++ b/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c @@ -38,17 +38,13 @@ #include "openpilot.h" #include "hwsettings.h" -#include "airspeed.h" #include "airspeedsettings.h" -#include "baroairspeed.h" // object that will be updated by the module +#include "airspeedsensor.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 @@ -57,47 +53,43 @@ // Private functions static uint16_t calibrationCount=0; +static uint16_t calibrationCount2=0; +static uint32_t calibrationSum = 0; -void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){ +void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings){ - 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); + //Check to see if airspeed sensor is returning airspeedSensor + airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed(); + if (airspeedSensor->SensorValue==-1) { + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + airspeedSensor->CalibratedAirspeed = 0; 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) { + // only calibrate if no stored calibration is available + if (!airspeedSettings->ZeroPoint) { + //Calibrate sensor by averaging zero point value + if (calibrationCount <= CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) { + calibrationCount++; + calibrationCount2++; + return; + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { + calibrationCount++; + calibrationCount2++; + calibrationSum += airspeedSensor->SensorValue; + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { - airspeedSettingsData.ZeroPoint = (int16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f); - AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint ); - } else { + airspeedSettings->ZeroPoint = (int16_t) (((float)calibrationSum) / calibrationCount2); + AirspeedSettingsZeroPointSet( &airspeedSettings->ZeroPoint ); + } 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; + airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint)); + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; } diff --git a/flight/Modules/Airspeed/revolution/baro_airspeed_mpxv.c b/flight/Modules/Airspeed/revolution/baro_airspeed_mpxv.c new file mode 100644 index 000000000..540a4a91d --- /dev/null +++ b/flight/Modules/Airspeed/revolution/baro_airspeed_mpxv.c @@ -0,0 +1,118 @@ +/** + ****************************************************************************** + * @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 "airspeedsettings.h" +#include "airspeedsensor.h" // object that will be updated by the module + +#if defined(PIOS_INCLUDE_MPXV) + +#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 //low pass filter + +// Private types + +// Private variables + +// Private functions + +static uint16_t calibrationCount=0; + +PIOS_MPXV_descriptor sensor = { .type=PIOS_MPXV_UNKNOWN }; + + +void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, int8_t airspeedADCPin){ + + //Ensure that the ADC pin is properly configured + if(airspeedADCPin <0){ + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + + return; + } + if (sensor.type==PIOS_MPXV_UNKNOWN) { + switch (airspeedSettings->AirspeedSensorType) { + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002: + sensor = PIOS_MPXV_7002_DESC(airspeedADCPin); + break; + case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004: + sensor = PIOS_MPXV_5004_DESC(airspeedADCPin); + break; + default: + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE; + return; + } + } + + airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor); + + if (!airspeedSettings->ZeroPoint) { + //Calibrate sensor by averaging zero point value + if (calibrationCount < CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) { //First let sensor warm up and stabilize. + calibrationCount++; + return; + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { //Then compute the average. + calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */ + + PIOS_MPXV_Calibrate(&sensor,airspeedSensor->SensorValue); + + //Set settings UAVO. The airspeed UAVO is set elsewhere in the function. + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) { + airspeedSettings->ZeroPoint = sensor.zeroPoint; + AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint); + } + return; + } + } + sensor.zeroPoint = airspeedSettings->ZeroPoint; + + //Filter CAS + float alpha=airspeedSettings->SamplePeriod/(airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter. + + airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor,airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed*(1.0f-alpha); + airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + +} + +#endif + +/** + * @} + * @} + */ diff --git a/flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h b/flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h index 2dd5e57b5..b19a2b631 100644 --- a/flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h +++ b/flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h @@ -28,14 +28,14 @@ * 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 +#ifndef BARO_AIRSPEED_ETASV3_H +#define BARO_AIRSPEED_ETASV3_H #if defined(PIOS_INCLUDE_ETASV3) -void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin); +void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings); #endif -#endif // ETASV3_AIRSPEED_H +#endif // BARO_AIRSPEED_ETASV3_H /** * @} diff --git a/flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h b/flight/Modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h similarity index 80% rename from flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h rename to flight/Modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h index 229196235..bb98da565 100644 --- a/flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h +++ b/flight/Modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h @@ -6,7 +6,7 @@ * @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements * @{ * - * @file baro_airspeed_analog.h + * @file baro_airspeed_mpxv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Airspeed module, reads temperature and pressure from BMP085 * @@ -28,14 +28,14 @@ * 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) +#ifndef BARO_AIRSPEED_MPXV_H +#define BARO_AIRSPEED_MPXV_H +#if defined(PIOS_INCLUDE_MPXV) -void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin); +void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, int8_t airspeedADCPin); #endif -#endif // ANALOG_AIRSPEED_H +#endif // BARO_AIRSPEED_MPXV_H /** * @} diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c index 7fc84f1c5..91ccc688c 100644 --- a/flight/Modules/Attitude/revolution/attitude.c +++ b/flight/Modules/Attitude/revolution/attitude.c @@ -53,6 +53,8 @@ #include "attitude.h" #include "accels.h" +#include "airspeedsensor.h" +#include "airspeedactual.h" #include "attitudeactual.h" #include "attitudesettings.h" #include "baroaltitude.h" @@ -82,6 +84,11 @@ // reasoning: updates at: 10 Hz, tau= 300 s settle time // exp(-(1/f) / tau ) ~=~ 0.9997 #define BARO_OFFSET_LOWPASS_ALPHA 0.9997f + +// simple IAS to TAS aproximation - 2% increase per 1000ft +// since we do not have flowing air temperature information +#define IAS2TAS(alt) (1.0f + (0.02f*(alt)/304.8f)) + // Private types // Private variables @@ -90,6 +97,7 @@ static xTaskHandle attitudeTaskHandle; static xQueueHandle gyroQueue; static xQueueHandle accelQueue; static xQueueHandle magQueue; +static xQueueHandle airspeedQueue; static xQueueHandle baroQueue; static xQueueHandle gpsQueue; static xQueueHandle gpsVelQueue; @@ -126,6 +134,8 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED); int32_t AttitudeInitialize(void) { AttitudeActualInitialize(); + AirspeedActualInitialize(); + AirspeedSensorInitialize(); AttitudeSettingsInitialize(); PositionActualInitialize(); VelocityActualInitialize(); @@ -170,6 +180,7 @@ int32_t AttitudeStart(void) gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); + airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); @@ -182,6 +193,7 @@ int32_t AttitudeStart(void) GyrosConnectQueue(gyroQueue); AccelsConnectQueue(accelQueue); MagnetometerConnectQueue(magQueue); + AirspeedSensorConnectQueue(airspeedQueue); BaroAltitudeConnectQueue(baroQueue); GPSPositionConnectQueue(gpsQueue); GPSVelocityConnectQueue(gpsVelQueue); @@ -473,6 +485,25 @@ static int32_t updateAttitudeComplementary(bool first_run) velocityActual.Down = gpsVelocity.Down; VelocityActualSet(&velocityActual); } + + if ( xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE ) { + // Calculate true airspeed from indicated airspeed + AirspeedSensorData airspeedSensor; + AirspeedSensorGet(&airspeedSensor); + + AirspeedActualData airspeed; + AirspeedActualGet(&airspeed); + + PositionActualData positionActual; + PositionActualGet(&positionActual); + + if (airspeedSensor.SensorConnected==AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { + // we have airspeed available + airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; + airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - positionActual.Down ); + AirspeedActualSet(&airspeed); + } + } AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); @@ -497,6 +528,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) GyrosData gyrosData; AccelsData accelsData; MagnetometerData magData; + AirspeedSensorData airspeedData; BaroAltitudeData baroData; GPSPositionData gpsData; GPSVelocityData gpsVelData; @@ -504,6 +536,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) static bool mag_updated = false; static bool baro_updated; + static bool airspeed_updated; static bool gps_updated; static bool gps_vel_updated; @@ -534,6 +567,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) if (inited) { mag_updated = 0; baro_updated = 0; + airspeed_updated = 0; gps_updated = 0; gps_vel_updated = 0; } @@ -544,6 +578,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) mag_updated = 0; baro_updated = 0; + airspeed_updated = 0; gps_updated = 0; gps_vel_updated = 0; @@ -554,6 +589,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; + airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; @@ -562,6 +598,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) AccelsGet(&accelsData); MagnetometerGet(&magData); BaroAltitudeGet(&baroData); + AirspeedSensorGet(&airspeedData); GPSPositionGet(&gpsData); GPSVelocityGet(&gpsVelData); GyrosBiasGet(&gyrosBias); @@ -572,6 +609,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) // switching between indoor and outdoor mode with Set = false) mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]); + // Discard airspeed if sensor not connected + airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE ); + // Have a minimum requirement for gps usage gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE); @@ -768,7 +808,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) * ( -NED[2] - baroData.Altitude ); } else if (!outdoor_mode) { - INSSetPosVelVar(1e2f, 1e2f); + INSSetPosVelVar(1e6f, 1e5f); vel[0] = vel[1] = vel[2] = 0; NED[0] = NED[1] = 0; NED[2] = -(baroData.Altitude + baroOffset); @@ -782,6 +822,27 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) vel[1] = gpsVelData.East; vel[2] = gpsVelData.Down; } + + if (airspeed_updated) { + // we have airspeed available + AirspeedActualData airspeed; + AirspeedActualGet(&airspeed); + + airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; + airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - Nav.Pos[2] ); + AirspeedActualSet(&airspeed); + + if ( !gps_vel_updated && !gps_updated) { + // feed airspeed into EKF, treat wind as 1e2 variance + sensors |= HORIZ_SENSORS | VERT_SENSORS; + INSSetPosVelVar(1e6f, 1e2f); + // rotate airspeed vector into NED frame - airspeed is measured in X axis only + float R[3][3]; + Quaternion2R(Nav.q,R); + float vtas[3] = {airspeed.TrueAirspeed,0,0}; + rot_mult(R,vtas,vel); + } + } /* * TODO: Need to add a general sanity check for all the inputs to make sure their kosher diff --git a/flight/Modules/PathPlanner/pathplanner.c b/flight/Modules/PathPlanner/pathplanner.c index aec41a2c1..de2fe9184 100644 --- a/flight/Modules/PathPlanner/pathplanner.c +++ b/flight/Modules/PathPlanner/pathplanner.c @@ -33,7 +33,7 @@ #include "paths.h" #include "flightstatus.h" -#include "baroairspeed.h" // TODO: make baroairspeed optional +#include "airspeedactual.h" #include "pathaction.h" #include "pathdesired.h" #include "pathstatus.h" @@ -103,7 +103,7 @@ int32_t PathPlannerInitialize() PathStatusInitialize(); PathDesiredInitialize(); PositionActualInitialize(); - BaroAirspeedInitialize(); + AirspeedActualInitialize(); VelocityActualInitialize(); WaypointInitialize(); WaypointActiveInitialize(); @@ -438,11 +438,9 @@ static uint8_t conditionAboveSpeed() { // use airspeed if requested and available if (pathAction.ConditionParameters[1]>0.5f) { - BaroAirspeedData baroAirspeed; - BaroAirspeedGet (&baroAirspeed); - if (baroAirspeed.BaroConnected == BAROAIRSPEED_BAROCONNECTED_TRUE) { - velocity = baroAirspeed.TrueAirspeed; - } + AirspeedActualData airspeed; + AirspeedActualGet (&airspeed); + velocity = airspeed.CalibratedAirspeed; } if ( velocity >= pathAction.ConditionParameters[0]) { diff --git a/flight/Modules/Sensors/simulated/sensors.c b/flight/Modules/Sensors/simulated/sensors.c index acd3a5d53..fef692598 100644 --- a/flight/Modules/Sensors/simulated/sensors.c +++ b/flight/Modules/Sensors/simulated/sensors.c @@ -54,7 +54,7 @@ #include "attitudeactual.h" #include "attitudesimulated.h" #include "attitudesettings.h" -#include "baroairspeed.h" +#include "rawairspeed.h" #include "baroaltitude.h" #include "gyros.h" #include "gyrosbias.h" @@ -112,7 +112,7 @@ int32_t SensorsInitialize(void) AccelsInitialize(); AttitudeSimulatedInitialize(); BaroAltitudeInitialize(); - BaroAirspeedInitialize(); + AirspeedSensorInitialize(); GyrosInitialize(); GyrosBiasInitialize(); GPSPositionInitialize(); @@ -748,11 +748,10 @@ static void simulateModelAirplane() // Update baro airpseed periodically static uint32_t last_airspeed_time = 0; if(PIOS_DELAY_DiffuS(last_airspeed_time) / 1.0e6 > BARO_PERIOD) { - BaroAirspeedData baroAirspeed; - baroAirspeed.Connected = BAROAIRSPEED_CONNECTED_TRUE; - baroAirspeed.ZeroPoint = 0; - baroAirspeed.Airspeed = forwardAirspeed; - BaroAirspeedSet(&baroAirspeed); + AirspeedSensorData airspeedSensor; + airspeedSensor.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE; + airspeedSensor.CalibratedAirspeed = forwardAirspeed; + AirspeedSensorSet(&airspeedSensor); last_airspeed_time = PIOS_DELAY_GetRaw(); } diff --git a/flight/PiOS/Common/pios_mpxv5004.c b/flight/PiOS/Common/pios_mpxv.c similarity index 60% rename from flight/PiOS/Common/pios_mpxv5004.c rename to flight/PiOS/Common/pios_mpxv.c index 5daf7991a..ff02a0266 100644 --- a/flight/PiOS/Common/pios_mpxv5004.c +++ b/flight/PiOS/Common/pios_mpxv.c @@ -2,13 +2,13 @@ ****************************************************************************** * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ - * @addtogroup PIOS_MPXV5004 MPXV5004 Functions - * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004. + * @addtogroup PIOS_MPXV Functions + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV*. * This is a differential sensor, so the value returned is first converted into * calibrated airspeed, using http://en.wikipedia.org/wiki/Calibrated_airspeed * @{ * - * @file pios_mpxv5004.h + * @file pios_mpxv.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief ETASV3 Airspeed Sensor Driver * @see The GNU Public License (GPL) Version 3 @@ -33,40 +33,32 @@ /* Project Includes */ #include "pios.h" -#if defined(PIOS_INCLUDE_MPXV5004) +#if defined(PIOS_INCLUDE_MPXV) -#include "pios_mpxv5004.h" - -static uint32_t calibrationSum = 0; //static? -static int16_t calibrationOffset; //static? +#define A0 340.27f //speed of sound at standard sea level in [m/s] +#define P0 101.325f //static air pressure at standard sea level in kPa +#define POWER (2.0f/7.0f) +#include "pios_mpxv.h" /* * Reads ADC. */ -uint16_t PIOS_MPXV5004_Measure(uint8_t airspeedADCPin) +uint16_t PIOS_MPXV_Measure(PIOS_MPXV_descriptor *desc) { - return PIOS_ADC_PinGet(airspeedADCPin); + if (desc) + return PIOS_ADC_PinGet(desc->airspeedADCPin); + return 0; } /* *Returns zeroPoint so that the user can inspect the calibration vs. the sensor value */ -uint16_t PIOS_MPXV5004_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount){ - calibrationSum += PIOS_MPXV5004_Measure(airspeedADCPin); - uint16_t zeroPoint = (uint16_t)(((float)calibrationSum) / calibrationCount + 0.5f); - - calibrationOffset = zeroPoint - (int16_t)(1.0f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 1.0V - - return zeroPoint; -} - - -/* - * Updates the calibration when zero point is manually set by user. - */ -void PIOS_MPXV5004_UpdateCalibration(uint16_t zeroPoint){ - calibrationOffset = zeroPoint - (int16_t)(1.0f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 1.0V +uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc,uint16_t measurement){ + desc->calibrationSum += measurement; + desc->calibrationCount++; + desc->zeroPoint = (uint16_t)(((float)desc->calibrationSum) / desc->calibrationCount); + return desc->zeroPoint; } @@ -74,12 +66,10 @@ void PIOS_MPXV5004_UpdateCalibration(uint16_t zeroPoint){ * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. * In the case of a failed read, returns -1. */ -float PIOS_MPXV5004_ReadAirspeed(uint8_t airspeedADCPin) +float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc,uint16_t measurement) { - float sensorVal = PIOS_MPXV5004_Measure(airspeedADCPin); - //Calculate dynamic pressure, as per docs - float Qc = 5.0f*(((sensorVal - calibrationOffset)/4096.0f*3.3f)/VCC - 0.2f); + float Qc = 3.3f/4096.0f * (float)(measurement - desc->zeroPoint); //Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need // to saturate on the upper bound, we'll handle that later with calibratedAirspeed. @@ -87,16 +77,15 @@ float PIOS_MPXV5004_ReadAirspeed(uint8_t airspeedADCPin) Qc=0; } - //Compute calibraterd airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed + //Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed float calibratedAirspeed = A0*sqrt(5.0f*(pow(Qc/P0+1.0f,POWER)-1.0f)); //Upper bound airspeed. No need to lower bound it, that comes from Qc - if (calibratedAirspeed > 80) { //in [m/s] - calibratedAirspeed=80; + if (calibratedAirspeed > desc->maxSpeed) { //in [m/s] + calibratedAirspeed=desc->maxSpeed; } - return calibratedAirspeed; } -#endif /* PIOS_INCLUDE_MPXV5004 */ +#endif /* PIOS_INCLUDE_MPXV */ diff --git a/flight/PiOS/Common/pios_mpxv7002.c b/flight/PiOS/Common/pios_mpxv7002.c deleted file mode 100644 index daf9a9168..000000000 --- a/flight/PiOS/Common/pios_mpxv7002.c +++ /dev/null @@ -1,102 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_MPXV7002 MPXV7002 Functions - * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV7002. - * This is a differential sensor, so the value returned is first converted into - * calibrated airspeed, using http://en.wikipedia.org/wiki/Calibrated_airspeed - * @{ - * - * @file pios_mpxv7002.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief ETASV3 Airspeed Sensor Driver - * @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 - */ - -/* Project Includes */ -#include "pios.h" - -#if defined(PIOS_INCLUDE_MPXV7002) - -#include "pios_mpxv7002.h" - -static uint32_t calibrationSum = 0; //static? -static int16_t calibrationOffset; //static? - - -/* - * Reads ADC. - */ -uint16_t PIOS_MPXV7002_Measure(uint8_t airspeedADCPin) -{ - return PIOS_ADC_PinGet(airspeedADCPin); -} - -/* - *Returns zeroPoint so that the user can inspect the calibration vs. the sensor value - */ -uint16_t PIOS_MPXV7002_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount){ - calibrationSum += PIOS_MPXV7002_Measure(airspeedADCPin); - uint16_t zeroPoint = (uint16_t)(((float)calibrationSum) / calibrationCount + 0.5f); - - calibrationOffset = zeroPoint - (int16_t)(2.5f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 2.5V - - return zeroPoint; -} - - -/* - * Updates the calibration when zero point is manually set by user. - */ -void PIOS_MPXV7002_UpdateCalibration(uint16_t zeroPoint){ - calibrationOffset = zeroPoint - (int16_t)(2.5f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 2.5V -} - - -/* - * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. - * In the case of a failed read, returns -1. - */ -float PIOS_MPXV7002_ReadAirspeed(uint8_t airspeedADCPin) -{ - float sensorVal = PIOS_MPXV7002_Measure(airspeedADCPin); - - //Calculate dynamic pressure, as per docs - float Qc = 5.0f*(((sensorVal - calibrationOffset)/4096.0f*3.3f)/VCC - 0.5f); - - //Saturate Qc on the lower bound, in order to make sure we don't have negative airspeeds. No need - // to saturate on the upper bound, we'll handle that later with calibratedAirspeed. - if (Qc < 0) { - Qc=0; - } - - //Compute calibraterd airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed - float calibratedAirspeed = A0*sqrt(5.0f*(pow(Qc/P0+1.0f,POWER)-1.0f)); - - //Upper bound airspeed. No need to lower bound it, that comes from Qc - if (calibratedAirspeed > 59) { //in [m/s] - calibratedAirspeed=59; - } - - - return calibratedAirspeed; -} - -#endif /* PIOS_INCLUDE_MPXV7002 */ diff --git a/flight/PiOS/inc/pios_mpxv5004.h b/flight/PiOS/inc/pios_mpxv.h similarity index 52% rename from flight/PiOS/inc/pios_mpxv5004.h rename to flight/PiOS/inc/pios_mpxv.h index 2ebf1dee9..824f0aa7b 100644 --- a/flight/PiOS/inc/pios_mpxv5004.h +++ b/flight/PiOS/inc/pios_mpxv.h @@ -2,11 +2,11 @@ ****************************************************************************** * @addtogroup PIOS PIOS Core hardware abstraction layer * @{ - * @addtogroup PIOS_MPXV5004 MPXV5004 Functions - * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004 + * @addtogroup PIOS_MPXV MPXV* Functions + * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004, 7002 or similar * @{ * - * @file pios_mpxv5004.h + * @file pios_mpxv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief ETASV3 Airspeed Sensor Driver * @see The GNU Public License (GPL) Version 3 @@ -28,16 +28,39 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef PIOS_MPXV5004_H__ +#ifndef PIOS_MPXV_H__ +#define PIOS_MPXV_H__ -#define A0 340.27f //speed of sound at standard sea level in [m/s] -#define P0 101.325f //static air pressure at standard sea level in kPa -#define VCC 5.0f //Supply voltage in V -#define POWER (2.0f/7.0f) +typedef enum{ PIOS_MPXV_UNKNOWN,PIOS_MPXV_5004,PIOS_MPXV_7002 } PIOS_MPXV_sensortype; +typedef struct{ + PIOS_MPXV_sensortype type; + uint8_t airspeedADCPin; + uint16_t calibrationCount; + uint32_t calibrationSum; + uint16_t zeroPoint; + float maxSpeed; +} PIOS_MPXV_descriptor; -uint16_t PIOS_MPXV5004_Measure(uint8_t airspeedADCPin); -uint16_t PIOS_MPXV5004_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount); -void PIOS_MPXV5004_UpdateCalibration(uint16_t zeroPoint); -float PIOS_MPXV5004_ReadAirspeed (uint8_t airspeedADCPin); +#define PIOS_MPXV_5004_DESC(pin) \ + (PIOS_MPXV_descriptor){ \ + .type = PIOS_MPXV_5004, \ + .airspeedADCPin = pin, \ + .maxSpeed = 80.0f, \ + .calibrationCount = 0, \ + .calibrationSum = 0, \ + } +#define PIOS_MPXV_7002_DESC(pin) \ + (PIOS_MPXV_descriptor){ \ + .type = PIOS_MPXV_7002, \ + .airspeedADCPin = pin, \ + .maxSpeed = 56.0f, \ + .calibrationCount = 0, \ + .calibrationSum = 0, \ + } -#endif // PIOS_MPXV5004_H__ \ No newline at end of file + +uint16_t PIOS_MPXV_Measure(PIOS_MPXV_descriptor *desc); +uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement); +float PIOS_MPXV_CalcAirspeed (PIOS_MPXV_descriptor *desc,uint16_t measurement); + +#endif // PIOS_MPXV_H__ diff --git a/flight/PiOS/inc/pios_mpxv7002.h b/flight/PiOS/inc/pios_mpxv7002.h deleted file mode 100644 index 5f9a8f3a7..000000000 --- a/flight/PiOS/inc/pios_mpxv7002.h +++ /dev/null @@ -1,43 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_MPXV7002 MPXV7002 Functions - * @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV7002 - * @{ - * - * @file pios_mpxv7002.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief ETASV3 Airspeed Sensor Driver - * @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 PIOS_MPXV7002_H__ - -#define A0 340.27f //speed of sound at standard sea level in [m/s] -#define P0 101.325f //static air pressure at standard sea level in kPa -#define VCC 5.0f //Supply voltage in V -#define POWER (2.0f/7.0f) - -uint16_t PIOS_MPXV7002_Measure(uint8_t airspeedADCPin); -uint16_t PIOS_MPXV7002_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount); -void PIOS_MPXV7002_UpdateCalibration(uint16_t zeroPoint); -float PIOS_MPXV7002_ReadAirspeed (uint8_t airspeedADCPin); - -#endif // PIOS_MPXV7002_H__ \ No newline at end of file diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index 23c2b598a..66de54a3e 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -101,11 +101,8 @@ /* PIOS Hardware Includes (Common) */ #include #include -#if defined(PIOS_INCLUDE_MPXV7002) -#include -#endif -#if defined(PIOS_INCLUDE_MPXV5004) -#include +#if defined(PIOS_INCLUDE_MPXV) +#include #endif #if defined(PIOS_INCLUDE_ETASV3) #include diff --git a/flight/RevoMini/Makefile b/flight/RevoMini/Makefile index 47cdb82bd..56a74198e 100644 --- a/flight/RevoMini/Makefile +++ b/flight/RevoMini/Makefile @@ -55,7 +55,7 @@ MODULES += ManualControl Stabilization Actuator MODULES += Battery MODULES += Altitude/revolution MODULES += GPS FirmwareIAP -#MODULES += Airspeed/revolution +MODULES += Airspeed/revolution MODULES += AltitudeHold FixedWingPathFollower PathPlanner TxPID #MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged MODULES += CameraStab @@ -164,6 +164,7 @@ include $(PIOS)/STM32F4xx/library.mk SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_bma180.c SRC += $(PIOSCOMMON)/pios_etasv3.c +SRC += $(PIOSCOMMON)/pios_mpxv.c SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_hmc5883.c diff --git a/flight/RevoMini/System/inc/pios_config.h b/flight/RevoMini/System/inc/pios_config.h index a4e18652c..5c166946b 100644 --- a/flight/RevoMini/System/inc/pios_config.h +++ b/flight/RevoMini/System/inc/pios_config.h @@ -66,7 +66,8 @@ #define PIOS_INCLUDE_MPU6000 #define PIOS_MPU6000_ACCEL #define PIOS_INCLUDE_MS5611 -//#define PIOS_INCLUDE_ETASV3 +#define PIOS_INCLUDE_ETASV3 +#define PIOS_INCLUDE_MPXV //#define PIOS_INCLUDE_HCSR04 #define FLASH_FREERTOS /* Com systems to include */ diff --git a/flight/RevoMini/UAVObjects.inc b/flight/RevoMini/UAVObjects.inc index 7b59a2585..468d40d75 100644 --- a/flight/RevoMini/UAVObjects.inc +++ b/flight/RevoMini/UAVObjects.inc @@ -37,7 +37,7 @@ UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += magbias UAVOBJSRCFILENAMES += baroaltitude -UAVOBJSRCFILENAMES += baroairspeed +UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedactual UAVOBJSRCFILENAMES += flightbatterysettings diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index 845b1456b..452de3053 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -182,8 +182,7 @@ include $(PIOS)/STM32F4xx/library.mk SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_bma180.c SRC += $(PIOSCOMMON)/pios_etasv3.c -SRC += $(PIOSCOMMON)/pios_mpxv5004.c -SRC += $(PIOSCOMMON)/pios_mpxv7002.c +SRC += $(PIOSCOMMON)/pios_mpxv.c SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_hmc5883.c diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index 522e15a80..8ac4b2109 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -65,8 +65,7 @@ #define PIOS_INCLUDE_L3GD20 #define PIOS_INCLUDE_MS5611 #define PIOS_INCLUDE_ETASV3 -#define PIOS_INCLUDE_MPXV5004 -#define PIOS_INCLUDE_MPXV7002 +#define PIOS_INCLUDE_MPXV //#define PIOS_INCLUDE_HCSR04 #define PIOS_FLASH_ON_ACCEL /* true for second revo */ #define FLASH_FREERTOS diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index 366e40820..545f6ecc7 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -37,7 +37,7 @@ UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += magbias UAVOBJSRCFILENAMES += baroaltitude -UAVOBJSRCFILENAMES += baroairspeed +UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedactual UAVOBJSRCFILENAMES += flightbatterysettings diff --git a/flight/SimPosix/UAVObjects.inc b/flight/SimPosix/UAVObjects.inc index f2fca086f..4a6c453ac 100644 --- a/flight/SimPosix/UAVObjects.inc +++ b/flight/SimPosix/UAVObjects.inc @@ -36,7 +36,7 @@ UAVOBJSRCFILENAMES += gyrosbias UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += baroaltitude -UAVOBJSRCFILENAMES += baroairspeed +UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedactual UAVOBJSRCFILENAMES += flightbatterysettings diff --git a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp index 88922f0f8..1a0967ca8 100644 --- a/ground/openpilotgcs/src/plugins/hitl/simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/simulator.cpp @@ -734,8 +734,8 @@ void Simulator::updateUAVOs(Output2Hardware out){ memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed; - airspeedActualData.alpha=out.angleOfAttack; - airspeedActualData.beta=out.angleOfSlip; + //airspeedActualData.alpha=out.angleOfAttack; // to be implemented + //airspeedActualData.beta=out.angleOfSlip; airspeedActual->setData(airspeedActualData); airspeedActualTime=airspeedActualTime.addMSecs(settings.airspeedActualRate); diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d28dd07be..c757e62db 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -25,7 +25,7 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/baroaltitude.h \ - $$UAVOBJECT_SYNTHETICS/baroairspeed.h \ + $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ $$UAVOBJECT_SYNTHETICS/airspeedactual.h \ $$UAVOBJECT_SYNTHETICS/attitudeactual.h \ @@ -103,7 +103,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \ - $$UAVOBJECT_SYNTHETICS/baroairspeed.cpp \ + $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedactual.cpp \ $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \ diff --git a/shared/uavobjectdefinition/airspeedactual.xml b/shared/uavobjectdefinition/airspeedactual.xml index e612b3e31..d5b0f90ae 100644 --- a/shared/uavobjectdefinition/airspeedactual.xml +++ b/shared/uavobjectdefinition/airspeedactual.xml @@ -1,13 +1,11 @@ UAVO for true airspeed, calibrated airspeed, angle of attack, and angle of slip - - - + - + diff --git a/shared/uavobjectdefinition/baroairspeed.xml b/shared/uavobjectdefinition/airspeedsensor.xml similarity index 63% rename from shared/uavobjectdefinition/baroairspeed.xml rename to shared/uavobjectdefinition/airspeedsensor.xml index 6fd9d1ac6..0ac1f3ecd 100644 --- a/shared/uavobjectdefinition/baroairspeed.xml +++ b/shared/uavobjectdefinition/airspeedsensor.xml @@ -1,11 +1,9 @@ - + The raw data from the dynamic pressure sensor with pressure, temperature and airspeed. - + - - diff --git a/shared/uavobjectdefinition/airspeedsettings.xml b/shared/uavobjectdefinition/airspeedsettings.xml index e24697f16..697c9a3ec 100644 --- a/shared/uavobjectdefinition/airspeedsettings.xml +++ b/shared/uavobjectdefinition/airspeedsettings.xml @@ -1,10 +1,10 @@ Settings for the @ref BaroAirspeed module used on CopterControl or Revolution - + - +