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

added low pass filter and a few safety checks to gps airspeed estimation

Browse Source
This commit is contained in:
Corvus Corax 2013-07-20 09:09:44 +02:00
parent 02f11eeb5b
commit 1049f483a5
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/modules/Airspeed/gps_airspeed.c
View File

@ -52,6 +52,7 @@ struct GPSGlobals {
float gpsVelOld_N;
float gpsVelOld_E;
float gpsVelOld_D;
float oldAirspeed;
};
// Private variables
@ -76,6 +77,8 @@ void gps_airspeedInitialize()
gps->gpsVelOld_E = gpsVelData.East;
gps->gpsVelOld_D = gpsVelData.Down;
gps->oldAirspeed = 0.0f;
AttitudeStateData attData;
AttitudeStateGet(&attData);
@ -121,6 +124,12 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, __attribute__((unused)) A
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
return; // do not calculate if gps velocity is insufficient...
}
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);
@ -128,12 +137,15 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, __attribute__((unused)) A
float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f);
// Airspeed magnitude is the ratio between the two difference norms
airspeedData->CalibratedAirspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
} else {
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
// need a low pass filter to filter out spikes in non coordinated maneuvers
airspeedData->CalibratedAirspeed = 0.99f * gps->oldAirspeed + 0.01f * airspeed;
gps->oldAirspeed = airspeedData->CalibratedAirspeed;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
}
// Save old variables for next pass