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

Merge branch 'revo-next' of ssh://git.openpilot.org/revo into revo-next

Browse Source
This commit is contained in:
a*morale 2012-11-03 21:36:51 +01:00
commit 75730e7260
10 changed files with 320 additions and 242 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,8 +44,11 @@
#include "gps_airspeed.h" #include "gps_airspeed.h"
#include "baroaltitude.h" #include "baroaltitude.h"
#include "baroairspeed.h" // object that will be updated by the module #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 "attitudeactual.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_analog.h"
// Private constants // Private constants
#if defined (PIOS_INCLUDE_GPS) #if defined (PIOS_INCLUDE_GPS)
@ -164,6 +167,7 @@ int32_t AirspeedInitialize()
#endif #endif
BaroAirspeedInitialize(); BaroAirspeedInitialize();
AirspeedActualInitialize();
AirspeedSettingsInitialize(); AirspeedSettingsInitialize();
AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb);
@ -181,7 +185,9 @@ static void airspeedTask(void *parameters)
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle()); AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
BaroAirspeedData airspeedData; BaroAirspeedData airspeedData;
AirspeedActualData airspeedActualData;
BaroAirspeedGet(&airspeedData); BaroAirspeedGet(&airspeedData);
AirspeedActualGet(&airspeedActualData);
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
@ -205,6 +211,7 @@ static void airspeedTask(void *parameters)
{ {
// Update the airspeed object // Update the airspeed object
BaroAirspeedGet(&airspeedData); BaroAirspeedGet(&airspeedData);
AirspeedActualGet(&airspeedActualData);
#ifdef BARO_AIRSPEED_PRESENT #ifdef BARO_AIRSPEED_PRESENT
float airspeed_tas_baro=0; float airspeed_tas_baro=0;
@ -308,7 +315,10 @@ static void airspeedTask(void *parameters)
#endif #endif
#endif #endif
//Set the UAVO //Set the UAVO
airspeedActualData.TrueAirspeed = airspeedData.TrueAirspeed;
airspeedActualData.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
BaroAirspeedSet(&airspeedData); BaroAirspeedSet(&airspeedData);
AirspeedActualSet(&airspeedActualData);
} }
} }
@ -321,6 +331,32 @@ static void GPSVelocityUpdatedCb(UAVObjEvent * ev)
} }
#endif #endif
void baro_airspeedGet(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
//Find out which sensor we're using.
switch (airspeedSensorType) {
#if defined(PIOS_INCLUDE_MPXV7002)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
#endif
#if defined(PIOS_INCLUDE_MPXV5004)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
#endif
#if defined(PIOS_INCLUDE_MPXV7002) || defined(PIOS_INCLUDE_MPXV5004)
//MPXV5004 and MPXV7002 sensors
baro_airspeedGetAnalog(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
#endif
#if defined(PIOS_INCLUDE_ETASV3)
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_EAGLETREEAIRSPEEDV3:
//Eagletree Airspeed v3
baro_airspeedGetETASV3(baroAirspeedData, lastSysTime, airspeedSensorType, airspeedADCPin);
break;
#endif
default:
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
}
}
static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev) static void AirspeedSettingsUpdatedCb(UAVObjEvent * ev)
{ {

105
flight/Modules/Airspeed/revolution/baro_airspeed.c → flight/Modules/Airspeed/revolution/baro_airspeed_analog.c
View File

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

109
flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c Normal file
View File

@ -0,0 +1,109 @@
/**
******************************************************************************
* @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_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
// Private variables
// Private functions
static uint16_t calibrationCount=0;
void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){
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);
return;
}
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) {
calibrationCount++;
return;
} else if (calibrationCount < (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
calibrationCount++;
calibrationSum += baroAirspeedData->SensorValue;
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) {
airspeedSettingsData.ZeroPoint = (int16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f);
AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint );
} else {
return;
}
}
//Compute airspeed
float calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf((float)abs(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint)); //Is this calibrated or indicated airspeed?
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
baroAirspeedData->CalibratedAirspeed = calibratedAirspeed;
}
#endif
/**
* @}
* @}
*/

43
flight/Modules/Airspeed/revolution/inc/baro_airspeed_analog.h Normal file
View File

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

43
flight/Modules/Airspeed/revolution/inc/baro_airspeed_etasv3.h Normal file
View File

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

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

@ -54,7 +54,7 @@
#include "manualcontrol.h" #include "manualcontrol.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "pathstatus.h" #include "pathstatus.h"
#include "baroairspeed.h" #include "airspeedactual.h"
#include "gpsvelocity.h" #include "gpsvelocity.h"
#include "gpsposition.h" #include "gpsposition.h"
#include "fixedwingpathfollowersettings.h" #include "fixedwingpathfollowersettings.h"
@ -91,7 +91,7 @@ static void SettingsUpdatedCb(UAVObjEvent * ev);
static void updatePathVelocity(); static void updatePathVelocity();
static uint8_t updateFixedDesiredAttitude(); static uint8_t updateFixedDesiredAttitude();
static void updateFixedAttitude(); static void updateFixedAttitude();
static void baroAirspeedUpdatedCb(UAVObjEvent * ev); static void airspeedActualUpdatedCb(UAVObjEvent * ev);
static float bound(float val, float min, float max); static float bound(float val, float min, float max);
/** /**
@ -125,7 +125,7 @@ int32_t FixedWingPathFollowerInitialize()
PathDesiredInitialize(); PathDesiredInitialize();
PathStatusInitialize(); PathStatusInitialize();
VelocityDesiredInitialize(); VelocityDesiredInitialize();
BaroAirspeedInitialize(); AirspeedActualInitialize();
} else { } else {
followerEnabled = false; followerEnabled = false;
} }
@ -144,7 +144,7 @@ static float powerIntegral = 0;
static float airspeedErrorInt=0; static float airspeedErrorInt=0;
// correct speed by measured airspeed // correct speed by measured airspeed
static float baroAirspeedBias = 0; static float indicatedAirspeedActualBias = 0;
/** /**
* Module thread, should not return. * Module thread, should not return.
@ -156,7 +156,7 @@ static void pathfollowerTask(void *parameters)
portTickType lastUpdateTime; portTickType lastUpdateTime;
BaroAirspeedConnectCallback(baroAirspeedUpdatedCb); AirspeedActualConnectCallback(airspeedActualUpdatedCb);
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb);
@ -282,7 +282,6 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_FLYCIRCLELEFT: case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT: case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity; groundspeed = pathDesired.EndingVelocity;
// groundspeed = fixedwingpathfollowerSettings.BestClimbRateSpeed;
altitudeSetpoint = pathDesired.End[2]; altitudeSetpoint = pathDesired.End[2];
break; break;
case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYENDPOINT:
@ -291,16 +290,19 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_DRIVEVECTOR: case PATHDESIRED_MODE_DRIVEVECTOR:
default: default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress,0,1); bound(progress.fractional_progress,0,1);
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1); bound(progress.fractional_progress,0,1);
break; break;
} }
// this ensures a significant forward component at least close to the real trajectory
if (groundspeed<fixedwingpathfollowerSettings.BestClimbRateSpeed/10.)
groundspeed=fixedwingpathfollowerSettings.BestClimbRateSpeed/10.;
// calculate velocity - can be zero if waypoints are too close // calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired; VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * fmaxf(groundspeed,1e-6); velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * fmaxf(groundspeed,1e-6); velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
@ -359,17 +361,13 @@ static uint8_t updateFixedDesiredAttitude()
FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
FixedWingPathFollowerStatusData fixedwingpathfollowerStatus; FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
BaroAirspeedData baroAirspeed; AirspeedActualData airspeedActual;
// float groundspeedActual; float groundspeedActual;
float groundspeedDesired; float groundspeedDesired;
float calibratedAirspeedActual; float indicatedAirspeedActual;
float airspeedDesired; float indicatedAirspeedDesired;
float airspeedError; float airspeedError;
float accelActual;
float accelDesired;
float accelError;
float accelCommand;
float pitchCommand; float pitchCommand;
@ -392,7 +390,7 @@ static uint8_t updateFixedDesiredAttitude()
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
AccelsGet(&accels); AccelsGet(&accels);
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
BaroAirspeedGet(&baroAirspeed); AirspeedActualGet(&airspeedActual);
/** /**
@ -400,17 +398,20 @@ static uint8_t updateFixedDesiredAttitude()
*/ */
// Current ground speed // Current ground speed
// groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
calibratedAirspeedActual = baroAirspeed.CalibratedAirspeed; // note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
// but thanks to accelerometers, groundspeed reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias;
// Desired ground speed // Desired ground speed
groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East); groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
airspeedDesired = bound( groundspeedDesired + baroAirspeedBias, indicatedAirspeedDesired = bound( groundspeedDesired + indicatedAirspeedActualBias,
fixedwingpathfollowerSettings.BestClimbRateSpeed, fixedwingpathfollowerSettings.BestClimbRateSpeed,
fixedwingpathfollowerSettings.CruiseSpeed); fixedwingpathfollowerSettings.CruiseSpeed);
// Airspeed error // Airspeed error
airspeedError = airspeedDesired - calibratedAirspeedActual; airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual;
// Vertical speed error // Vertical speed error
descentspeedDesired = bound ( descentspeedDesired = bound (
@ -421,35 +422,35 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if (groundspeedDesired - baroAirspeedBias <= 0 ) { if (groundspeedDesired - indicatedAirspeedActualBias <= 0 ) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
result = 0; result = 0;
} }
// Error condition: plane too slow or too fast // Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
if ( calibratedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) { if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.AirSpeedMax) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
result = 0; result = 0;
} }
if ( calibratedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) { if ( indicatedAirspeedActual > fixedwingpathfollowerSettings.CruiseSpeed * 1.2f) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
result = 0; result = 0;
} }
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF if (indicatedAirspeedActual < fixedwingpathfollowerSettings.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
result = 0; result = 0;
} }
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0; result = 0;
} }
if (calibratedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) { if (indicatedAirspeedActual < fixedwingpathfollowerSettings.StallSpeedDirty && 1) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0; result = 0;
} }
if (calibratedAirspeedActual<1e-6) { if (indicatedAirspeedActual<1e-6) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops // also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
@ -473,47 +474,14 @@ static uint8_t updateFixedDesiredAttitude()
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI], -fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]
)*(1.0f-1.0f/(1.0f+30.0f/dT)); )*(1.0f-1.0f/(1.0f+30.0f/dT));
} } else powerIntegral = 0;
// Compute final throttle response // Compute final throttle response
powerCommand = (powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] + powerCommand = bound(
powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL]; (powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL],
if (0) { fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN],
//Saturate command, and reduce integral as a way of further avoiding integral windup fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]);
if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) {
if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) {
powerIntegral = bound(
powerIntegral -
( powerCommand
- fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]
)/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
}
powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX];
}
if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) {
if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) {
powerIntegral = bound(
powerIntegral +
( powerCommand
- fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]
)/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]);
}
}
}
//Saturate throttle command.
if ( powerCommand > fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]) {
powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX];
}
if ( powerCommand < fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN]) {
powerCommand = fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN];
}
//Output internal state to telemetry //Output internal state to telemetry
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = powerError; fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = powerError;
@ -552,66 +520,28 @@ static uint8_t updateFixedDesiredAttitude()
/** /**
* Compute desired pitch command * Compute desired pitch command
*/ */
// compute desired acceleration
if(0){
accelDesired = bound( (airspeedError/calibratedAirspeedActual) * fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_KP],
-fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX],
fixedwingpathfollowerSettings.SpeedP[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDP_MAX]);
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = 0.0f;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = accelDesired;
// exclude gravity from acceleration. If the aicraft is flying at high pitch, it fights gravity without getting faster
accelActual = accels.x - (sinf( DEG2RAD * attitudeActual.Pitch) * GEE);
accelError = accelDesired - accelActual;
if (fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI] > 0){
accelIntegral = bound(accelIntegral + accelError * dT,
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
}
accelCommand = (accelError * fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP] + if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0){
accelIntegral*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]); //Integrate with saturation
airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI],
fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]);
}
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = accelError; //Compute the cross feed from vertical speed to pitch, with saturation
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = accelIntegral; float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = accelCommand; -fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
pitchCommand= -accelCommand + bound ( (-descentspeedError/calibratedAirspeedActual) * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP], );
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX] //Compute the pitch command as err*Kp + errInt*Ki + X_feed.
); pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP]
} + airspeedErrorInt*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]
else { ) + verticalSpeedToPitchCommandComponent;
if (fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI] > 0){ fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
//Integrate with saturation fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
airspeedErrorInt=bound(airspeedErrorInt + airspeedError * dT, fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
-fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI],
fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_ILIMIT]/fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]);
}
//Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent=bound (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
);
//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KP]
+ airspeedErrorInt*fixedwingpathfollowerSettings.AccelPI[FIXEDWINGPATHFOLLOWERSETTINGS_ACCELPI_KI]
) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = airspeedDesired;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_ACCEL] = -123;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_ACCEL] = -123;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_ACCEL] = pitchCommand;
}
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] + stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
pitchCommand, pitchCommand,
@ -665,7 +595,7 @@ static uint8_t updateFixedDesiredAttitude()
/** /**
* Compute desired yaw command * Compute desired yaw command
*/ */
// TODO implement raw control mode for yaw and base on Accels.X // TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0; stabDesired.Yaw = 0;
@ -700,21 +630,19 @@ static void SettingsUpdatedCb(UAVObjEvent * ev)
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
} }
static void baroAirspeedUpdatedCb(UAVObjEvent * ev) static void airspeedActualUpdatedCb(UAVObjEvent * ev)
{ {
BaroAirspeedData baroAirspeed; AirspeedActualData airspeedActual;
VelocityActualData velocityActual; VelocityActualData velocityActual;
BaroAirspeedGet(&baroAirspeed); AirspeedActualGet(&airspeedActual);
if (baroAirspeed.BaroConnected != BAROAIRSPEED_BAROCONNECTED_TRUE && BaroAirspeedReadOnly()) { VelocityActualGet(&velocityActual);
baroAirspeedBias = 0; float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
} else {
VelocityActualGet(&velocityActual);
float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
baroAirspeedBias = baroAirspeed.TrueAirspeed - groundspeed; indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed;
} // note - we do fly by Indicated Airspeed (== calibrated airspeed)
// however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement.
} }

1
flight/Revolution/UAVObjects.inc
View File

@ -39,6 +39,7 @@ UAVOBJSRCFILENAMES += magbias
UAVOBJSRCFILENAMES += baroaltitude UAVOBJSRCFILENAMES += baroaltitude
UAVOBJSRCFILENAMES += baroairspeed UAVOBJSRCFILENAMES += baroairspeed
UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedactual
UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += flightbatterysettings
UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += firmwareiapobj
UAVOBJSRCFILENAMES += flightbatterystate UAVOBJSRCFILENAMES += flightbatterystate

2
flight/SimPosix/UAVObjects.inc
View File

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

11
shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
View File

@ -6,9 +6,9 @@
<field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="20"/> <field name="AirSpeedMax" units="m/s" type="float" elements="1" defaultvalue="20"/>
<!-- Vne, i.e. maximum airspeed the airframe can handle --> <!-- Vne, i.e. maximum airspeed the airframe can handle -->
<field name="CruiseSpeed" units="m/s" type="float" elements="1" defaultvalue="20"/> <field name="CruiseSpeed" units="m/s" type="float" elements="1" defaultvalue="20"/>
<!-- Cruise speed the in level flight - leave some safety margin --> <!-- Maximum speed the autopilot will try to achieve, usually for long distances -->
<field name="BestClimbRateSpeed" units="m/s" type="float" elements="1" defaultvalue="10"/> <field name="BestClimbRateSpeed" units="m/s" type="float" elements="1" defaultvalue="10"/>
<!-- V_y, i.e. airspeed at which plane climbs the best --> <!-- V_y, Minimum speed the autopilot will try to fly, for example when loitering -->
<field name="StallSpeedClean" units="m/s" type="float" elements="1" defaultvalue="8"/> <field name="StallSpeedClean" units="m/s" type="float" elements="1" defaultvalue="8"/>
<!-- Vs1, i.e. stall speed in clean configuration- leave some safety margin --> <!-- Vs1, i.e. stall speed in clean configuration- leave some safety margin -->
<field name="StallSpeedDirty" units="m/s" type="float" elements="1" defaultvalue="8"/> <field name="StallSpeedDirty" units="m/s" type="float" elements="1" defaultvalue="8"/>
@ -28,12 +28,9 @@
<field name="BearingPI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/> <field name="BearingPI" units="deg/deg" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="0.2, 0, 0"/>
<!-- coefficients for desired bank angle in relation to ground bearing error - this controls heading --> <!-- coefficients for desired bank angle in relation to ground bearing error - this controls heading -->
<field name="SpeedP" units="(m/s^2) / ((m/s)/(m/s)" type="float" elementnames="Kp,Max" defaultvalue="10,10"/> <field name="SpeedPI" units="deg / (m/s)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, .15, 20"/>
<!-- coefficients for desired acceleration
in relation to relative airspeed error (IASerror/IASactual)-->
<field name="AccelPI" units="deg / (m/s^2)" type="float" elements="3" elementnames="Kp,Ki,ILimit" defaultvalue="1.5, .15, 20"/>
<!-- coefficients for desired pitch <!-- coefficients for desired pitch
in relation to acceleration error --> in relation to relative speed error (IASerror/IASactual) -->
<field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/> <field name="VerticalToPitchCrossFeed" units="deg / ((m/s)/(m/s))" type="float" elementnames="Kp,Max" defaultvalue="5, 10"/>
<!-- coefficients for adjusting desired pitch <!-- coefficients for adjusting desired pitch
in relation to "vertical speed error relative to airspeed" (verror/IASactual) --> in relation to "vertical speed error relative to airspeed" (verror/IASactual) -->

6
shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
View File

@ -1,9 +1,9 @@
<xml> <xml>
<object name="FixedWingPathFollowerStatus" singleinstance="true" settings="false"> <object name="FixedWingPathFollowerStatus" singleinstance="true" settings="false">
<description>Object Storing Debugging Information on PID internals</description> <description>Object Storing Debugging Information on PID internals</description>
<field name="Error" units="" type="float" elementnames="Bearing,Speed,Accel,Power" /> <field name="Error" units="" type="float" elementnames="Bearing,Speed,Power" />
<field name="ErrorInt" units="" type="float" elementnames="Bearing,Speed,Accel,Power" /> <field name="ErrorInt" units="" type="float" elementnames="Bearing,Speed,Power" />
<field name="Command" units="" type="float" elementnames="Bearing,Speed,Accel,Power" /> <field name="Command" units="" type="float" elementnames="Bearing,Speed,Power" />
<field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Pitchcontrol" /> <field name="Errors" units="" type="uint8" elementnames="Wind,Stallspeed,Lowspeed,Highspeed,Overspeed,Lowpower,Highpower,Pitchcontrol" />
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>