From a786c6e6b94269bfb29fb87a5b05d358743b354f Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Thu, 31 May 2012 18:21:20 +0200
Subject: [PATCH] Attitude: Fixed calculation for baro offset adjustment time

---
 flight/Modules/Attitude/revolution/attitude.c | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/flight/Modules/Attitude/revolution/attitude.c b/flight/Modules/Attitude/revolution/attitude.c
index f6d157408..3bebe73c0 100644
--- a/flight/Modules/Attitude/revolution/attitude.c
+++ b/flight/Modules/Attitude/revolution/attitude.c
@@ -78,7 +78,11 @@
 #define F_PI 3.14159265358979323846f
 #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 variables
@@ -582,7 +586,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 			float pos[3] = {0.0f, 0.0f, 0.0f};
 
 			// Initialize barometric offset to homelocation altitude
-			baroOffset = -home.Altitude;
+			baroOffset = -baroData.Altitude;
 			pos[2] = -(baroData.Altitude + baroOffset);
 
 			// Reset the INS algorithm
@@ -743,10 +747,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 		getNED(&gpsData, NED);
 
 		// Track barometric altitude offset with a low pass filter
-		if (baro_updated) {
-			baroOffset = (1.0f - (dT/BaroOffsetLowPassTime)) * baroOffset  + (dT/BaroOffsetLowPassTime) *
-			    ( -NED[2] - baroData.Altitude );
-		}
+		baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
+		    (1.0f - BARO_OFFSET_LOWPASS_ALPHA )
+		    * ( -NED[2] - baroData.Altitude );
 
 		// Store this for inspecting offline
 		NEDPositionData nedPos;