rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

dded support for multiple airspeed sources. Added GPS airspeed estimation. Extended BaroAirspeed UAVO. Added new AirspeedSettings UAVO.

Browse Source
This commit is contained in:
Kenz Dale 2012-07-20 13:08:54 +02:00
parent 5a5694883d
commit 1d976b3095
14 changed files with 672 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3,12 +3,12 @@
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Communicate with BMP085 and update @ref BaroAirspeed "BaroAirspeed UAV Object"
* @brief Calculate airspeed from diverse sources and update @ref Airspeed "Airspeed UAV Object"
* @{
*
* @file airspeed.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Airspeed module, handles temperature and pressure readings from BMP085
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Airspeed module
*
* @see The GNU Public License (GPL) Version 3
*
@ -39,29 +39,67 @@
#include "openpilot.h"
#include "hwsettings.h"
#include "airspeed.h"
#include "baroairspeed.h" // object that will be updated by the module
#if defined(PIOS_INCLUDE_HCSR04)
#include "sonarairspeed.h" // object that will be updated by the module
#endif
#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 "attitudeactual.h"
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 500
#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
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
#define SAMPLING_DELAY_MS 50
#define CALIBRATION_IDLE 40
#define CALIBRATION_COUNT 40
#define ETS_AIRSPEED_SCALE 1.0f
#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 SAMPLING_DELAY_MS_GPS 100 //Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO
#define F_PI 3.141526535897932f
#define DEG2RAD (F_PI/180.0f)
// Private types
// Private variables
static xTaskHandle taskHandle;
static bool airspeedEnabled = false;
volatile bool gpsNew = false;
static uint8_t airspeedSensorType;
static uint16_t gpsSamplePeriod_ms;
#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
static bool airspeedEnabled = false;
/**
* Initialise the module, called on startup
@ -69,14 +107,15 @@ static bool airspeedEnabled = false;
*/
int32_t AirspeedStart()
{
//Check if module is enabled or not
if (airspeedEnabled == false) {
return -1;
}
// Start main task
xTaskCreate(airspeedTask, (signed char *)"Airspeed", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_AIRSPEED, taskHandle);
return 0;
}
@ -86,73 +125,197 @@ int32_t AirspeedStart()
*/
int32_t AirspeedInitialize()
{
#ifdef MODULE_AIRSPEED_BUILTIN
#ifdef MODULE_AIRSPEED_BUILTIN
airspeedEnabled = true;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
airspeedEnabled = true;
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
airspeedEnabled = true;
} else {
airspeedEnabled = false;
return -1;
} else {
airspeedEnabled = false;
return -1;
}
#endif
#ifdef BARO_AIRSPEED_PRESENT
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingGet(adcRouting);
//Determine if the barometric airspeed sensor is routed to an ADC pin
for (int i=0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
if (adcRouting[i] == HWSETTINGS_ADCROUTING_ANALOGAIRSPEED) {
airspeedADCPin = i;
}
#endif
}
#endif
BaroAirspeedInitialize();
AirspeedSettingsInitialize();
AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);
return 0;
}
MODULE_INITCALL(AirspeedInitialize, AirspeedStart)
/**
* Module thread, should not return.
*/
static void airspeedTask(void *parameters)
{
BaroAirspeedData data;
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
uint8_t calibrationCount = 0;
uint32_t calibrationSum = 0;
BaroAirspeedData airspeedData;
BaroAirspeedGet(&airspeedData);
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
#ifdef BARO_AIRSPEED_PRESENT
portTickType lastGPSTime= xTaskGetTickCount();
float airspeedErrInt=0;
#endif
//GPS airspeed calculation variables
#ifdef GPS_AIRSPEED_PRESENT
GPSVelocityConnectCallback(GPSVelocityUpdatedCb);
gps_airspeedInitialize();
#endif
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1)
{
// Update the airspeed
vTaskDelay(SAMPLING_DELAY_MS);
float airspeed_tas_baro=0;
BaroAirspeedGet(&data);
data.SensorValue = PIOS_ETASV3_ReadAirspeed();
if (data.SensorValue==-1) {
data.Connected = BAROAIRSPEED_CONNECTED_FALSE;
data.Airspeed = 0;
BaroAirspeedSet(&data);
continue;
// Update the airspeed object
BaroAirspeedGet(&airspeedData);
#ifdef BARO_AIRSPEED_PRESENT
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
#define T0 288.15f
float baroTemperature;
BaroAltitudeTemperatureGet(&baroTemperature);
baroTemperature-=5; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
airspeed_tas_baro=airspeedData.CAS * sqrtf((baroTemperature+273.15)/T0) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
}
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 - lastGPSTime)/1000.0f;
if ( (delT > portTICK_RATE_MS || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY)
&& gpsNew) {
lastGPSTime=lastSysTime;
#else
if (gpsNew) {
#endif
gpsNew=false; //Do this first
if (calibrationCount<CALIBRATION_IDLE) {
calibrationCount++;
} else if (calibrationCount<CALIBRATION_IDLE + CALIBRATION_COUNT) {
calibrationCount++;
calibrationSum += data.SensorValue;
if (calibrationCount==CALIBRATION_IDLE+CALIBRATION_COUNT) {
data.ZeroPoint = calibrationSum / CALIBRATION_COUNT;
} else {
continue;
//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;
}
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);
}
}
data.Connected = BAROAIRSPEED_CONNECTED_TRUE;
data.Airspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(data.SensorValue - data.ZeroPoint));
// Update the AirspeedActual UAVObject
BaroAirspeedSet(&data);
#endif
//Legacy UAVO support, this needs to be removed in favor of using TAS and CAS, as appropriate.
if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_FALSE){ //If we only have a GPS, use GPS data as airspeed...
airspeedData.Airspeed=airspeedData.TrueAirspeed;
}
else{ //...otherwise, use the only baro airspeed because we still don't trust true airspeed calculations
airspeedData.Airspeed=airspeedData.CAS;
}
//Set the UAVO
BaroAirspeedSet(&airspeedData);
}
}
#ifdef GPS_AIRSPEED_PRESENT
static void GPSVelocityUpdatedCb(UAVObjEvent * ev)
{
gpsNew=true;
}
#endif
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
{
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
airspeedSensorType=airspeedSettingsData.AirspeedSensorType;
gpsSamplePeriod_ms=airspeedSettingsData.GPSSamplePeriod_ms;
#if defined(PIOS_INCLUDE_MPXV7002)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
}
/**
* @}
* @}

222
flight/Modules/Airspeed/revolution/baro_airspeed.c Normal file
View File

@ -0,0 +1,222 @@
/**
******************************************************************************
* @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.
*
*/
#define AIRSPEED_STORED_IN_A_PROPER_UAVO FALSE //AIRSPEED SHOULD NOT GO INTO BaroAirspeed. IT IS A STATE VARIABLE, AND NOT A MEASUREMENT
#include "openpilot.h"
#include "hwsettings.h"
#include "airspeed.h"
#include "airspeedsettings.h"
#include "baroairspeed.h" // object that will be updated by the module
#define SAMPLING_DELAY_MS_ETASV3 50 //Update at 20Hz
#define SAMPLING_DELAY_MS_MPXV 10 //Update at 100Hz
#define ETS_AIRSPEED_SCALE 1.0f //???
#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms]
#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms]
#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100.0f //Needs to be settable in a UAVO
// Private types
// Private variables
// Private functions
static uint16_t calibrationCount=0;
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
#if defined(PIOS_INCLUDE_ETASV3)
static uint32_t calibrationSum = 0;
#endif
//Find out which sensor we're using.
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002 || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){ //MPXV5004 and MPXV7002 sensors
#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004)
//Ensure that the ADC pin is properly configured
if(airspeedADCPin <0){ //It's not, so revert to former sensor type
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
airspeedSettingsData.AirspeedSensorType=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY;
AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT?
return;
}
//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 */
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
uint16_t sensorCalibration=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
airspeedSettingsData.ZeroPoint=sensorCalibration;
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
uint16_t sensorCalibration=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
airspeedSettingsData.ZeroPoint=sensorCalibration;
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
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)
AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT?
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->CAS*(1.0f-alpha);
//Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one
baroAirspeedData->CAS = filteredAirspeed;
#else
//Whoops, no sensor defined in pios_config.h. Revert to GPS.
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
airspeedSettingsData.AirspeedSensorType=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY;
AirspeedSettingsSet(&airspeedSettingsData); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT?
return;
#endif
}
else if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3){ //Eagletree Airspeed v3
#if defined(PIOS_INCLUDE_ETASV3)
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
//Wait until our turn. //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE
vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_ETASV3 / portTICK_RATE_MS);
//Check to see if airspeed sensor is returning baroAirspeedData
baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (baroAirspeedData->SensorValue==-1) {
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
baroAirspeedData->Airspeed = 0;
BaroAirspeedSet(&baroAirspeedData);
return;
}
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) {
calibrationCount++;
return;
} else if (calibrationCount < (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
calibrationCount++;
calibrationSum += baroAirspeedData->SensorValue;
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
airspeedSettingsData.ZeroPoint=(uint16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f);
AirspeedSettingsSet(&airspeedSettingsData);
} else {
return;
}
}
//Compute airspeed
float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed?
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
baroAirspeedData->CAS = calibratedAirspeed;
#else
//Whoops, no EagleTree sensor defined in pios_config.h. Declare it unconnected...
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
//...and revert to GPS.
if (airspeedSensorType!=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) {
uint8_t tmpVal=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY;
AirspeedSettingsAirspeedSensorTypeSet(&tmpVal); // IS THERE ANY WAY TO SET A MUTEX ON THIS BEFORE SETTING IT?
}
#endif
}
}
/**
* @}
* @}
*/

154
flight/Modules/Airspeed/revolution/gps_airspeed.c Normal file
View File

@ -0,0 +1,154 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Use GPS data to estimate airspeed
* @{
*
* @file gps_airspeed.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @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
*/
#include "openpilot.h"
#include "gps_airspeed.h"
#include "gpsvelocity.h"
#include "attitudeactual.h"
#include "CoordinateConversions.h"
// Private constants
#define GPS_AIRSPEED_BIAS_KP 0.1f //Needs to be settable in a UAVO
#define GPS_AIRSPEED_BIAS_KI 0.1f //Needs to be settable in a UAVO
#define SAMPLING_DELAY_MS_GPS 100 //Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO
#define F_PI 3.141526535897932f
#define DEG2RAD (F_PI/180.0f)
// Private types
struct GPSGlobals {
float RbeCol1_old[3];
float gpsVelOld_N;
float gpsVelOld_E;
float gpsVelOld_D;
};
// Private variables
static struct GPSGlobals *gps;
// Private functions
/*
* Initialize function loads first data sets, and allocates memory for structure.
*/
void gps_airspeedInitialize()
{
//This method saves memory in case we don't use the GPS module.
gps=(struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals));
//GPS airspeed calculation variables
GPSVelocityData gpsVelData;
GPSVelocityGet(&gpsVelData);
gps->gpsVelOld_N=gpsVelData.North;
gps->gpsVelOld_E=gpsVelData.East;
gps->gpsVelOld_D=gpsVelData.Down;
AttitudeActualData attData;
AttitudeActualGet(&attData);
float Rbe[3][3];
float q[4] ={attData.q1, attData.q2, attData.q3, attData.q4};
//Calculate rotation matrix
Quaternion2R(q, Rbe);
gps->RbeCol1_old[0]=Rbe[0][0];
gps->RbeCol1_old[1]=Rbe[0][1];
gps->RbeCol1_old[2]=Rbe[0][2];
}
/*
* Calculate airspeed as a function of GPS groundspeed and vehicle attitude.
* From "IMU Wind Estimation (Theory)", by William Premerlani.
* The idea is that V_gps=V_air+V_wind. If we assume wind constant, =>
* V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1.
* If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1),
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
*/
void gps_airspeedGet(float *v_air_GPS)
{
float Rbe[3][3];
{ //Scoping to save memory. We really just need Rbe.
AttitudeActualData attData;
AttitudeActualGet(&attData);
float q[4] ={attData.q1, attData.q2, attData.q3, attData.q4};
//Calculate rotation matrix
Quaternion2R(q, Rbe);
}
//Calculate the cos(angle) between the two fuselage basis vectors
float cosDiff=(Rbe[0][0]*gps->RbeCol1_old[0]) + (Rbe[0][1]*gps->RbeCol1_old[1]) + (Rbe[0][2]*gps->RbeCol1_old[2]);
//If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabs(cosDiff) < cos(5.0f*DEG2RAD)) {
GPSVelocityData gpsVelData;
GPSVelocityGet(&gpsVelData);
//Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North-gps->gpsVelOld_N,2.0f) + powf(gpsVelData.East-gps->gpsVelOld_E,2.0f) + powf(gpsVelData.Down-gps->gpsVelOld_D,2.0f);
//Calculate the norm^2 of the difference between the two fuselage vectors
float normDiffAttitude2=powf(Rbe[0][0]-gps->RbeCol1_old[0],2.0f) + powf(Rbe[0][1]-gps->RbeCol1_old[1],2.0f) + powf(Rbe[0][2]-gps->RbeCol1_old[2],2.0f);
//Airspeed magnitude is the ratio between the two difference norms
*v_air_GPS = sqrtf(normDiffGPS2/normDiffAttitude2);
//Save old variables for next pass
gps->gpsVelOld_N=gpsVelData.North;
gps->gpsVelOld_E=gpsVelData.East;
gps->gpsVelOld_D=gpsVelData.Down;
gps->RbeCol1_old[0]=Rbe[0][0];
gps->RbeCol1_old[1]=Rbe[0][1];
gps->RbeCol1_old[2]=Rbe[0][2];
}
else {
}
}
/**
* @}
* @}
*/

42
flight/Modules/Airspeed/revolution/inc/gps_airspeed.h Normal file
View File

@ -0,0 +1,42 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
* @{
*
* @file gps_airspeed.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Airspeed module, reads temperature and pressure from BMP085
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef GPS_AIRSPEED_H
#define GPS_AIRSPEED_H
void gps_airspeedInitialize();
void gps_airspeedGet(float *v_air_GPS);
#endif // GPS_AIRSPEED_H
/**
* @}
* @}
*/

2
flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -708,7 +708,7 @@ static void baroAirspeedUpdatedCb(UAVObjEvent * ev)
VelocityActualData velocityActual;
BaroAirspeedGet(&baroAirspeed);
if (baroAirspeed.Connected != BAROAIRSPEED_CONNECTED_TRUE && BaroAirspeedReadOnly()) {
if (baroAirspeed.BaroConnected != BAROAIRSPEED_BAROCONNECTED_TRUE && BaroAirspeedReadOnly()) {
baroAirspeedBias = 0;
} else {
VelocityActualGet(&velocityActual);

2
flight/Modules/PathPlanner/pathplanner.c
View File

@ -484,7 +484,7 @@ static uint8_t conditionAboveSpeed() {
if (pathAction.ConditionParameters[1]>0.5f) {
BaroAirspeedData baroAirspeed;
BaroAirspeedGet (&baroAirspeed);
if (baroAirspeed.Connected == BAROAIRSPEED_CONNECTED_TRUE) {
if (baroAirspeed.BaroConnected == BAROAIRSPEED_BAROCONNECTED_TRUE) {
velocity = baroAirspeed.Airspeed;
}
}

8
flight/PiOS/Boards/STM32F4xx_Revolution.h
View File

@ -217,8 +217,8 @@ extern uint32_t pios_com_vcp_id;
//-------------------------
// Receiver DSM input
//-------------------------
#define PIOS_DSM_MAX_DEVS 2
#define PIOS_DSM_NUM_INPUTS 12
#define PIOS_DSM_MAX_DEVS 2
#define PIOS_DSM_NUM_INPUTS 12
//-------------------------
// Servo outputs
@ -242,8 +242,8 @@ extern uint32_t pios_com_vcp_id;
{ \
{GPIOC, GPIO_Pin_0, ADC_Channel_10}, \
{GPIOC, GPIO_Pin_1, ADC_Channel_11}, \
{NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \
{NULL, 0, ADC_Channel_TempSensor} /* Temperature sensor */ \
{NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \
{NULL, 0, ADC_Channel_TempSensor} /* Temperature sensor */ \
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */

3
flight/PiOS/Common/pios_mpxv5004.c
View File

@ -76,10 +76,7 @@ void PIOS_MPXV5004_UpdateCalibration(uint16_t zeroPoint){
*/
float PIOS_MPXV5004_ReadAirspeed(uint8_t airspeedADCPin)
{
float sensorVal = PIOS_MPXV5004_Measure(airspeedADCPin);
if (sensorVal == 0) //As of June 20th, 2012, the ADC read module was written in such a way that it reset the value to 0 immediately after a read. In case another module inadvertently reads the ADC, we prefer not to use a reading of 0
return -1;
//Calculate dynamic pressure, as per docs
float Qc = 5.0f*(((sensorVal - calibrationOffset)/4096.0f*3.3f)/VCC - 0.2f);

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

@ -76,10 +76,7 @@ void PIOS_MPXV7002_UpdateCalibration(uint16_t zeroPoint){
*/
float PIOS_MPXV7002_ReadAirspeed(uint8_t airspeedADCPin)
{
float sensorVal = PIOS_MPXV7002_Measure(airspeedADCPin);
if (sensorVal == 0) //As of June 20th, 2012, the ADC read module was written in such a way that it reset the value to 0 immediately after a read. In case someone else reads the ADC inadvertently, we prefer not to use a reading of 0
return -1;
//Calculate dynamic pressure, as per docs
float Qc = 5.0f*(((sensorVal - calibrationOffset)/4096.0f*3.3f)/VCC - 0.5f);

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

@ -64,8 +64,8 @@
#define PIOS_INCLUDE_L3GD20
#define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_ETASV3
//#define PIOS_INCLUDE_MPXV5004
//#define PIOS_INCLUDE_MPXV7002
#define PIOS_INCLUDE_MPXV5004
#define PIOS_INCLUDE_MPXV7002
//#define PIOS_INCLUDE_HCSR04
#define PIOS_FLASH_ON_ACCEL /* true for second revo */
#define FLASH_FREERTOS
@ -101,12 +101,12 @@
#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 1000
#define HEAP_LIMIT_CRITICAL 500
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
#define HEAP_LIMIT_WARNING 1000
#define HEAP_LIMIT_CRITICAL 500
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
// This actually needs calibrating
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD (8379692)

1
flight/Revolution/UAVObjects.inc
View File

@ -37,6 +37,7 @@ UAVOBJSRCFILENAMES += accels
UAVOBJSRCFILENAMES += magnetometer
UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += baroairspeed
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate

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

@ -26,6 +26,7 @@ OTHER_FILES += UAVObjects.pluginspec
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/baroaltitude.h \
$$UAVOBJECT_SYNTHETICS/baroairspeed.h \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
$$UAVOBJECT_SYNTHETICS/attitudeactual.h \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
@ -99,6 +100,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \
$$UAVOBJECT_SYNTHETICS/baroairspeed.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \
$$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \

13
shared/uavobjectdefinition/airspeedsettings.xml Normal file
View File

@ -0,0 +1,13 @@
<xml>
<object name="AirspeedSettings" singleinstance="true" settings="true">
<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="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="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"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

6
shared/uavobjectdefinition/baroairspeed.xml
View File

@ -1,10 +1,12 @@
<xml>
<object name="BaroAirspeed" singleinstance="true" settings="false">
<description>The raw data from the dynamic pressure sensor with pressure, temperature and airspeed.</description>
<field name="Connected" units="" type="enum" elements="1" options="False,True"/>
<field name="BaroConnected" units="" type="enum" elements="1" options="False,True"/>
<field name="SensorValue" units="raw" type="uint16" elements="1"/>
<field name="ZeroPoint" units="raw" type="uint16" elements="1"/>
<field name="Airspeed" units="m/s" type="float" elements="1"/>
<field name="CAS" 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"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>