From a6ee46086c5a95cfe8d2b3d2ac0e887a340ec908 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 27 Aug 2014 11:50:07 +1000
Subject: [PATCH] Plane: allow continued use of rangefinder data for 5s after
 loss of contact

this allows short outages to be ridden out
---
 ArduPlane/ArduPlane.pde |  2 ++
 ArduPlane/altitude.pde  | 64 ++++++++++++++++++++++++++++++++++-------
 ArduPlane/sensors.pde   | 29 ++-----------------
 3 files changed, 57 insertions(+), 38 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index d9c0d8c624..955cde1ea6 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -307,6 +307,8 @@ static RangeFinder rangefinder;
 static struct {
     bool in_range;
     float height_estimate;
+    float correction;
+    uint32_t last_correction_time_ms;
     uint8_t in_range_count;
 } rangefinder_state;
 
diff --git a/ArduPlane/altitude.pde b/ArduPlane/altitude.pde
index 76c84da961..e8e3220097 100644
--- a/ArduPlane/altitude.pde
+++ b/ArduPlane/altitude.pde
@@ -513,7 +513,8 @@ static float lookahead_adjustment(void)
  */
 static float rangefinder_correction(void)
 {
-    if (!rangefinder_state.in_range) {
+    if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
+        // we haven't had any rangefinder data for 5s - don't use it
         return 0;
     }
 
@@ -523,20 +524,61 @@ static float rangefinder_correction(void)
                               (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
                                flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL));
     if (!using_rangefinder) {
-        return false;
+        return 0;
     }
 
-    // base correction is the difference between baro altitude and
-    // rangefinder estimate
-    float correction = relative_altitude() - rangefinder_state.height_estimate;
+    return rangefinder_state.correction;
+}
+
+
+/*
+  update the offset between rangefinder height and terrain height
+ */
+static void rangefinder_height_update(void)
+{
+    uint16_t distance_cm = rangefinder.distance_cm();
+    int16_t max_distance_cm = rangefinder.max_distance_cm();
+    if (rangefinder.healthy() && distance_cm < max_distance_cm) {
+        // correct the range for attitude (multiply by DCM.c.z, which
+        // is cos(roll)*cos(pitch))
+        float corrected_range = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
+        if (rangefinder_state.in_range_count == 0) {
+            // we've just come back into range, start with this value
+            rangefinder_state.height_estimate = corrected_range;
+        } else {
+            // low pass filter to reduce noise. This runs at 50Hz, so
+            // converges fast. We don't want too much filtering
+            // though, as it would introduce lag which would delay flaring
+            rangefinder_state.height_estimate = 0.75f * rangefinder_state.height_estimate + 0.25f * corrected_range;
+        }
+        // we consider ourselves to be fully in range when we have 10
+        // good samples (0.2s)
+        if (rangefinder_state.in_range_count < 10) {
+            rangefinder_state.in_range_count++;
+        } else {
+            rangefinder_state.in_range = true;
+        }
+    } else {
+        rangefinder_state.in_range_count = 0;
+        rangefinder_state.in_range = false;
+    }
+
+    if (rangefinder_state.in_range) {
+        // base correction is the difference between baro altitude and
+        // rangefinder estimate
+        float correction = relative_altitude() - rangefinder_state.height_estimate;
 
 #if AP_TERRAIN_AVAILABLE
-    // if we are terrain following then correction is based on terrain data
-    float terrain_altitude;
-    if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true)) {
-        correction = terrain_altitude - rangefinder_state.height_estimate;
-    }
+        // if we are terrain following then correction is based on terrain data
+        float terrain_altitude;
+        if ((target_altitude.terrain_following || g.terrain_follow) && 
+            terrain.height_above_terrain(terrain_altitude, true)) {
+            correction = terrain_altitude - rangefinder_state.height_estimate;
+        }
 #endif    
 
-    return correction;
+        // remember the last correction
+        rangefinder_state.correction = correction;
+        rangefinder_state.last_correction_time_ms = hal.scheduler->millis();    
+    }
 }
diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde
index 9548054f88..75fa56814f 100644
--- a/ArduPlane/sensors.pde
+++ b/ArduPlane/sensors.pde
@@ -20,35 +20,10 @@ static void read_rangefinder(void)
 {
     rangefinder.update();
 
-    uint16_t distance_cm = rangefinder.distance_cm();
-    int16_t max_distance_cm = rangefinder.max_distance_cm();
-    if (rangefinder.healthy() && distance_cm < max_distance_cm) {
-        // correct the range for attitude (multiply by DCM.c.z, which
-        // is cos(roll)*cos(pitch))
-        float corrected_range = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
-        if (rangefinder_state.in_range_count == 0) {
-            // we've just come back into range, start with this value
-            rangefinder_state.height_estimate = corrected_range;
-        } else {
-            // low pass filter to reduce noise. This runs at 50Hz, so
-            // converges fast. We don't want too much filtering
-            // though, as it would introduce lag which would delay flaring
-            rangefinder_state.height_estimate = 0.75f * rangefinder_state.height_estimate + 0.25f * corrected_range;
-        }
-        // we consider ourselves to be fully in range when we have 10
-        // good samples (0.2s)
-        if (rangefinder_state.in_range_count < 10) {
-            rangefinder_state.in_range_count++;
-        } else {
-            rangefinder_state.in_range = true;
-        }
-    } else {
-        rangefinder_state.in_range_count = 0;
-        rangefinder_state.in_range = false;
-    }
-
     if (should_log(MASK_LOG_SONAR))
         Log_Write_Sonar();
+
+    rangefinder_height_update();
 }
 
 /*