Copter: added rangefinder height interpolated using inertial alt

ArduCopter/Copter.h

@@ -261,13 +261,19 @@ class Copter : public AP_Vehicle {
         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
         int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
         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

ArduCopter/mode.cpp

@@ -502,9 +502,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;

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 {

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
  */