From 89e0b48320214e4c88803c6285fad91f0174911d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sun, 23 Feb 2014 14:27:08 +1100
Subject: [PATCH] AP_NavEKF : Bug fix for height drift due to timer wrap-around

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 35 ++++++++++++++++---------------
 libraries/AP_NavEKF/AP_NavEKF.h   |  3 ++-
 2 files changed, 20 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index b15538272e..21559fa885 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -573,15 +573,16 @@ void NavEKF::SelectVelPosFusion()
             ResetVelocity();
             StoreStatesReset();
             }
-        }
-
-        // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same
-        // measurement until the next one arrives
-        if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
+        } else if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
+            // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same
+            // measurement until the next one arrives to provide a smoother output
             fuseVelData = false;
             fusePosData = false;
         }
 
+        // Read height data
+        readHgtData();
+
         // Command fusion of height measurements if new ones available
         if (newDataHgt)
         {
@@ -589,14 +590,12 @@ void NavEKF::SelectVelPosFusion()
             newDataHgt = false;
             // enable fusion
             fuseHgtData = true;
-        }
-
-        // Timeout fusion of height data if stale. Needed because we repeatedly fuse the same
-        // measurement until the next one arrives
-        readHgtData();
-        if (hal.scheduler->millis() > lastHgtUpdate + _msecHgtAvg + 40) {
+        } else if (hal.scheduler->millis() > lastHgtTime_ms + _msecHgtAvg + 40) {
+            // Timeout fusion of height data if stale. Needed because we repeatedly fuse the same
+            // measurement until the next one arrives to provide a smoother output
             fuseHgtData = false;
         }
+
     } else {
         // we only fuse position and height in static mode
         fuseVelData = false;
@@ -2473,11 +2472,14 @@ void NavEKF::readGpsData()
 
 void NavEKF::readHgtData()
 {
-    if (_baro.get_last_update() != lastHgtUpdate) {
-        lastHgtUpdate = _baro.get_last_update();
+    if (_baro.get_last_update() != lastHgtMeasTime) {
+        // time stamp used to check for new measurement
+        lastHgtMeasTime = _baro.get_last_update();
+        // time stamp used to check for timeout
+        lastHgtTime_ms = hal.scheduler->millis();
         hgtMea = _baro.get_altitude();
         newDataHgt = true;
-        // recall states from compass measurement time
+        // recall states at effective measurement time
         RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay));
     } else {
         newDataHgt = false;
@@ -2489,10 +2491,9 @@ void NavEKF::readMagData()
     // scale compass data to improve numerical conditioning
     if (_ahrs->get_compass()->last_update != lastMagUpdate) {
         lastMagUpdate = _ahrs->get_compass()->last_update;
-
+        // Body fixed magnetic bias is opposite sign to APM compass offsets
         magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
         magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
-
         // Recall states from compass measurement time
         RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay));
         newDataMag = true;
@@ -2620,7 +2621,7 @@ void NavEKF::ZeroVariables()
     HGTmsecPrev = 0;
     lastMagUpdate = 0;
     lastAirspeedUpdate = 0;
-    lastHgtUpdate = 0;
+    lastHgtMeasTime = 0;
     dtIMU = 0;
     dt = 0;
     hgtMea = 0;
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index 7f8c15a995..687fa54d64 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -397,7 +397,8 @@ private:
     bool newDataTas;                // true when new airspeed data has arrived
     bool tasDataWaiting;            // true when new airspeed data is waiting to be fused
     bool newDataHgt;                // true when new height data has arrived
-    uint32_t lastHgtUpdate;         // time of last height measurement received (msec)
+    uint32_t lastHgtMeasTime;       // time of last height measurement used to determine if new data has arrived
+    uint32_t lastHgtTime_ms;        // time of last height update (msec) used to calculate timeout
     float hgtVarScaler;             // scaler applied to height measurement variance to allow for oversampling
     uint32_t velFailTime;           // time stamp when GPS velocity measurement last failed covaraiance consistency check (msec)
     uint32_t posFailTime;           // time stamp when GPS position measurement last failed covaraiance consistency check (msec)