Plane: handle large negative baro offsets (corner case) to self-trigger a land-abort go-around

new param: LAND_ABORT_DEG
@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 higher than the intended slope path. Steeper slopes can result in crashes so this allows the option to remember the baro offset and self-abort the landing and come around for a another landing with the correct baro offset applied for a perfect slope. An auto-abort go-around will only happen once, next attempt will not auto-abort again. This operation happens entirely automatically in AUTO mode. This value is the delta degrees threshold to trigger the go-around. Example: if set to 5 deg and the mission planned slope is 15 deg then if the new slope is 21 then it will go-around. Set to 0 to disable. Requires LAND_SLOPE_RCALC > 0.

ArduPlane/Parameters.cpp

@@ -249,6 +249,15 @@ const AP_Param::Info Plane::var_info[] = {
     // @User: Advanced
     GSCALAR(land_slope_recalc_shallow_threshold,          "LAND_SLOPE_RCALC",  2.0f),
 
+    // @Param: LAND_ABORT_DEG
+    // @DisplayName: Landing auto-abort slope 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 actual altitude is higher than the intended slope path. Normally it would pitch down steeply but that can result in a crash with high airspeed so this allows remembering the baro offset and self-abort the landing and come around for another landing with the correct baro offset applied for a perfect slope. An auto-abort go-around will only happen once, next attempt will not auto-abort again. This operation happens entirely automatically in AUTO mode. This value is the delta degrees threshold to trigger the go-around compared to the origional slope. Example: if set to 5 deg and the mission planned slope is 15 deg then if the new slope is 21 then it will go-around. Set to 0 to disable. Requires LAND_SLOPE_RCALC > 0.
+    // @Range: 0 90
+    // @Units: degrees
+    // @Increment: 0.1
+    // @User: Advanced
+    GSCALAR(land_slope_recalc_steep_threshold_to_abort,          "LAND_ABORT_DEG", 0),
+
     // @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

@@ -301,6 +301,7 @@ public:
         k_param_flight_mode6,
         k_param_initial_mode,
         k_param_land_slope_recalc_shallow_threshold,
+        k_param_land_slope_recalc_steep_threshold_to_abort,
 
         //
         // 220: Waypoint data
@@ -474,6 +475,7 @@ public:
     AP_Float land_pre_flare_alt;
     AP_Float land_pre_flare_sec;
     AP_Float land_slope_recalc_shallow_threshold;
+    AP_Float land_slope_recalc_steep_threshold_to_abort;
     AP_Int32 min_gndspeed_cm;
     AP_Int16 pitch_trim_cd;
     AP_Int16 FBWB_min_altitude_cm;

ArduPlane/Plane.h

@@ -510,6 +510,9 @@ private:
         // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - aparm.land_flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc)
         float land_slope;
 
+        // same as land_slope but sampled once before a rangefinder changes the slope. This should be the original mission planned slope
+        float initial_land_slope;
+
         // barometric altitude at start of takeoff
         float baro_takeoff_alt;
 

ArduPlane/altitude.cpp

@@ -629,6 +629,7 @@ void Plane::rangefinder_height_update(void)
         if (millis() - rangefinder_state.last_correction_time_ms > 5000) {
             rangefinder_state.correction = correction;
             rangefinder_state.initial_correction = correction;
+            auto_state.initial_land_slope = auto_state.land_slope;
         } else {
             rangefinder_state.correction = 0.8f*rangefinder_state.correction + 0.2f*correction;
             if (fabsf(rangefinder_state.correction - rangefinder_state.initial_correction) > 30) {

ArduPlane/landing.cpp

@@ -186,6 +186,28 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void)
 
     GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing glide slope re-calculated as %.1f degrees", (double)new_slope_deg);
 
+    if (rangefinder_state.correction >= 0) { // we're too low or object is below us
+        // correction positive means we're too low so we should continue on with
+        // the newly computed shallower slope instead of pitching/throttling up
+
+    } else if (g.land_slope_recalc_steep_threshold_to_abort > 0) {
+        // correction negative means we're too high and need to point down (and speed up) to re-align
+        // to land on target. A large negative correction means we would have to dive down a lot and will
+        // generating way too much speed that we can not bleed off in time. It is better to remember
+        // the large baro altitude offset and abort the landing to come around again with the correct altitude
+        // offset and "perfect" slope.
+
+        // calculate projected slope with projected alt
+        float initial_slope_deg = degrees(atan(auto_state.initial_land_slope));
+
+        // is projected slope too steep?
+        if (new_slope_deg - initial_slope_deg > g.land_slope_recalc_steep_threshold_to_abort) {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Slope re-calculated too steep, abort landing!");
+            barometer.set_baro_drift_altitude(barometer.get_baro_drift_offset() - rangefinder_state.correction);
+            auto_state.commanded_go_around = 1;
+            g.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once
+        }
+    }
 #endif
 }