mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
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
This commit is contained in:
Corvus Corax
parent
ce595a06d9
commit
b09027afe5
@ -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);
|
||||
AirspeedSensorGet(&airspeedData);
|
||||
|
||||
#ifdef BARO_AIRSPEED_PRESENT
|
||||
float airspeed_tas_baro=0;
|
||||
|
||||
if(airspeedSensorType!=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY){
|
||||
//Fetch calibrated airspeed from sensors
|
||||
baro_airspeedGet(&airspeedData, &lastSysTime, airspeedSensorType, airspeedADCPin);
|
||||
|
||||
//Calculate true airspeed, not taking into account compressibility effects
|
||||
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
|
||||
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)
|
||||
switch (airspeedSettings.AirspeedSensorType) {
|
||||
#if defined(PIOS_INCLUDE_MPXV)
|
||||
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);
|
||||
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);
|
||||
baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
|
||||
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
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -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,50 +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;
|
||||
}
|
||||
|
||||
// only calibrate if no stored calibration is available
|
||||
if (!airspeedSettingsData.ZeroPoint) {
|
||||
if (!airspeedSettings->ZeroPoint) {
|
||||
//Calibrate sensor by averaging zero point value
|
||||
if (calibrationCount <= CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) {
|
||||
if (calibrationCount <= CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) {
|
||||
calibrationCount++;
|
||||
calibrationCount2++;
|
||||
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++;
|
||||
calibrationSum += baroAirspeedData->SensorValue;
|
||||
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
|
||||
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 {
|
||||
return;
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
118
flight/Modules/Airspeed/revolution/baro_airspeed_mpxv.c
Normal file
118
flight/Modules/Airspeed/revolution/baro_airspeed_mpxv.c
Normal 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
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -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
|
||||
|
||||
/**
|
||||
* @}
|
||||
|
||||
@ -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
|
||||
|
||||
/**
|
||||
* @}
|
||||
@ -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();
|
||||
}
|
||||
|
||||
|
||||
@ -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 */
|
||||
@ -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 */
|
||||
@ -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__
|
||||
|
||||
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__
|
||||
@ -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__
|
||||
@ -101,11 +101,8 @@
|
||||
/* PIOS Hardware Includes (Common) */
|
||||
#include <pios_sdcard.h>
|
||||
#include <pios_com.h>
|
||||
#if defined(PIOS_INCLUDE_MPXV7002)
|
||||
#include <pios_mpxv7002.h>
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_MPXV5004)
|
||||
#include <pios_mpxv5004.h>
|
||||
#if defined(PIOS_INCLUDE_MPXV)
|
||||
#include <pios_mpxv.h>
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_ETASV3)
|
||||
#include <pios_etasv3.h>
|
||||
|
||||
@ -37,11 +37,9 @@ UAVOBJSRCFILENAMES += accels
|
||||
UAVOBJSRCFILENAMES += magnetometer
|
||||
UAVOBJSRCFILENAMES += magbias
|
||||
UAVOBJSRCFILENAMES += baroaltitude
|
||||
UAVOBJSRCFILENAMES += baroairspeed
|
||||
UAVOBJSRCFILENAMES += airspeedsensor
|
||||
UAVOBJSRCFILENAMES += airspeedsettings
|
||||
UAVOBJSRCFILENAMES += airspeedactual
|
||||
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings
|
||||
UAVOBJSRCFILENAMES += fixedwingpathfollowerstatus
|
||||
UAVOBJSRCFILENAMES += flightbatterysettings
|
||||
UAVOBJSRCFILENAMES += firmwareiapobj
|
||||
UAVOBJSRCFILENAMES += flightbatterystate
|
||||
|
||||
@ -181,8 +181,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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -37,7 +37,7 @@ UAVOBJSRCFILENAMES += accels
|
||||
UAVOBJSRCFILENAMES += magnetometer
|
||||
UAVOBJSRCFILENAMES += magbias
|
||||
UAVOBJSRCFILENAMES += baroaltitude
|
||||
UAVOBJSRCFILENAMES += baroairspeed
|
||||
UAVOBJSRCFILENAMES += airspeedsensor
|
||||
UAVOBJSRCFILENAMES += airspeedsettings
|
||||
UAVOBJSRCFILENAMES += airspeedactual
|
||||
UAVOBJSRCFILENAMES += flightbatterysettings
|
||||
|
||||
@ -36,7 +36,7 @@ UAVOBJSRCFILENAMES += gyrosbias
|
||||
UAVOBJSRCFILENAMES += accels
|
||||
UAVOBJSRCFILENAMES += magnetometer
|
||||
UAVOBJSRCFILENAMES += baroaltitude
|
||||
UAVOBJSRCFILENAMES += baroairspeed
|
||||
UAVOBJSRCFILENAMES += airspeedsensor
|
||||
UAVOBJSRCFILENAMES += airspeedsettings
|
||||
UAVOBJSRCFILENAMES += airspeedactual
|
||||
UAVOBJSRCFILENAMES += flightbatterysettings
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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 \
|
||||
@ -105,7 +105,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 \
|
||||
|
||||
@ -1,13 +1,11 @@
|
||||
<xml>
|
||||
<object name="AirspeedActual" singleinstance="true" settings="false">
|
||||
<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="alpha" units="rad" type="float" elements="1"/>
|
||||
<field name="beta" units="rad" 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="manual" period="1000"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
<logging updatemode="periodic" period="1000"/>
|
||||
</object>
|
||||
</xml>
|
||||
|
||||
@ -1,11 +1,9 @@
|
||||
<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>
|
||||
<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="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"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
@ -1,10 +1,10 @@
|
||||
<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="SamplePeriod" 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"/>
|
||||
<field name="AirspeedSensorType" units="" type="enum" elements="1" options="EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002" defaultvalue="DIYDronesMPXV7002"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user