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

Attitude: Fixed calculation for baro offset adjustment time

Browse Source
This commit is contained in:
Corvus Corax 2012-05-31 18:21:20 +02:00
parent 8c4dc2be32
commit a786c6e6b9
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -78,7 +78,11 @@
#define F_PI 3.14159265358979323846f #define F_PI 3.14159265358979323846f
#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI) #define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
#define BaroOffsetLowPassTime 300 // low pass filter settle time aprox 5 minutes TODO: make a setting // low pass filter configuration to calculate offset
// of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time
// exp(-(1/f) / tau ) ~=~ 0.9997
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// Private types // Private types
// Private variables // Private variables
@ -582,7 +586,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
float pos[3] = {0.0f, 0.0f, 0.0f}; float pos[3] = {0.0f, 0.0f, 0.0f};
// Initialize barometric offset to homelocation altitude // Initialize barometric offset to homelocation altitude
baroOffset = -home.Altitude; baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset); pos[2] = -(baroData.Altitude + baroOffset);
// Reset the INS algorithm // Reset the INS algorithm
@ -743,10 +747,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
getNED(&gpsData, NED); getNED(&gpsData, NED);
// Track barometric altitude offset with a low pass filter // Track barometric altitude offset with a low pass filter
if (baro_updated) { baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
baroOffset = (1.0f - (dT/BaroOffsetLowPassTime)) * baroOffset + (dT/BaroOffsetLowPassTime) * (1.0f - BARO_OFFSET_LOWPASS_ALPHA )
( -NED[2] - baroData.Altitude ); * ( -NED[2] - baroData.Altitude );
}
// Store this for inspecting offline // Store this for inspecting offline
NEDPositionData nedPos; NEDPositionData nedPos;