Plane: re-calc landing glide slope to gracefully handle baro offset during long flights

New param: LAND_SLOPE_RCALC
@Description: This parameter is used when using a rangefinder during landing for altitude correction from baro drift (RNGFND_LANDING=1) and the altitude correction indicates your altitude is lower than the intended slope path. This value is the threshold of the correction to re-calculate the landing approach slope. Set to zero to keep the original slope all the way down and any detected baro drift will be corrected by pitching/throttling up to snap back to resume the original slope path. Otherwise, when a rangefinder altitude correction exceeds this threshold it will trigger a slope re-calculate to give a shallower slope. This also smoothes out the approach when flying over objects such as trees. Recommend a value of 2m.

default value is 2 (so, enabled by default)

ArduPlane/Parameters.cpp

@@ -240,6 +240,15 @@ const AP_Param::Info Plane::var_info[] = {
     // @User: User
     GSCALAR(level_roll_limit,              "LEVEL_ROLL_LIMIT",   5),
 
+    // @Param: LAND_SLOPE_RCALC
+    // @DisplayName: Landing slope re-calc threshold
+    // @Description: This parameter is used when using a rangefinder during landing for altitude correction from baro drift (RNGFND_LANDING=1) and the altitude correction indicates your altitude is lower than the intended slope path. This value is the threshold of the correction to re-calculate the landing approach slope. Set to zero to keep the original slope all the way down and any detected baro drift will be corrected by pitching/throttling up to snap back to resume the original slope path. Otherwise, when a rangefinder altitude correction exceeds this threshold it will trigger a slope re-calculate to give a shallower slope. This also smoothes out the approach when flying over objects such as trees. Recommend a value of 2m.
+    // @Range: 0 5
+    // @Units: meters
+    // @Increment: 0.5
+    // @User: Advanced
+    GSCALAR(land_slope_recalc_shallow_threshold,          "LAND_SLOPE_RCALC",  2.0f),
+
     // @Param: LAND_PITCH_CD
     // @DisplayName: Landing Pitch
     // @Description: Used in autoland to give the minimum pitch in the final stage of landing (after the flare). This parameter can be used to ensure that the final landing attitude is appropriate for the type of undercarriage on the aircraft. Note that it is a minimum pitch only - the landing code will control pitch above this value to try to achieve the configured landing sink rate.

ArduPlane/Parameters.h

@@ -300,6 +300,7 @@ public:
         k_param_flight_mode5,
         k_param_flight_mode6,
         k_param_initial_mode,
+        k_param_land_slope_recalc_shallow_threshold,
 
         //
         // 220: Waypoint data
@@ -472,6 +473,7 @@ public:
     AP_Int8 land_abort_throttle_enable;
     AP_Float land_pre_flare_alt;
     AP_Float land_pre_flare_sec;
+    AP_Float land_slope_recalc_shallow_threshold;
     AP_Int32 min_gndspeed_cm;
     AP_Int16 pitch_trim_cd;
     AP_Int16 FBWB_min_altitude_cm;

ArduPlane/Plane.h

@@ -198,6 +198,7 @@ private:
         float initial_range;
         float correction;
         float initial_correction;
+        float last_stable_correction;
         uint32_t last_correction_time_ms;
         uint8_t in_range_count;
         float height_estimate;
@@ -889,6 +890,7 @@ private:
     bool verify_land();
     void disarm_if_autoland_complete();
     void setup_landing_glide_slope(void);
+    void adjust_landing_slope_for_rangefinder_bump(void);
     bool jump_to_landing_sequence(void);
     float tecs_hgt_afe(void);
     void set_nav_controller(void);

ArduPlane/altitude.cpp

@@ -37,6 +37,7 @@ void Plane::adjust_altitude_target()
     } else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
             flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE) {
         setup_landing_glide_slope();
+        adjust_landing_slope_for_rangefinder_bump();
     } else if (reached_loiter_target()) {
         // once we reach a loiter target then lock to the final
         // altitude target

ArduPlane/landing.cpp

@@ -160,7 +160,31 @@ void Plane::disarm_if_autoland_complete()
     }
 }
 
+void Plane::adjust_landing_slope_for_rangefinder_bump(void)
+{
+#if RANGEFINDER_ENABLED == ENABLED
+    // check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by
+    // determining the slope from your current location to the land point then following that back up to the approach
+    // altitude and moving the prev_wp to that location. From there
+    float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction);
+
+    if (g.land_slope_recalc_shallow_threshold <= 0 ||
+            fabsf(correction_delta) < g.land_slope_recalc_shallow_threshold) {
+        return;
+    }
+
+    rangefinder_state.last_stable_correction = rangefinder_state.correction;
 
+    float corrected_alt_m = (adjusted_altitude_cm() - next_WP_loc.alt)*0.01f - rangefinder_state.correction;
+    float total_distance_m = get_distance(prev_WP_loc, next_WP_loc);
+    float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / auto_state.wp_distance;
+    prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt;
+
+    // re-calculate with updated prev_WP_loc
+    setup_landing_glide_slope();
+    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Slope re-calculated");
+#endif
+}
 
 /*
   a special glide slope calculation for the landing approach