From dd5fafe30e7f52dc175cbad44709dbad8012f06d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 19 Feb 2020 09:22:31 +1100
Subject: [PATCH] Copter: added rangefinder height interpolated using inertial
 alt

this smooths rangefinder heights and allows for good estimated for
precision landing even with loss of some rangefinder samples during landing
---
 ArduCopter/Copter.h     |  6 ++++++
 ArduCopter/mode.cpp     |  4 +---
 ArduCopter/mode_rtl.cpp |  4 +---
 ArduCopter/sensors.cpp  | 21 +++++++++++++++++++++
 4 files changed, 29 insertions(+), 6 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index af34398640..1b265d6c0d 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -261,6 +261,7 @@ private:
         bool enabled:1;
         bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
         int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
+        float inertial_alt_cm; // inertial alt at time of last rangefinder sample
         uint32_t last_healthy_ms;
         LowPassFilterFloat alt_cm_filt; // altitude filter
         int16_t alt_cm_glitch_protected;    // last glitch protected altitude
@@ -268,6 +269,11 @@ private:
         uint32_t glitch_cleared_ms; // system time glitch cleared
     } rangefinder_state, rangefinder_up_state;
 
+    /*
+      return rangefinder height interpolated using inertial altitude
+     */
+    bool get_rangefinder_height_interpolated_cm(int32_t& ret);
+
     class SurfaceTracking {
     public:
         // get desired climb rate (in cm/s) to achieve surface tracking
diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index 93899e43cf..0c2c2ee5f1 100644
--- a/ArduCopter/mode.cpp
+++ b/ArduCopter/mode.cpp
@@ -509,9 +509,7 @@ void Mode::make_safe_spool_down()
 int32_t Mode::get_alt_above_ground_cm(void)
 {
     int32_t alt_above_ground;
-    if (copter.rangefinder_alt_ok()) {
-        alt_above_ground = copter.rangefinder_state.alt_cm_filt.get();
-    } else {
+    if (!copter.get_rangefinder_height_interpolated_cm(alt_above_ground)) {
         bool navigating = pos_control->is_active_xy();
         if (!navigating || !copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground)) {
             alt_above_ground = copter.current_loc.alt;
diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp
index c631fbc803..21c34dae00 100644
--- a/ArduCopter/mode_rtl.cpp
+++ b/ArduCopter/mode_rtl.cpp
@@ -461,9 +461,7 @@ void ModeRTL::compute_return_target()
 
     // set curr_alt and return_target.alt from range finder
     if (alt_type == ReturnTargetAltType::RETURN_TARGET_ALTTYPE_RANGEFINDER) {
-        if (copter.rangefinder_state.alt_healthy) {
-            // set curr_alt based on rangefinder altitude
-            curr_alt = copter.rangefinder_state.alt_cm_filt.get();
+        if (copter.get_rangefinder_height_interpolated_cm(curr_alt)) {
             // set return_target.alt
             rtl_path.return_target.set_alt_cm(MAX(curr_alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)), Location::AltFrame::ABOVE_TERRAIN);
         } else {
diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp
index 1d6369ae0f..aff1b30758 100644
--- a/ArduCopter/sensors.cpp
+++ b/ArduCopter/sensors.cpp
@@ -54,6 +54,9 @@ void Copter::read_rangefinder(void)
         // tilt corrected but unfiltered, not glitch protected alt
         rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient);
 
+        // remember inertial alt to allow us to interpolate rangefinder
+        rf_state.inertial_alt_cm = inertial_nav.get_altitude();
+
         // glitch handling.  rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
         // are considered a glitch and glitch_count becomes non-zero
         // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
@@ -120,6 +123,24 @@ bool Copter::rangefinder_up_ok()
     return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy);
 }
 
+/*
+  get inertially interpolated rangefinder height. Inertial height is
+  recorded whenever we update the rangefinder height, then we use the
+  difference between the inertial height at that time and the current
+  inertial height to give us interpolation of height from rangefinder
+ */
+bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret)
+{
+    if (!rangefinder_alt_ok()) {
+        return false;
+    }
+    ret = rangefinder_state.alt_cm_filt.get();
+    float inertial_alt_cm = inertial_nav.get_altitude();
+    ret += inertial_alt_cm - rangefinder_state.inertial_alt_cm;
+    return true;
+}
+
+
 /*
   update RPM sensors
  */