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

Fix filtering rate for case where we have a baro sensor and a gps, but

Browse Source 
have no current gps data.

Also fixed minor timing bug that allowed the GPS to be used too often
when our primary instrument is the baro airspeed sensor.
This commit is contained in:
Kenz Dale 2012-07-24 15:16:52 +02:00
parent e2887ca7fe
commit 8a5f4d40c5
1 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -74,8 +74,8 @@
#define GPS_AIRSPEED_BIAS_KP 0.01f //Needs to be settable in a UAVO #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_BIAS_KI 0.01f //Needs to be settable in a UAVO
//#define SAMPLING_DELAY_MS_GPS 100 //Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO #define 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 F_PI 3.141526535897932f
#define DEG2RAD (F_PI/180.0f) #define DEG2RAD (F_PI/180.0f)
@ -187,7 +187,8 @@ static void airspeedTask(void *parameters)
airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE; airspeedData.BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
#ifdef BARO_AIRSPEED_PRESENT #ifdef BARO_AIRSPEED_PRESENT
portTickType lastGPSTime= xTaskGetTickCount(); portTickType lastGPSTime = xTaskGetTickCount(); //Time since last GPS-derived airspeed calculation
portTickType lastLoopTime= xTaskGetTickCount(); //Time since last loop
float airspeedErrInt=0; float airspeedErrInt=0;
#endif #endif
@ -243,8 +244,9 @@ static void airspeedTask(void *parameters)
//sensor or not. In the case we do, shoot for about once per second. Otherwise, consume GPS //sensor or not. In the case we do, shoot for about once per second. Otherwise, consume GPS
//as quickly as possible. //as quickly as possible.
#ifdef BARO_AIRSPEED_PRESENT #ifdef BARO_AIRSPEED_PRESENT
float delT = (lastSysTime - lastGPSTime)/1000.0f; float delT = (lastSysTime - lastLoopTime)/(portTICK_RATE_MS*1000.0f);
if ( (delT > portTICK_RATE_MS || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY) lastLoopTime=lastSysTime;
if ( ((lastSysTime - lastGPSTime) > 1000*portTICK_RATE_MS || airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GPSONLY)
&& gpsNew) { && gpsNew) {
lastGPSTime=lastSysTime; lastGPSTime=lastSysTime;
#else #else
@ -299,9 +301,9 @@ static void airspeedTask(void *parameters)
} }
#ifdef BARO_AIRSPEED_PRESENT #ifdef BARO_AIRSPEED_PRESENT
else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){ else if (airspeedData.BaroConnected==BAROAIRSPEED_BAROCONNECTED_TRUE){
//No GPS velocity estimate this loop, so filter data with baro airspeed //No GPS velocity estimate this loop, so filter true airspeed data with baro airspeed
float alpha=gpsSamplePeriod_ms/(gpsSamplePeriod_ms + GPS_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter. float alpha=delT/(delT + BARO_TRUEAIRSPEED_TIME_CONSTANT_S); //Low pass filter.
airspeedData.TrueAirspeed=airspeedData.TrueAirspeed*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha); airspeedData.TrueAirspeed=airspeed_tas_baro*(alpha) + airspeedData.TrueAirspeed*(1.0f-alpha);
} }
#endif #endif
#endif #endif