From b651eac48d8a425fb1d1d11debeb6e42aada5d00 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 3 Jan 2015 09:12:23 +1100
Subject: [PATCH] AP_NavEKF: Apply timeout to terrain offset validity reporting

The terrain offset solution status is usable for a short period of time without state updates so a timeout has been added which prevents the rapid changes in solution status due to short duration sensor read errors.
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 8 +++++---
 libraries/AP_NavEKF/AP_NavEKF.h   | 2 +-
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 425b7f6624..71e9773371 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -2542,10 +2542,12 @@ void NavEKF::RunAuxiliaryEKF()
     // calculate a predicted LOS rate squared
     float losRateSq = (sq(state.velocity.x) + sq(state.velocity.y)) / sq(flowStates[1] - state.position[2]);
     // don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable
+    // record the time we last updated the terrain offset state
     if ((losRateSq < 0.01f || PV_AidingMode == AID_RELATIVE) && !fuseRngData) {
         inhibitGndState = true;
     } else {
         inhibitGndState = false;
+        gndHgtValidTime_ms = imuSampleTime_ms;
     }
     // Don't update focal length offset if there is no range finder or not using GPS, or we are not flying fast enough to generate a useful LOS rate
     if (!fuseRngData || PV_AidingMode == AID_RELATIVE || losRateSq < 0.01f || (flowStates[1] - state.position[2]) < 3.0f) {
@@ -4092,7 +4094,6 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
         statesAtRngTime = statesAtFlowTime;
         rngMea = rawSonarRange;
         newDataRng = true;
-        rngMeaTime_ms = imuSampleTime_ms;
     } else {
         newDataRng = false;
     }
@@ -4304,7 +4305,7 @@ void NavEKF::InitialiseVariables()
     flowValidMeaTime_ms = imuSampleTime_ms;
     flowMeaTime_ms = 0;
     prevFlowFusionTime_ms = imuSampleTime_ms;
-    rngMeaTime_ms = imuSampleTime_ms;
+    gndHgtValidTime_ms = 0;
     ekfStartTime_ms = imuSampleTime_ms;
 
     // initialise other variables
@@ -4518,13 +4519,14 @@ void  NavEKF::getFilterStatus(uint8_t &status) const
     bool notDeadReckoning = !constVelMode && !constPosMode;
     bool someVertRefData = (!velTimeout && (_fusionModeGPS == 0)) || !hgtTimeout;
     bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
+    bool gndOffsetValid = (imuSampleTime_ms - gndHgtValidTime_ms < 5000);
     status = (!state.quat.is_nan()<<0 | // attitude valid (we need a better check)
               (someHorizRefData && notDeadReckoning)<<1 | // horizontal velocity estimate valid
               someVertRefData<<2 | // vertical velocity estimate valid
               ((doingFlowNav || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning)<<3 | // relative horizontal position estimate valid
               (doingNormalGpsNav && notDeadReckoning)<<4 | // absolute horizontal position estimate valid
               !hgtTimeout<<5 | // vertical position estimate valid
-              !inhibitGndState<<6 | // terrain height estimate valid
+              gndOffsetValid<<6 | // terrain height estimate valid
               constPosMode<<7); // constant position mode
 }
 
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index ede311b8fa..25d5779e76 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -607,7 +607,7 @@ private:
     uint32_t flowValidMeaTime_ms;   // time stamp from latest valid flow measurement (msec)
     uint32_t flowMeaTime_ms;        // time stamp from latest flow measurement (msec)
     uint8_t flowQuality;            // unsigned integer representing quality of optical flow data. 255 is maximum quality.
-    uint32_t rngMeaTime_ms;         // time stamp from latest range measurement (msec)
+    uint32_t gndHgtValidTime_ms;    // time stamp from last terrain offset state update (msec)
     Vector3f omegaAcrossFlowTime;   // body angular rates averaged across the optical flow sample period
     Matrix3f Tnb_flow;              // transformation matrix from nav to body axes at the middle of the optical flow sample period
     Matrix3f Tbn_flow;              // transformation matrix from body to nav axes at the middle of the optical flow sample period