From cfc69214e6eab9d4e69c2edb716e3c71ccaddfe5 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir@skeyetech.fr>
Date: Tue, 23 Jan 2018 13:33:24 +0100
Subject: [PATCH] Copter: add LAND_ALT_LOW parameter

---
 ArduCopter/Parameters.cpp | 9 +++++++++
 ArduCopter/Parameters.h   | 3 +++
 ArduCopter/config.h       | 3 ---
 ArduCopter/mode_land.cpp  | 4 ++--
 4 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index 616bdab2ee..4f6624f752 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -947,6 +947,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0),
 
+    // @Param: LAND_ALT_LOW
+    // @DisplayName: Land alt low
+    // @Description: Altitude during Landing at which vehicle slows to LAND_SPEED
+    // @Units: cm
+    // @Range: 100 10000
+    // @Increment: 10
+    // @User: Advanced
+    AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000),
+
     AP_GROUPEND
 };
 
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 58a0ebe130..8434aa4c22 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -556,6 +556,9 @@ public:
 
     // Additional pilot velocity items
     AP_Int16    pilot_speed_dn;
+
+    // Land alt final stage
+    AP_Int16 land_alt_low;
 };
 
 extern const AP_Param::Info        var_info[];
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index ef991e0953..517371a0f5 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -312,9 +312,6 @@
 #ifndef LAND_SPEED
  # define LAND_SPEED    50          // the descent speed for the final stage of landing in cm/s
 #endif
-#ifndef LAND_START_ALT
- # define LAND_START_ALT 1000         // altitude in cm where land controller switches to slow rate of descent
-#endif
 #ifndef LAND_REPOSITION_DEFAULT
  # define LAND_REPOSITION_DEFAULT   1   // by default the pilot can override roll/pitch during landing
 #endif
diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp
index b2a7893ab8..df013d031a 100644
--- a/ArduCopter/mode_land.cpp
+++ b/ArduCopter/mode_land.cpp
@@ -183,8 +183,8 @@ void Copter::land_run_vertical_control(bool pause_descent)
         // Don't speed up for landing.
         max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
 
-        // Compute a vertical velocity demand such that the vehicle approaches LAND_START_ALT. Without the below constraint, this would cause the vehicle to hover at LAND_START_ALT.
-        cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), G_Dt);
+        // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
+        cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), G_Dt);
 
         // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
         cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));