rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-26 15:54:15 +01:00
Code Issues Releases Activity

Completely refactored airspeed module, now a pure sensor input module without any complex sensor fusion features -- changed driver for mpxv analog airspeed to be more versatile

Browse Source
This commit is contained in:
Corvus Corax 2012-11-08 01:31:21 +01:00
parent ce595a06d9
commit b09027afe5
22 changed files with 267 additions and 656 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

267
flight/Modules/Airspeed/revolution/airspeed.c
View File

@ -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 "openpilot.h"
#include "hwsettings.h" #include "hwsettings.h"
#include "airspeed.h"
#include "gpsvelocity.h"
#include "airspeedsettings.h" #include "airspeedsettings.h"
#include "gps_airspeed.h" #include "airspeedsensor.h" // object that will be updated by the module
#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_etasv3.h"
#include "baro_airspeed_analog.h" #include "baro_airspeed_mpxv.h"
// Private constants // 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 STACK_SIZE_BYTES 500
#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 TASK_PRIORITY (tskIDLE_PRIORITY+1) #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 types
// Private variables // Private variables
static xTaskHandle taskHandle; static xTaskHandle taskHandle;
static bool airspeedEnabled = false; static bool airspeedEnabled = false;
volatile bool gpsNew = false; static AirspeedSettingsData airspeedSettings;
static uint8_t airspeedSensorType;
static uint16_t gpsSamplePeriod_ms;
#ifdef BARO_AIRSPEED_PRESENT
static int8_t airspeedADCPin=-1; static int8_t airspeedADCPin=-1;
#endif
// Private functions // Private functions
static void airspeedTask(void *parameters); static void airspeedTask(void *parameters);
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin);
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev); static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev);
#ifdef GPS_AIRSPEED_PRESENT
static void GPSVelocityUpdatedCb(UAVObjEvent * ev);
#endif
/** /**
* Initialise the module, called on startup * Initialise the module, called on startup
@ -115,9 +71,6 @@ static void GPSVelocityUpdatedCb(UAVObjEvent * ev);
*/ */
int32_t AirspeedStart() int32_t AirspeedStart()
{ {
#if defined (NO_AIRSPEED_SENSOR_PRESENT)
return -1;
#endif
//Check if module is enabled or not //Check if module is enabled or not
if (airspeedEnabled == false) { if (airspeedEnabled == false) {
@ -153,7 +106,6 @@ int32_t AirspeedInitialize()
} }
#endif #endif
#ifdef BARO_AIRSPEED_PRESENT
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingGet(adcRouting); HwSettingsADCRoutingGet(adcRouting);
@ -164,10 +116,7 @@ int32_t AirspeedInitialize()
} }
} }
#endif AirspeedSensorInitialize();
BaroAirspeedInitialize();
AirspeedActualInitialize();
AirspeedSettingsInitialize(); AirspeedSettingsInitialize();
AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);
@ -184,198 +133,54 @@ static void airspeedTask(void *parameters)
{ {
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
BaroAirspeedData airspeedData; AirspeedSensorData airspeedData;
AirspeedActualData airspeedActualData; AirspeedSensorGet(&airspeedData);
BaroAirspeedGet(&airspeedData);
AirspeedActualGet(&airspeedActualData); AirspeedSettingsUpdatedCb(NULL);
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
#ifdef BARO_AIRSPEED_PRESENT
portTickType lastGPSTime = xTaskGetTickCount(); //Time since last GPS-derived airspeed calculation
portTickType lastLoopTime= xTaskGetTickCount(); //Time since last loop
float airspeedErrInt=0;
#endif
//GPS airspeed calculation variables
#ifdef GPS_AIRSPEED_PRESENT
GPSVelocityConnectCallback(GPSVelocityUpdatedCb);
gps_airspeedInitialize();
#endif
// Main task loop // Main task loop
portTickType lastSysTime = xTaskGetTickCount(); portTickType lastSysTime = xTaskGetTickCount();
while (1) while (1)
{ {
vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS);
// Update the airspeed object // Update the airspeed object
BaroAirspeedGet(&airspeedData); AirspeedSensorGet(&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
//Calculate airspeed as a function of GPS groundspeed and vehicle attitude. From "IMU Wind Estimation (Theory)", by William Premerlani switch (airspeedSettings.AirspeedSensorType) {
gps_airspeedGet(&v_air_GPS); #if defined(PIOS_INCLUDE_MPXV)
} case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
//MPXV5004 and MPXV7002 sensors
//Use the GPS error to correct the airspeed estimate. baro_airspeedGetMPXV(&airspeedData, &airspeedSettings,airspeedADCPin);
if (v_air_GPS > 0) //We have valid GPS estimate... break;
{
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;
#endif #endif
#if defined(PIOS_INCLUDE_ETASV3) #if defined(PIOS_INCLUDE_ETASV3)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3: case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
//Eagletree Airspeed v3 //Eagletree Airspeed v3
baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin); baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
break; break;
#endif #endif
default: default:
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
}
//Set the UAVO
AirspeedSensorSet(&airspeedData);
} }
} }
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev) static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
{ {
AirspeedSettingsData airspeedSettingsData; AirspeedSettingsGet(&airspeedSettings);
AirspeedSettingsGet(&airspeedSettingsData);
airspeedSensorType=airspeedSettingsData.AirspeedSensorType;
gpsSamplePeriod_ms=airspeedSettingsData.GPSSamplePeriod_ms;
#if defined(PIOS_INCLUDE_MPXV7002)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
} }

156
flight/Modules/Airspeed/revolution/baro_airspeed_analog.c
View File

@ -1,156 +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;
}
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
//Wait until our turn //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE
vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_MPXV / portTICK_RATE_MS);
if (!airspeedSettingsData.ZeroPoint) {
//Calibrate sensor by averaging zero point value
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 */
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002) {
airspeedSettingsData.ZeroPoint=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
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.
} else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004) {
airspeedSettingsData.ZeroPoint=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
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.
}
//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(&airspeedSettingsData.ZeroPoint);
return;
}
} else if (!calibrationCount) {
// do this only once
calibrationCount++;
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.
} else 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.
}
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->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
baroAirspeedData->CalibratedAirspeed = filteredAirspeed;
}
#endif
/**
* @}
* @}
*/

53
flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c
View File

@ -38,17 +38,13 @@
#include "openpilot.h" #include "openpilot.h"
#include "hwsettings.h" #include "hwsettings.h"
#include "airspeed.h"
#include "airspeedsettings.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) #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_IDLE_MS 2000 //Time to wait before calibrating, in [ms]
#define CALIBRATION_COUNT_MS 2000 //Time to spend 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 types
@ -57,50 +53,43 @@
// Private functions // Private functions
static uint16_t calibrationCount=0; 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; //Check to see if airspeed sensor is returning airspeedSensor
AirspeedSettingsData airspeedSettingsData; airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed();
AirspeedSettingsGet(&airspeedSettingsData); if (airspeedSensor->SensorValue==-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
//Wait until our turn. //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE airspeedSensor->CalibratedAirspeed = 0;
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; return;
} }
// only calibrate if no stored calibration is available // only calibrate if no stored calibration is available
if (!airspeedSettingsData.ZeroPoint) { if (!airspeedSettings->ZeroPoint) {
//Calibrate sensor by averaging zero point value //Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) { if (calibrationCount <= CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) {
calibrationCount++; calibrationCount++;
calibrationCount2++;
return; return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) {
calibrationCount++; calibrationCount++;
calibrationSum += baroAirspeedData->SensorValue; calibrationCount2++;
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { calibrationSum += airspeedSensor->SensorValue;
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) {
airspeedSettingsData.ZeroPoint = (int16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f); airspeedSettings->ZeroPoint = (int16_t) (((float)calibrationSum) / calibrationCount2);
AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint ); AirspeedSettingsZeroPointSet( &airspeedSettings->ZeroPoint );
} else {
return;
} }
return;
} }
} }
//Compute airspeed //Compute airspeed
float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed? airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
baroAirspeedData->CalibratedAirspeed = calibratedAirspeed;
} }

118
flight/Modules/Airspeed/revolution/baro_airspeed_mpxv.c Normal file
View File

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

8
flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h
View File

@ -28,14 +28,14 @@
* with this program; if not, write to the Free Software Foundation, Inc., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef ETASV3_AIRSPEED_H #ifndef BARO_AIRSPEED_ETASV3_H
#define ETASV3_AIRSPEED_H #define BARO_AIRSPEED_ETASV3_H
#if defined(PIOS_INCLUDE_ETASV3) #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
#endif // ETASV3_AIRSPEED_H #endif // BARO_AIRSPEED_ETASV3_H
/** /**
* @} * @}

12
flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h → flight/Modules/Airspeed/revolution/inc/baro_airspeed_mpxv.h
View File

@ -6,7 +6,7 @@
* @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements * @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. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Airspeed module, reads temperature and pressure from BMP085 * @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., * with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/ */
#ifndef ANALOG_AIRSPEED_H #ifndef BARO_AIRSPEED_MPXV_H
#define ANALOG_AIRSPEED_H #define BARO_AIRSPEED_MPXV_H
#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004) #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
#endif // ANALOG_AIRSPEED_H #endif // BARO_AIRSPEED_MPXV_H
/** /**
* @} * @}

13
flight/Modules/Sensors/simulated/sensors.c
View File

@ -54,7 +54,7 @@
#include "attitudeactual.h" #include "attitudeactual.h"
#include "attitudesimulated.h" #include "attitudesimulated.h"
#include "attitudesettings.h" #include "attitudesettings.h"
#include "baroairspeed.h" #include "rawairspeed.h"
#include "baroaltitude.h" #include "baroaltitude.h"
#include "gyros.h" #include "gyros.h"
#include "gyrosbias.h" #include "gyrosbias.h"
@ -112,7 +112,7 @@ int32_t SensorsInitialize(void)
AccelsInitialize(); AccelsInitialize();
AttitudeSimulatedInitialize(); AttitudeSimulatedInitialize();
BaroAltitudeInitialize(); BaroAltitudeInitialize();
BaroAirspeedInitialize(); AirspeedSensorInitialize();
GyrosInitialize(); GyrosInitialize();
GyrosBiasInitialize(); GyrosBiasInitialize();
GPSPositionInitialize(); GPSPositionInitialize();
@ -748,11 +748,10 @@ static void simulateModelAirplane()
// Update baro airpseed periodically // Update baro airpseed periodically
static uint32_t last_airspeed_time = 0; static uint32_t last_airspeed_time = 0;
if(PIOS_DELAY_DiffuS(last_airspeed_time) / 1.0e6 > BARO_PERIOD) { if(PIOS_DELAY_DiffuS(last_airspeed_time) / 1.0e6 > BARO_PERIOD) {
BaroAirspeedData baroAirspeed; AirspeedSensorData airspeedSensor;
baroAirspeed.Connected = BAROAIRSPEED_CONNECTED_TRUE; airspeedSensor.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
baroAirspeed.ZeroPoint = 0; airspeedSensor.CalibratedAirspeed = forwardAirspeed;
baroAirspeed.Airspeed = forwardAirspeed; AirspeedSensorSet(&airspeedSensor);
BaroAirspeedSet(&baroAirspeed);
last_airspeed_time = PIOS_DELAY_GetRaw(); last_airspeed_time = PIOS_DELAY_GetRaw();
} }

57
flight/PiOS/Common/pios_mpxv5004.c → flight/PiOS/Common/pios_mpxv.c
View File

@ -2,13 +2,13 @@
****************************************************************************** ******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer * @addtogroup PIOS PIOS Core hardware abstraction layer
* @{ * @{
* @addtogroup PIOS_MPXV5004 MPXV5004 Functions * @addtogroup PIOS_MPXV Functions
* @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004. * @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 * This is a differential sensor, so the value returned is first converted into
* calibrated airspeed, using http://en.wikipedia.org/wiki/Calibrated_airspeed * 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. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief ETASV3 Airspeed Sensor Driver * @brief ETASV3 Airspeed Sensor Driver
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
@ -33,40 +33,32 @@
/* Project Includes */ /* Project Includes */
#include "pios.h" #include "pios.h"
#if defined(PIOS_INCLUDE_MPXV5004) #if defined(PIOS_INCLUDE_MPXV)
#include "pios_mpxv5004.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
static uint32_t calibrationSum = 0; //static? #define POWER (2.0f/7.0f)
static int16_t calibrationOffset; //static?
#include "pios_mpxv.h"
/* /*
* Reads ADC. * 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 *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){ uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc,uint16_t measurement){
calibrationSum += PIOS_MPXV5004_Measure(airspeedADCPin); desc->calibrationSum += measurement;
uint16_t zeroPoint = (uint16_t)(((float)calibrationSum) / calibrationCount + 0.5f); desc->calibrationCount++;
desc->zeroPoint = (uint16_t)(((float)desc->calibrationSum) / desc->calibrationCount);
calibrationOffset = zeroPoint - (int16_t)(1.0f/3.3f*4096.0f+0.5f); //The offset should set the zero point to 1.0V return desc->zeroPoint;
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
} }
@ -74,12 +66,10 @@ void PIOS_MPXV5004_UpdateCalibration(uint16_t zeroPoint){
* Reads the airspeed and returns CAS (calibrated airspeed) in the case of success. * Reads the airspeed and returns CAS (calibrated airspeed) in the case of success.
* In the case of a failed read, returns -1. * 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 //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 //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. // 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; 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)); 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 //Upper bound airspeed. No need to lower bound it, that comes from Qc
if (calibratedAirspeed > 80) { //in [m/s] if (calibratedAirspeed > desc->maxSpeed) { //in [m/s]
calibratedAirspeed=80; calibratedAirspeed=desc->maxSpeed;
} }
return calibratedAirspeed; return calibratedAirspeed;
} }
#endif /* PIOS_INCLUDE_MPXV5004 */ #endif /* PIOS_INCLUDE_MPXV */

102
flight/PiOS/Common/pios_mpxv7002.c
View File

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

49
flight/PiOS/inc/pios_mpxv5004.h → flight/PiOS/inc/pios_mpxv.h
View File

@ -2,11 +2,11 @@
****************************************************************************** ******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer * @addtogroup PIOS PIOS Core hardware abstraction layer
* @{ * @{
* @addtogroup PIOS_MPXV5004 MPXV5004 Functions * @addtogroup PIOS_MPXV MPXV* Functions
* @brief Hardware functions to deal with the DIYDrones airspeed kit, using MPXV5004 * @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. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief ETASV3 Airspeed Sensor Driver * @brief ETASV3 Airspeed Sensor Driver
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
@ -28,16 +28,39 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 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] typedef enum{ PIOS_MPXV_UNKNOWN,PIOS_MPXV_5004,PIOS_MPXV_7002 } PIOS_MPXV_sensortype;
#define P0 101.325f //static air pressure at standard sea level in kPa typedef struct{
#define VCC 5.0f //Supply voltage in V PIOS_MPXV_sensortype type;
#define POWER (2.0f/7.0f) 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); #define PIOS_MPXV_5004_DESC(pin) \
uint16_t PIOS_MPXV5004_Calibrate(uint8_t airspeedADCPin, uint16_t calibrationCount); (PIOS_MPXV_descriptor){ \
void PIOS_MPXV5004_UpdateCalibration(uint16_t zeroPoint); .type = PIOS_MPXV_5004, \
float PIOS_MPXV5004_ReadAirspeed (uint8_t airspeedADCPin); .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__
uint16_t PIOS_MPXV_Measure(PIOS_MPXV_descriptor *desc);
uint16_t PIOS_MPXV_Calibrate(PIOS_MPXV_descriptor *desc, uint16_t measurement);
float PIOS_MPXV5004_CalcAirspeed (PIOS_MPXV_descriptor *desc,uint16_t measurement);
#endif // PIOS_MPXV_H__

43
flight/PiOS/inc/pios_mpxv7002.h
View File

@ -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__

7
flight/PiOS/pios.h
View File

@ -101,11 +101,8 @@
/* PIOS Hardware Includes (Common) */ /* PIOS Hardware Includes (Common) */
#include <pios_sdcard.h> #include <pios_sdcard.h>
#include <pios_com.h> #include <pios_com.h>
#if defined(PIOS_INCLUDE_MPXV7002) #if defined(PIOS_INCLUDE_MPXV)
#include <pios_mpxv7002.h> #include <pios_mpxv.h>
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
#include <pios_mpxv5004.h>
#endif #endif
#if defined(PIOS_INCLUDE_ETASV3) #if defined(PIOS_INCLUDE_ETASV3)
#include <pios_etasv3.h> #include <pios_etasv3.h>

4
flight/RevoMini/UAVObjects.inc
View File

@ -37,11 +37,9 @@ UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += magbias UAVOBJSRCFILENAMES += magbias
UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += baroairspeed UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedactual UAVOBJSRCFILENAMES += airspeedactual
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate UAVOBJSRCFILENAMES += flightbatterystate

3
flight/Revolution/Makefile
View File

@ -181,8 +181,7 @@ include $(PIOS)/STM32F4xx/library.mk
SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_mpu6000.c
SRC += $(PIOSCOMMON)/pios_bma180.c SRC += $(PIOSCOMMON)/pios_bma180.c
SRC += $(PIOSCOMMON)/pios_etasv3.c SRC += $(PIOSCOMMON)/pios_etasv3.c
SRC += $(PIOSCOMMON)/pios_mpxv5004.c SRC += $(PIOSCOMMON)/pios_mpxv.c
SRC += $(PIOSCOMMON)/pios_mpxv7002.c
SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_l3gd20.c
SRC += $(PIOSCOMMON)/pios_hmc5883.c SRC += $(PIOSCOMMON)/pios_hmc5883.c

3
flight/Revolution/System/inc/pios_config.h
View File

@ -65,8 +65,7 @@
#define PIOS_INCLUDE_L3GD20 #define PIOS_INCLUDE_L3GD20
#define PIOS_INCLUDE_MS5611 #define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_ETASV3 #define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_MPXV5004 #define PIOS_INCLUDE_MPXV
#define PIOS_INCLUDE_MPXV7002
//#define PIOS_INCLUDE_HCSR04 //#define PIOS_INCLUDE_HCSR04
#define PIOS_FLASH_ON_ACCEL /* true for second revo */ #define PIOS_FLASH_ON_ACCEL /* true for second revo */
#define FLASH_FREERTOS #define FLASH_FREERTOS

2
flight/Revolution/UAVObjects.inc
View File

@ -37,7 +37,7 @@ UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += magbias UAVOBJSRCFILENAMES += magbias
UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += baroairspeed UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedactual UAVOBJSRCFILENAMES += airspeedactual
UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += flightbatterysettings

2
flight/SimPosix/UAVObjects.inc
View File

@ -36,7 +36,7 @@ UAVOBJSRCFILENAMES += gyrosbias
UAVOBJSRCFILENAMES += accels UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += baroairspeed UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedactual UAVOBJSRCFILENAMES += airspeedactual
UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += flightbatterysettings

4
ground/openpilotgcs/src/plugins/hitl/simulator.cpp
View File

@ -734,8 +734,8 @@ void Simulator::updateUAVOs(Output2Hardware out){
memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields)); memset(&airspeedActualData, 0, sizeof(AirspeedActual::DataFields));
airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed; airspeedActualData.CalibratedAirspeed = out.calibratedAirspeed + noise.airspeedActual.CalibratedAirspeed;
airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed; airspeedActualData.TrueAirspeed = out.trueAirspeed + noise.airspeedActual.TrueAirspeed;
airspeedActualData.alpha=out.angleOfAttack; //airspeedActualData.alpha=out.angleOfAttack; // to be implemented
airspeedActualData.beta=out.angleOfSlip; //airspeedActualData.beta=out.angleOfSlip;
airspeedActual->setData(airspeedActualData); airspeedActual->setData(airspeedActualData);
airspeedActualTime=airspeedActualTime.addMSecs(settings.airspeedActualRate); airspeedActualTime=airspeedActualTime.addMSecs(settings.airspeedActualRate);

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -25,7 +25,7 @@ OTHER_FILES += UAVObjects.pluginspec
# Add in all of the synthetic/generated uavobject files # Add in all of the synthetic/generated uavobject files
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/baroaltitude.h \ $$UAVOBJECT_SYNTHETICS/baroaltitude.h \
$$UAVOBJECT_SYNTHETICS/baroairspeed.h \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
$$UAVOBJECT_SYNTHETICS/airspeedactual.h \ $$UAVOBJECT_SYNTHETICS/airspeedactual.h \
$$UAVOBJECT_SYNTHETICS/attitudeactual.h \ $$UAVOBJECT_SYNTHETICS/attitudeactual.h \
@ -105,7 +105,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \ $$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \
$$UAVOBJECT_SYNTHETICS/baroairspeed.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedactual.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedactual.cpp \
$$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \ $$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \

6
shared/uavobjectdefinition/airspeedactual.xml
View File

@ -1,13 +1,11 @@
<xml> <xml>
<object name="AirspeedActual" singleinstance="true" settings="false"> <object name="AirspeedActual" singleinstance="true" settings="false">
<description>UAVO for true airspeed, calibrated airspeed, angle of attack, and angle of slip</description> <description>UAVO for true airspeed, calibrated airspeed, angle of attack, and angle of slip</description>
<field name="TrueAirspeed" units="m/s" type="float" elements="1"/>
<field name="CalibratedAirspeed" units="m/s" type="float" elements="1"/> <field name="CalibratedAirspeed" units="m/s" type="float" elements="1"/>
<field name="alpha" units="rad" type="float" elements="1"/> <field name="TrueAirspeed" units="m/s" type="float" elements="1"/>
<field name="beta" units="rad" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="manual" period="1000"/> <telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/> <logging updatemode="periodic" period="1000"/>
</object> </object>
</xml> </xml>

6
shared/uavobjectdefinition/baroairspeed.xml → shared/uavobjectdefinition/airspeedsensor.xml
View File

@ -1,11 +1,9 @@
<xml> <xml>
<object name="BaroAirspeed" singleinstance="true" settings="false"> <object name="AirspeedSensor" singleinstance="true" settings="false">
<description>The raw data from the dynamic pressure sensor with pressure, temperature and airspeed.</description> <description>The raw data from the dynamic pressure sensor with pressure, temperature and airspeed.</description>
<field name="BaroConnected" units="" type="enum" elements="1" options="False,True"/> <field name="SensorConnected" units="" type="enum" elements="1" options="False,True"/>
<field name="SensorValue" units="raw" type="uint16" elements="1"/> <field name="SensorValue" units="raw" type="uint16" elements="1"/>
<field name="CalibratedAirspeed" units="m/s" type="float" elements="1"/> <field name="CalibratedAirspeed" units="m/s" type="float" elements="1"/>
<field name="GPSAirspeed" units="m/s" type="float" elements="1"/>
<field name="TrueAirspeed" units="m/s" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/> <telemetryflight acked="false" updatemode="periodic" period="1000"/>

4
shared/uavobjectdefinition/airspeedsettings.xml
View File

@ -1,10 +1,10 @@
<xml> <xml>
<object name="AirspeedSettings" singleinstance="true" settings="true"> <object name="AirspeedSettings" singleinstance="true" settings="true">
<description>Settings for the @ref BaroAirspeed module used on CopterControl or Revolution</description> <description>Settings for the @ref BaroAirspeed module used on CopterControl or Revolution</description>
<field name="GPSSamplePeriod_ms" units="ms" type="uint8" elements="1" defaultvalue="100"/> <field name="SamplePeriod" units="ms" type="uint8" elements="1" defaultvalue="100"/>
<field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/> <field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/>
<field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/> <field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/>
<field name="AirspeedSensorType" units="" type="enum" elements="1" options="EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002,GPSOnly" defaultvalue="DIYDronesMPXV7002"/> <field name="AirspeedSensorType" units="" type="enum" elements="1" options="EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002" defaultvalue="DIYDronesMPXV7002"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>