From 4da3236c07770acc3bb981fb1616b023e61a9e4a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 20 Jul 2016 09:09:21 +1000
Subject: [PATCH] Plane: separate out landing height adjustment from barometer
 changes

this stores a landing height adjustment for an aborted landing
without adjusting barometer readings, applying them only on landing
---
 ArduPlane/ArduPlane.cpp |  4 ++--
 ArduPlane/Plane.h       |  4 ++++
 ArduPlane/altitude.cpp  | 22 ++++++++++++++++++++--
 ArduPlane/landing.cpp   |  6 ++++--
 4 files changed, 30 insertions(+), 6 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 8ee9b2a67a..0f45c17cc7 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -485,8 +485,8 @@ void Plane::update_GPS_10Hz(void)
         if (!hal.util->get_soft_armed()) {
             update_home();
 
-            // zero out any baro drift
-            barometer.set_baro_drift_altitude(0);
+            // reset the landing altitude correction
+            auto_state.land_alt_offset = 0;
         }
 
         // update wind estimate
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 5a23ec71aa..7a2cb999d0 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -524,6 +524,9 @@ private:
 
         // are we doing loiter mode as a VTOL?
         bool vtol_loiter:1;
+
+        // landing altitude offset (meters)
+        float land_alt_offset;
     } auto_state;
 
     struct {
@@ -856,6 +859,7 @@ private:
     void setup_terrain_target_alt(Location &loc);
     int32_t adjusted_altitude_cm(void);
     int32_t adjusted_relative_altitude_cm(void);
+    float mission_alt_offset(void);
     float height_above_target(void);
     float lookahead_adjustment(void);
     float rangefinder_correction(void);
diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp
index 718514092c..aaf1e606bb 100644
--- a/ArduPlane/altitude.cpp
+++ b/ArduPlane/altitude.cpp
@@ -249,7 +249,7 @@ int32_t Plane::relative_target_altitude_cm(void)
     }
 #endif
     int32_t relative_alt = target_altitude.amsl_cm - home.alt;
-    relative_alt += int32_t(g.alt_offset)*100;
+    relative_alt += mission_alt_offset()*100;
     relative_alt += rangefinder_correction() * 100;
     return relative_alt;
 }
@@ -451,7 +451,7 @@ void Plane::setup_terrain_target_alt(Location &loc)
  */
 int32_t Plane::adjusted_altitude_cm(void)
 {
-    return current_loc.alt - (g.alt_offset*100);
+    return current_loc.alt - (mission_alt_offset()*100);
 }
 
 /*
@@ -464,6 +464,24 @@ int32_t Plane::adjusted_relative_altitude_cm(void)
     return adjusted_altitude_cm() - home.alt;
 }
 
+
+/*
+  return the mission altitude offset. This raises or lowers all
+  mission items. It is primarily set using the ALT_OFFSET parameter,
+  but can also be adjusted by the rangefinder landing code for a
+  NAV_LAND command if we have aborted a steep landing
+ */
+float Plane::mission_alt_offset(void)
+{
+    float ret = g.alt_offset;
+    if (control_mode == AUTO && auto_state.land_in_progress) {
+        // when landing after an aborted landing due to too high glide
+        // slope we use an offset from the last landing attempt
+        ret += auto_state.land_alt_offset;
+    }
+    return ret;
+}
+
 /*
   return the height in meters above the next_WP_loc altitude
  */
diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp
index 5a01335401..5a5000ee77 100644
--- a/ArduPlane/landing.cpp
+++ b/ArduPlane/landing.cpp
@@ -200,8 +200,10 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void)
 
         // 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);
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
+                                             (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
+            auto_state.land_alt_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
         }