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

Merge branch 'revo-fixes' of ssh://git.openpilot.org/OpenPilot into brian/rfm22_autoconf

Browse Source 
Conflicts:
	flight/RevoMini/Makefile
This commit is contained in:
Brian Webb 2012-12-30 07:59:09 -07:00
commit 9f3d7a24f8
36 changed files with 502 additions and 674 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/CopterControl/System/pios_board.c
View File

@ -111,7 +111,7 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
.exti_cfg = &pios_exti_mpu6000_cfg,
.Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
// Clock at 8 khz, downsampled by 8 for 500 Hz
// Clock at 8 khz, downsampled by 16 for 500 Hz
.Smpl_rate_div = 15,
.interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
.interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY,

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 "hwsettings.h"
#include "airspeed.h"
#include "gpsvelocity.h"
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "baroaltitude.h"
#include "baroairspeed.h" // object that will be updated by the module
#include "airspeedactual.h" // object that will be updated by the module
#include "attitudeactual.h"
#include "CoordinateConversions.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_analog.h"
#include "baro_airspeed_mpxv.h"
// Private constants
#if defined (PIOS_INCLUDE_GPS)
#define GPS_AIRSPEED_PRESENT
#endif
#if defined (PIOS_INCLUDE_MPXV5004) || defined (PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_ETASV3)
#define BARO_AIRSPEED_PRESENT
#endif
#if defined (GPS_AIRSPEED_PRESENT) && defined (BARO_AIRSPEED_PRESENT)
#define STACK_SIZE_BYTES 700
#elif defined (GPS_AIRSPEED_PRESENT)
#define STACK_SIZE_BYTES 600
#elif defined (BARO_AIRSPEED_PRESENT)
#define STACK_SIZE_BYTES 550
#else
#define STACK_SIZE_BYTES 0
#define NO_AIRSPEED_SENSOR_PRESENT
#endif
#define STACK_SIZE_BYTES 500
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
#define SAMPLING_DELAY_MS_FALLTHROUGH 50 //Fallthrough update at 20Hz. The fallthrough runs faster than the GPS to ensure that we don't miss GPS updates because we're slightly out of sync
#define GPS_AIRSPEED_BIAS_KP 0.01f //Needs to be settable in a UAVO
#define GPS_AIRSPEED_BIAS_KI 0.01f //Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO
#define BARO_TRUEAIRSPEED_TIME_CONSTANT_S 1.0f //Needs to be settable in a UAVO
#define F_PI 3.141526535897932f
#define DEG2RAD (F_PI/180.0f)
#define T0 288.15f
#define BARO_TEMPERATURE_OFFSET 5
// Private types
// Private variables
static xTaskHandle taskHandle;
static bool airspeedEnabled = false;
volatile bool gpsNew = false;
static uint8_t airspeedSensorType;
static uint16_t gpsSamplePeriod_ms;
static AirspeedSettingsData airspeedSettings;
#ifdef BARO_AIRSPEED_PRESENT
static int8_t airspeedADCPin=-1;
#endif
// Private functions
static void airspeedTask(void *parameters);
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin);
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev);
#ifdef GPS_AIRSPEED_PRESENT
static void GPSVelocityUpdatedCb(UAVObjEvent * ev);
#endif
/**
* Initialise the module, called on startup
@ -115,9 +71,6 @@ static void GPSVelocityUpdatedCb(UAVObjEvent * ev);
*/
int32_t AirspeedStart()
{
#if defined (NO_AIRSPEED_SENSOR_PRESENT)
return -1;
#endif
//Check if module is enabled or not
if (airspeedEnabled == false) {
@ -153,7 +106,6 @@ int32_t AirspeedInitialize()
}
#endif
#ifdef BARO_AIRSPEED_PRESENT
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingGet(adcRouting);
@ -164,10 +116,7 @@ int32_t AirspeedInitialize()
}
}
#endif
BaroAirspeedInitialize();
AirspeedActualInitialize();
AirspeedSensorInitialize();
AirspeedSettingsInitialize();
AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);
@ -184,198 +133,54 @@ static void airspeedTask(void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
BaroAirspeedData airspeedData;
AirspeedActualData airspeedActualData;
BaroAirspeedGet(&airspeedData);
AirspeedActualGet(&airspeedActualData);
AirspeedSensorData airspeedData;
AirspeedSensorGet(&airspeedData);
AirspeedSettingsUpdatedCb(NULL);
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
#ifdef BARO_AIRSPEED_PRESENT
portTickType lastGPSTime = xTaskGetTickCount(); //Time since last GPS-derived airspeed calculation
portTickType lastLoopTime= xTaskGetTickCount(); //Time since last loop
float airspeedErrInt=0;
#endif
//GPS airspeed calculation variables
#ifdef GPS_AIRSPEED_PRESENT
GPSVelocityConnectCallback(GPSVelocityUpdatedCb);
gps_airspeedInitialize();
#endif
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
// Main task loop
portTickType lastSysTime = xTaskGetTickCount();
while (1)
{
vTaskDelayUntil(&lastSysTime, airspeedSettings.SamplePeriod / portTICK_RATE_MS);
// Update the airspeed object
BaroAirspeedGet(&airspeedData);
AirspeedActualGet(&airspeedActualData);
#ifdef BARO_AIRSPEED_PRESENT
float airspeed_tas_baro=0;
if(airspeedSensorType!=AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY){
//Fetch calibrated airspeed from sensors
baro_airspeedGet(&airspeedData, &lastSysTime, airspeedSensorType, airspeedADCPin);
//Calculate true airspeed, not taking into account compressibility effects
float baroTemperature;
BaroAltitudeTemperatureGet(&baroTemperature);
baroTemperature-=BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
#ifdef GPS_AIRSPEED_PRESENT
//GPS present, so use baro sensor to filter TAS
airspeed_tas_baro=airspeedData.CalibratedAirspeed * sqrtf((baroTemperature+273.15)/T0) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#else
//No GPS, so TAS comes only from baro sensor
airspeedData.TrueAirspeed=airspeedData.CalibratedAirspeed * sqrtf((baroTemperature+273.15)/T0) + airspeedErrInt * GPS_AIRSPEED_BIAS_KI;
#endif
}
else
#endif
{ //Have to catch the fallthrough, or else this loop will monopolize the processor!
airspeedData.BaroConnected=BAROAIRSPEED_BAROCONNECTED_FALSE;
airspeedData.SensorValue=12345;
//Likely, we have a GPS, so let's configure the fallthrough at close to GPS refresh rates
vTaskDelayUntil(&lastSysTime, SAMPLING_DELAY_MS_FALLTHROUGH / portTICK_RATE_MS);
}
#ifdef GPS_AIRSPEED_PRESENT
float v_air_GPS=-1.0f;
//Check if sufficient time has passed. This will depend on whether we have a pitot tube
//sensor or not. In the case we do, shoot for about once per second. Otherwise, consume GPS
//as quickly as possible.
#ifdef BARO_AIRSPEED_PRESENT
float delT = (lastSysTime - lastLoopTime)/(portTICK_RATE_MS*1000.0f);
lastLoopTime=lastSysTime;
if ( ((lastSysTime - lastGPSTime) > 1000*portTICK_RATE_MS || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY)
&& gpsNew) {
lastGPSTime=lastSysTime;
#else
if (gpsNew) {
#endif
gpsNew=false; //Do this first
AirspeedSensorGet(&airspeedData);
//Calculate airspeed as a function of GPS groundspeed and vehicle attitude. From "IMU Wind Estimation (Theory)", by William Premerlani
gps_airspeedGet(&v_air_GPS);
}
//Use the GPS error to correct the airspeed estimate.
if (v_air_GPS > 0) //We have valid GPS estimate...
{
airspeedData.GPSAirspeed=v_air_GPS;
#ifdef BARO_AIRSPEED_PRESENT
if(airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){ //Check if there is an airspeed sensors present...
//Calculate error and error integral
float airspeedErr=v_air_GPS - airspeed_tas_baro;
airspeedErrInt+=airspeedErr * delT;
//Saturate integral component at 5 m/s
airspeedErrInt = airspeedErrInt > (5.0f / GPS_AIRSPEED_BIAS_KI) ? (5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
airspeedErrInt = airspeedErrInt < -(5.0f / GPS_AIRSPEED_BIAS_KI) ? -(5.0f / GPS_AIRSPEED_BIAS_KI) : airspeedErrInt;
//There's already an airspeed sensor, so instead correct it for bias with P correction. The I correction happened earlier in the function.
airspeedData.TrueAirspeed = airspeed_tas_baro + airspeedErr * GPS_AIRSPEED_BIAS_KP;
/* Note:
This would be a good place to change the airspeed calibration, so that it matches the GPS computed values. However,
this might be a bad idea, as their are two degrees of freedom here: temperature and sensor calibration. I don't
know how to control for temperature bias.
*/
}
else
#endif
{
//...there's no airspeed sensor, so everything comes from GPS. In this
//case, filter the airspeed for smoother output
float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
airspeedData.TrueAirspeed=v_air_GPS*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
//Calculate calibrated airspeed from GPS, since we're not getting it from a discrete airspeed sensor
float baroTemperature;
BaroAltitudeTemperatureGet(&baroTemperature);
baroTemperature-=BARO_TEMPERATURE_OFFSET; //Do this just because we suspect that the board heats up relative to its surroundings. THIS IS BAD(tm)
airspeedData.CalibratedAirspeed =airspeedData.TrueAirspeed / sqrtf((baroTemperature+273.15)/T0);
}
}
#ifdef BARO_AIRSPEED_PRESENT
else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){
//No GPS velocity estimate this loop, so filter true airspeed data with baro airspeed
float alpha=delT/(delT + BARO_TRUEAIRSPEED_TIME_CONSTANT_S); //Low pass filter.
airspeedData.TrueAirspeed=airspeed_tas_baro*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
}
#endif
#endif
//Set the UAVO
airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed;
airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
BaroAirspeedSet(&airspeedData);
AirspeedActualSet(&airspeedActualData);
}
}
#ifdef GPS_AIRSPEED_PRESENT
static void GPSVelocityUpdatedCb(UAVObjEvent * ev)
{
gpsNew=true;
}
#endif
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
//Find out which sensor we're using.
switch (airspeedSensorType) {
#if defined(PIOS_INCLUDE_MPXV7002)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
#endif
#if defined(PIOS_INCLUDE_MPXV7002) || defined(PIOS_INCLUDE_MPXV5004)
//MPXV5004 and MPXV7002 sensors
baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
switch (airspeedSettings.AirspeedSensorType) {
#if defined(PIOS_INCLUDE_MPXV)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
//MPXV5004 and MPXV7002 sensors
baro_airspeedGetMPXV(&airspeedData, &airspeedSettings,airspeedADCPin);
break;
#endif
#if defined(PIOS_INCLUDE_ETASV3)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
//Eagletree Airspeed v3
baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
//Eagletree Airspeed v3
baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
break;
#endif
default:
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
default:
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
}
//Set the UAVO
AirspeedSensorSet(&airspeedData);
}
}
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
{
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
AirspeedSettingsGet(&airspeedSettings);
airspeedSensorType=airspeedSettingsData.AirspeedSensorType;
gpsSamplePeriod_ms=airspeedSettingsData.GPSSamplePeriod_ms;
#if defined(PIOS_INCLUDE_MPXV7002)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
#endif
}

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

@ -1,143 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Communicate with airspeed sensors and return values
* @{
*
* @file baro_airspeed.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Airspeed module, handles temperature and pressure readings from BMP085
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Output object: BaroAirspeed
*
* This module will periodically update the value of the BaroAirspeed object.
*
*/
#include "openpilot.h"
#include "hwsettings.h"
#include "airspeed.h"
#include "airspeedsettings.h"
#include "baroairspeed.h" // object that will be updated by the module
#if defined(PIOS_INCLUDE_MPXV7002) || defined (PIOS_INCLUDE_MPXV5004)
#define SAMPLING_DELAY_MS_MPXV 10 //Update at 100Hz
#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms]
#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms]
#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100.0f //Needs to be settable in a UAVO
// Private types
// Private variables
// Private functions
static uint16_t calibrationCount=0;
void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
//Ensure that the ADC pin is properly configured
if(airspeedADCPin <0){ //It's not, so revert to former sensor type
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
return;
}
//Wait until our turn //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE
vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_MPXV / portTICK_RATE_MS);
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV) { //First let sensor warm up and stabilize.
calibrationCount++;
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV) { //Then compute the average.
calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */
uint16_t sensorCalibration;
if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
sensorCalibration=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
PIOS_MPXV7002_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
sensorCalibration=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
PIOS_MPXV5004_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
//Set settings UAVO. The airspeed UAVO is set elsewhere in the function.
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV)
AirspeedSettingsZeroPointSet(&sensorCalibration);
return;
}
//Get CAS
float calibratedAirspeed=0;
if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
calibratedAirspeed = PIOS_MPXV7002_ReadAirspeed(airspeedADCPin);
if (calibratedAirspeed < 0) //This only occurs when there's a bad ADC reading.
return;
//Get sensor value, just for telemetry purposes.
//This is a silly waste of resources, and should probably be removed at some point in the future.
//At this time, PIOS_MPXV7002_Measure() should become a static function and be removed from the header file.
//
//Moreover, due to the way the ADC driver is currently written, this code will return 0 more often than
//not. This is something that will have to change on the ADC side of things.
baroAirspeedData->SensorValue=PIOS_MPXV7002_Measure(airspeedADCPin);
}
else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
calibratedAirspeed = PIOS_MPXV5004_ReadAirspeed(airspeedADCPin);
if (calibratedAirspeed < 0) //This only occurs when there's a bad ADC reading.
return;
//Get sensor value, just for telemetry purposes.
//This is a silly waste of resources, and should probably be removed at some point in the future.
//At this time, PIOS_MPXV7002_Measure() should become a static function and be removed from the header file.
//
//Moreover, due to the way the ADC driver is currently written, this code will return 0 more often than
//not. This is something that will have to change on the ADC side of things.
baroAirspeedData->SensorValue=PIOS_MPXV5004_Measure(airspeedADCPin);
}
//Filter CAS
float alpha=SAMPLING_DELAY_MS_MPXV/(SAMPLING_DELAY_MS_MPXV + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
float filteredAirspeed = calibratedAirspeed*(alpha) + baroAirspeedData->CalibratedAirspeed*(1.0f-alpha);
//Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one
baroAirspeedData->CalibratedAirspeed = filteredAirspeed;
}
#endif
/**
* @}
* @}
*/

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

@ -38,17 +38,13 @@
#include "openpilot.h"
#include "hwsettings.h"
#include "airspeed.h"
#include "airspeedsettings.h"
#include "baroairspeed.h" // object that will be updated by the module
#include "airspeedsensor.h" // object that will be updated by the module
#if defined(PIOS_INCLUDE_ETASV3)
#define SAMPLING_DELAY_MS_ETASV3 50 //Update at 20Hz
#define ETS_AIRSPEED_SCALE 1.0f //???
#define CALIBRATION_IDLE_MS 2000 //Time to wait before calibrating, in [ms]
#define CALIBRATION_COUNT_MS 2000 //Time to spend calibrating, in [ms]
#define ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS 100.0f //Needs to be settable in a UAVO
// Private types
@ -57,47 +53,43 @@
// Private functions
static uint16_t calibrationCount=0;
static uint16_t calibrationCount2=0;
static uint32_t calibrationSum = 0;
void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings){
static uint32_t calibrationSum = 0;
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
//Wait until our turn. //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE
vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_ETASV3 / portTICK_RATE_MS);
//Check to see if airspeed sensor is returning baroAirspeedData
baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (baroAirspeedData->SensorValue==-1) {
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
baroAirspeedData->CalibratedAirspeed = 0;
BaroAirspeedSet(&baroAirspeedData);
//Check to see if airspeed sensor is returning airspeedSensor
airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (airspeedSensor->SensorValue==-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
return;
}
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) {
calibrationCount++;
return;
} else if (calibrationCount < (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
calibrationCount++;
calibrationSum += baroAirspeedData->SensorValue;
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
//Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS/airspeedSettings->SamplePeriod) {
calibrationCount++;
calibrationCount2++;
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) {
calibrationCount++;
calibrationCount2++;
calibrationSum += airspeedSensor->SensorValue;
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/airspeedSettings->SamplePeriod) {
airspeedSettingsData.ZeroPoint = (int16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f);
AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint );
} else {
airspeedSettings->ZeroPoint = (int16_t) (((float)calibrationSum) / calibrationCount2);
AirspeedSettingsZeroPointSet( &airspeedSettings->ZeroPoint );
}
return;
}
}
//Compute airspeed
float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed?
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
baroAirspeedData->CalibratedAirspeed = calibratedAirspeed;
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
}

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

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

63
flight/Modules/Attitude/revolution/attitude.c
View File

@ -53,6 +53,8 @@
#include "attitude.h"
#include "accels.h"
#include "airspeedsensor.h"
#include "airspeedactual.h"
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "baroaltitude.h"
@ -82,6 +84,11 @@
// reasoning: updates at: 10 Hz, tau= 300 s settle time
// exp(-(1/f) / tau ) ~=~ 0.9997
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// simple IAS to TAS aproximation - 2% increase per 1000ft
// since we do not have flowing air temperature information
#define IAS2TAS(alt) (1.0f + (0.02f*(alt)/304.8f))
// Private types
// Private variables
@ -90,6 +97,7 @@ static xTaskHandle attitudeTaskHandle;
static xQueueHandle gyroQueue;
static xQueueHandle accelQueue;
static xQueueHandle magQueue;
static xQueueHandle airspeedQueue;
static xQueueHandle baroQueue;
static xQueueHandle gpsQueue;
static xQueueHandle gpsVelQueue;
@ -126,6 +134,8 @@ static int32_t getNED(GPSPositionData * gpsPosition, float * NED);
int32_t AttitudeInitialize(void)
{
AttitudeActualInitialize();
AirspeedActualInitialize();
AirspeedSensorInitialize();
AttitudeSettingsInitialize();
PositionActualInitialize();
VelocityActualInitialize();
@ -170,6 +180,7 @@ int32_t AttitudeStart(void)
gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent));
baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
@ -182,6 +193,7 @@ int32_t AttitudeStart(void)
GyrosConnectQueue(gyroQueue);
AccelsConnectQueue(accelQueue);
MagnetometerConnectQueue(magQueue);
AirspeedSensorConnectQueue(airspeedQueue);
BaroAltitudeConnectQueue(baroQueue);
GPSPositionConnectQueue(gpsQueue);
GPSVelocityConnectQueue(gpsVelQueue);
@ -473,6 +485,25 @@ static int32_t updateAttitudeComplementary(bool first_run)
velocityActual.Down = gpsVelocity.Down;
VelocityActualSet(&velocityActual);
}
if ( xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE ) {
// Calculate true airspeed from indicated airspeed
AirspeedSensorData airspeedSensor;
AirspeedSensorGet(&airspeedSensor);
AirspeedActualData airspeed;
AirspeedActualGet(&airspeed);
PositionActualData positionActual;
PositionActualGet(&positionActual);
if (airspeedSensor.SensorConnected==AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
// we have airspeed available
airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - positionActual.Down );
AirspeedActualSet(&airspeed);
}
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
@ -497,6 +528,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
GyrosData gyrosData;
AccelsData accelsData;
MagnetometerData magData;
AirspeedSensorData airspeedData;
BaroAltitudeData baroData;
GPSPositionData gpsData;
GPSVelocityData gpsVelData;
@ -504,6 +536,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
static bool mag_updated = false;
static bool baro_updated;
static bool airspeed_updated;
static bool gps_updated;
static bool gps_vel_updated;
@ -534,6 +567,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if (inited) {
mag_updated = 0;
baro_updated = 0;
airspeed_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
}
@ -544,6 +578,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
mag_updated = 0;
baro_updated = 0;
airspeed_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
@ -554,6 +589,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
@ -562,6 +598,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
AccelsGet(&accelsData);
MagnetometerGet(&magData);
BaroAltitudeGet(&baroData);
AirspeedSensorGet(&airspeedData);
GPSPositionGet(&gpsData);
GPSVelocityGet(&gpsVelData);
GyrosBiasGet(&gyrosBias);
@ -572,6 +609,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// switching between indoor and outdoor mode with Set = false)
mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]);
// Discard airspeed if sensor not connected
airspeed_updated &= ( airspeedData.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE );
// Have a minimum requirement for gps usage
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);
@ -768,7 +808,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
* ( -NED[2] - baroData.Altitude );
} else if (!outdoor_mode) {
INSSetPosVelVar(1e2f, 1e2f);
INSSetPosVelVar(1e6f, 1e5f);
vel[0] = vel[1] = vel[2] = 0;
NED[0] = NED[1] = 0;
NED[2] = -(baroData.Altitude + baroOffset);
@ -782,6 +822,27 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
vel[1] = gpsVelData.East;
vel[2] = gpsVelData.Down;
}
if (airspeed_updated) {
// we have airspeed available
AirspeedActualData airspeed;
AirspeedActualGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = airspeed.CalibratedAirspeed * IAS2TAS( homeLocation.Altitude - Nav.Pos[2] );
AirspeedActualSet(&airspeed);
if ( !gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar(1e6f, 1e2f);
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
Quaternion2R(Nav.q,R);
float vtas[3] = {airspeed.TrueAirspeed,0,0};
rot_mult(R,vtas,vel);
}
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher

5
flight/Modules/FirmwareIAP/firmwareiap.c
View File

@ -108,6 +108,11 @@ int32_t FirmwareIAPInitialize()
return 0;
}
int32_t FirmwareIAPStart()
{
return 0;
}
/*!
* \brief FirmwareIAPCallback - callback function for firmware IAP requests
* \param[in] ev - pointer objevent

2
flight/Modules/FirmwareIAP/inc/firmwareiap.h
View File

@ -27,7 +27,7 @@
#define FIRMWAREIAP_H
int32_t FirmwareIAPInitialize();
int32_t FirmwareIAPStart() {return 0;};
int32_t FirmwareIAPStart();
#endif // FIRMWAREIAP_H

19
flight/Modules/OveroSync/overosync.c
View File

@ -2,13 +2,13 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup TelemetryModule Telemetry Module
* @brief Main telemetry module
* Starts three tasks (RX, TX, and priority TX) that watch event queues
* and handle all the telemetry of the UAVobjects
* @addtogroup Overo Sync Module
* @brief Overo sync module
* Starts a sync tasks that watch event queues
* and push to Overo spi port UAVobjects
* @{
*
* @file telemetry.c
* @file overosync.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Telemetry module, handles telemetry and UAVObject updates
* @see The GNU Public License (GPL) Version 3
@ -37,7 +37,6 @@
#include "systemstats.h"
// Private constants
#define OVEROSYNC_PACKET_SIZE 1024
#define MAX_QUEUE_SIZE 200
#define STACK_SIZE_BYTES 512
#define TASK_PRIORITY (tskIDLE_PRIORITY + 0)
@ -48,8 +47,6 @@
static xQueueHandle queue;
static UAVTalkConnection uavTalkCon;
static xTaskHandle overoSyncTaskHandle;
volatile bool buffer_swap_failed;
volatile uint32_t buffer_swap_timeval;
static bool overoEnabled;
// Private functions
@ -71,7 +68,7 @@ struct overosync {
struct overosync *overosync;
/**
* Initialise the telemetry module
* Initialise the overosync module
* \return -1 if initialisation failed
* \return 0 on success
*/
@ -108,7 +105,7 @@ int32_t OveroSyncInitialize(void)
}
/**
* Initialise the telemetry module
* Initialise the overosync module
* \return -1 if initialisation failed
* \return 0 on success
*/
@ -128,7 +125,7 @@ int32_t OveroSyncStart(void)
// Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject);
// Start telemetry tasks
// Start overosync tasks
xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &overoSyncTaskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle);

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

@ -33,7 +33,7 @@
#include "paths.h"
#include "flightstatus.h"
#include "baroairspeed.h" // TODO: make baroairspeed optional
#include "airspeedactual.h"
#include "pathaction.h"
#include "pathdesired.h"
#include "pathstatus.h"
@ -103,7 +103,7 @@ int32_t PathPlannerInitialize()
PathStatusInitialize();
PathDesiredInitialize();
PositionActualInitialize();
BaroAirspeedInitialize();
AirspeedActualInitialize();
VelocityActualInitialize();
WaypointInitialize();
WaypointActiveInitialize();
@ -438,11 +438,9 @@ static uint8_t conditionAboveSpeed() {
// use airspeed if requested and available
if (pathAction.ConditionParameters[1]>0.5f) {
BaroAirspeedData baroAirspeed;
BaroAirspeedGet (&baroAirspeed);
if (baroAirspeed.BaroConnected == BAROAIRSPEED_BAROCONNECTED_TRUE) {
velocity = baroAirspeed.TrueAirspeed;
}
AirspeedActualData airspeed;
AirspeedActualGet (&airspeed);
velocity = airspeed.CalibratedAirspeed;
}
if ( velocity >= pathAction.ConditionParameters[0]) {

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

@ -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();
}

1
flight/PiOS/Boards/STM32F4xx_RevoMini.h
View File

@ -270,7 +270,6 @@ extern uint32_t pios_packet_handler;
{GPIOC, GPIO_Pin_2, ADC_Channel_12}, \
{NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \
{NULL, 0, ADC_Channel_TempSensor}, /* Temperature sensor */ \
{GPIOC, GPIO_Pin_2, ADC_Channel_12} \
}
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */

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

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

3
flight/PiOS/STM32F10x/pios_i2c.c
View File

@ -933,7 +933,8 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
/* Lock the bus */
portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdTRUE);
if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE)
return -2;
#else
uint32_t timeout = 0xfff;
while(i2c_adapter->busy && --timeout);

6
flight/PiOS/STM32F4xx/pios_i2c.c
View File

@ -970,7 +970,8 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
/* Lock the bus */
portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdTRUE);
if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE)
return -2;
#else
PIOS_IRQ_Disable();
if(i2c_adapter->busy) {
@ -1046,7 +1047,8 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx
/* Lock the bus */
portTickType timeout;
timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
semaphore_success &= (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdTRUE);
if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE)
return -2;
#else
if(i2c_adapter->busy) {
PIOS_IRQ_Enable();

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_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_MPXV_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) */
#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>

7
flight/RevoMini/Makefile
View File

@ -55,9 +55,11 @@ MODULES += ManualControl Stabilization Actuator
MODULES += Battery
MODULES += Altitude/revolution
MODULES += GPS FirmwareIAP
#MODULES += Airspeed/revolution
MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner TxPID
MODULES += Airspeed/revolution
MODULES += AltitudeHold FixedWingPathFollower PathPlanner TxPID
#MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
MODULES += CameraStab
#MODULES += OveroSync
MODULES += Telemetry
PYMODULES =
#FlightPlan
@ -161,6 +163,7 @@ include $(PIOS)/STM32F4xx/library.mk
SRC += $(PIOSCOMMON)/pios_mpu6000.c
SRC += $(PIOSCOMMON)/pios_bma180.c
SRC += $(PIOSCOMMON)/pios_etasv3.c
SRC += $(PIOSCOMMON)/pios_mpxv.c
SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
SRC += $(PIOSCOMMON)/pios_l3gd20.c
SRC += $(PIOSCOMMON)/pios_hmc5883.c

3
flight/RevoMini/Makefile.osx
View File

@ -51,7 +51,8 @@ USE_THUMB_MODE = YES
# List of modules to include
MODULES += Actuator ManualControl Stabilization
MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner
MODULES += AltitudeHold FixedWingPathFollower PathPlanner
#MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
MODULES += Attitude/revolution
#MODULES += OveroSync/simulated

5
flight/RevoMini/System/inc/pios_config.h
View File

@ -67,7 +67,8 @@
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
#define PIOS_INCLUDE_MS5611
//#define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_MPXV
//#define PIOS_INCLUDE_HCSR04
#define FLASH_FREERTOS
/* Com systems to include */
@ -98,6 +99,8 @@
/* Other Interfaces */
//#define PIOS_INCLUDE_I2C_ESC
//#define PIOS_OVERO_SPI
/* Flags that alter behaviors - mostly to lower resources for CC */
#define PIOS_INCLUDE_INITCALL /* Include init call structures */
#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */

2
flight/RevoMini/UAVObjects.inc
View File

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

6
flight/Revolution/Makefile
View File

@ -79,7 +79,8 @@ endif
MODULES = Sensors Attitude/revolution ManualControl Stabilization Actuator
MODULES += Altitude/revolution FirmwareIAP
MODULES += Airspeed/revolution
MODULES += GPS AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner
MODULES += GPS AltitudeHold FixedWingPathFollower PathPlanner
#MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
MODULES += CameraStab
MODULES += OveroSync
MODULES += Telemetry
@ -181,8 +182,7 @@ include $(PIOS)/STM32F4xx/library.mk
SRC += $(PIOSCOMMON)/pios_mpu6000.c
SRC += $(PIOSCOMMON)/pios_bma180.c
SRC += $(PIOSCOMMON)/pios_etasv3.c
SRC += $(PIOSCOMMON)/pios_mpxv5004.c
SRC += $(PIOSCOMMON)/pios_mpxv7002.c
SRC += $(PIOSCOMMON)/pios_mpxv.c
SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
SRC += $(PIOSCOMMON)/pios_l3gd20.c
SRC += $(PIOSCOMMON)/pios_hmc5883.c

3
flight/Revolution/Makefile.osx
View File

@ -57,7 +57,8 @@ USE_THUMB_MODE = YES
# List of modules to include
MODULES += Actuator ManualControl Stabilization
MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner
MODULES += AltitudeHold FixedWingPathFollower PathPlanner
#MODULES += VtolPathFollower ## OP-700: VtolPathFollower disabled because its currently unsafe - remove this line once Sambas code has been merged
MODULES += Attitude/revolution
MODULES += FirmwareIAP
#MODULES += OveroSync/simulated

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

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

2
flight/Revolution/UAVObjects.inc
View File

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

2
flight/SimPosix/UAVObjects.inc
View File

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

127
flight/board_hw_defs/revomini/board_hw_defs.c
View File

@ -104,6 +104,133 @@ const struct pios_led_cfg * PIOS_BOARD_HW_DEFS_GetLedCfg (uint32_t board_revisio
#if defined(PIOS_INCLUDE_SPI)
#include <pios_spi_priv.h>
#if defined(PIOS_OVERO_SPI)
/* SPI2 Interface
* - Used for Flexi/IO/Overo communications
3: PB12 = SPI2 NSS, CAN2 RX
4: PB13 = SPI2 SCK, CAN2 TX, USART3 CTS
5: PB14 = SPI2 MISO, TIM12 CH1, USART3 RTS
6: PB15 = SPI2 MOSI, TIM12 CH2
*/
#include <pios_overo_priv.h>
void PIOS_OVERO_irq_handler(void);
void DMA1_Stream7_IRQHandler(void) __attribute__((alias("PIOS_OVERO_irq_handler")));
static const struct pios_overo_cfg pios_overo_cfg = {
.regs = SPI2,
.remap = GPIO_AF_SPI2,
.init = {
.SPI_Mode = SPI_Mode_Slave,
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
.SPI_DataSize = SPI_DataSize_8b,
.SPI_NSS = SPI_NSS_Hard,
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2,
},
.use_crc = false,
.dma = {
.irq = {
// Note this is the stream ID that triggers interrupts (in this case TX)
.flags = (DMA_IT_TCIF7),
.init = {
.NVIC_IRQChannel = DMA1_Stream7_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.channel = DMA1_Stream0,
.init = {
.DMA_Channel = DMA_Channel_0,
.DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_PeripheralToMemory,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Circular,
.DMA_Priority = DMA_Priority_Medium,
//TODO: Enable FIFO
.DMA_FIFOMode = DMA_FIFOMode_Disable,
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
.tx = {
.channel = DMA1_Stream7,
.init = {
.DMA_Channel = DMA_Channel_0,
.DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR),
.DMA_DIR = DMA_DIR_MemoryToPeripheral,
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
.DMA_MemoryInc = DMA_MemoryInc_Enable,
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
.DMA_Mode = DMA_Mode_Circular,
.DMA_Priority = DMA_Priority_Medium,
.DMA_FIFOMode = DMA_FIFOMode_Disable,
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full,
.DMA_MemoryBurst = DMA_MemoryBurst_Single,
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single,
},
},
},
.sclk = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_13,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.miso = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_14,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.mosi = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_15,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
},
},
.slave_count = 1,
.ssel = { {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_50MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
} },
};
uint32_t pios_overo_id = 0;
void PIOS_OVERO_irq_handler(void)
{
/* Call into the generic code to handle the IRQ for this specific device */
PIOS_OVERO_DMA_irq_handler(pios_overo_id);
}
#endif /* PIOS_OVERO_SPI */
/*
* SPI1 Interface
* Used for MPU6000 gyro and accelerometer

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));
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);

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
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/baroaltitude.h \
$$UAVOBJECT_SYNTHETICS/baroairspeed.h \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
$$UAVOBJECT_SYNTHETICS/airspeedactual.h \
$$UAVOBJECT_SYNTHETICS/attitudeactual.h \
@ -103,7 +103,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/baroaltitude.cpp \
$$UAVOBJECT_SYNTHETICS/baroairspeed.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedactual.cpp \
$$UAVOBJECT_SYNTHETICS/attitudeactual.cpp \

6
shared/uavobjectdefinition/airspeedactual.xml
View File

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

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

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

4
shared/uavobjectdefinition/airspeedsettings.xml
View File

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