From 5c43b1867258a0619023dca13d82c924f1602371 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 11 Sep 2019 17:20:04 +1000
Subject: [PATCH] Plane: fixed height_above_ground() for case when rangefinder
 is below min

this prevents a rangefinder that goes below min distance from causing
the calculations that depend on height above ground to fail
---
 ArduPlane/altitude.cpp  |  9 ++++++++-
 ArduPlane/quadplane.cpp | 10 +++++++++-
 ArduPlane/quadplane.h   |  5 +++++
 3 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp
index 69c8358b91..7b202b1265 100644
--- a/ArduPlane/altitude.cpp
+++ b/ArduPlane/altitude.cpp
@@ -126,7 +126,14 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available)
 {
    if (use_rangefinder_if_available && rangefinder_state.in_range) {
         return rangefinder_state.height_estimate;
-    }
+   }
+
+   if (use_rangefinder_if_available && quadplane.in_vtol_land_final() &&
+       rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_OutOfRangeLow) {
+       // a special case for quadplane landing when rangefinder goes
+       // below minimum. Consider our height above ground to be zero
+       return 0;
+   }
 
 #if AP_TERRAIN_AVAILABLE
     float altitude;
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index e7fd2ca467..b961cf82f0 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2795,7 +2795,7 @@ int8_t QuadPlane::forward_throttle_pct(void)
         // lidar could cause the aircraft not to be able to
         // approach the landing point when landing below the takeoff point
         vel_forward.last_pct = vel_forward.integrator;
-    } else if (in_vtol_land_descent() && poscontrol.state == QPOS_LAND_FINAL && motors->limit.throttle_lower) {
+    } else if (in_vtol_land_final() && motors->limit.throttle_lower) {
         // we're in the settling phase of landing, disable fwd motor
         vel_forward.last_pct = 0;
         vel_forward.integrator = 0;
@@ -3109,3 +3109,11 @@ bool QuadPlane::in_vtol_land_descent(void) const
     }
     return false;
 }
+
+/*
+  see if we are in the final phase of a VTOL landing
+ */
+bool QuadPlane::in_vtol_land_final(void) const
+{
+    return in_vtol_land_descent() && poscontrol.state == QPOS_LAND_FINAL;
+}
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 464c2a54b9..48d11957c9 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -540,6 +540,11 @@ private:
       are we in the descent phase of a VTOL landing?
      */
     bool in_vtol_land_descent(void) const;
+
+    /*
+      are we in the final landing phase of a VTOL landing?
+     */
+    bool in_vtol_land_final(void) const;
     
 public:
     void motor_test_output();