From 186806c76849acf748d0129821a7acbd4380f5ac Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 27 Aug 2014 18:25:17 +1000
Subject: [PATCH] Plane: lowpass the rangefinder correction, not height

this should produce less lag as the rangefinder correction should be
changing much less
---
 ArduPlane/ArduPlane.pde |  1 -
 ArduPlane/Log.pde       |  6 +++---
 ArduPlane/altitude.pde  | 28 +++++++++++++---------------
 3 files changed, 16 insertions(+), 19 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 955cde1ea6..01e4e1357e 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -306,7 +306,6 @@ static RangeFinder rangefinder;
 
 static struct {
     bool in_range;
-    float height_estimate;
     float correction;
     uint32_t last_correction_time_ms;
     uint8_t in_range_count;
diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index 387d2ae009..0c71efff39 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -354,7 +354,7 @@ struct PACKED log_Sonar {
     float groundspeed;
     uint8_t throttle;
     uint8_t count;
-    float height_estimate;
+    float correction;
 };
 
 // Write a sonar packet
@@ -369,7 +369,7 @@ static void Log_Write_Sonar()
         groundspeed : gps.ground_speed(),
         throttle    : (uint8_t)(100 * channel_throttle->norm_output()),
         count       : rangefinder_state.in_range_count,
-        height_estimate : rangefinder_state.height_estimate
+        correction  : rangefinder_state.correction
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -540,7 +540,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),         
       "NTUN", "ICIccccfI",   "TimeMS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
     { LOG_SONAR_MSG, sizeof(log_Sonar),             
-      "SONR", "IffffBBf",   "TimeMS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,HEst" },
+      "SONR", "IffffBBf",   "TimeMS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" },
     { LOG_MODE_MSG, sizeof(log_Mode),             
       "MODE", "IMB",         "TimeMS,Mode,ModeNum" },
     { LOG_CURRENT_MSG, sizeof(log_Current),             
diff --git a/ArduPlane/altitude.pde b/ArduPlane/altitude.pde
index e125486d0c..3641fa3530 100644
--- a/ArduPlane/altitude.pde
+++ b/ArduPlane/altitude.pde
@@ -526,19 +526,12 @@ 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) {
+    float height_estimate = 0;
+    if (rangefinder.healthy() && distance_cm < max_distance_cm && home_is_set) {
         // 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;
-        }
+        height_estimate = distance_cm * 0.01f * ahrs.get_dcm_matrix().c.z;
+
         // we consider ourselves to be fully in range when we have 10
         // good samples (0.2s)
         if (rangefinder_state.in_range_count < 10) {
@@ -554,19 +547,24 @@ static void rangefinder_height_update(void)
     if (rangefinder_state.in_range) {
         // base correction is the difference between baro altitude and
         // rangefinder estimate
-        float correction = relative_altitude() - rangefinder_state.height_estimate;
+        float correction = relative_altitude() - height_estimate;
 
 #if AP_TERRAIN_AVAILABLE
         // 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;
+            correction = terrain_altitude - height_estimate;
         }
 #endif    
 
-        // remember the last correction
-        rangefinder_state.correction = correction;
+        // remember the last correction. Use a low pass filter unless
+        // the old data is more than 5 seconds old
+        if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
+            rangefinder_state.correction = correction;
+        } else {
+            rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
+        }
         rangefinder_state.last_correction_time_ms = hal.scheduler->millis();    
     }
 }