From 5634f191143fd04fee655104adbd84dd8419b282 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 6 Sep 2017 08:22:38 +1000
Subject: [PATCH] Plane: adjust target altitude slowly in QRTL mode

this fixes a sudden throttle drop when starting the descent in QRTL mode
thanks to Marco for finding this issue
---
 ArduPlane/quadplane.cpp | 37 +++++++++++++++++++++++++++----------
 ArduPlane/quadplane.h   |  6 ++++++
 2 files changed, 33 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index d147d4aeda..742e7ebbe0 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -748,7 +748,7 @@ void QuadPlane::run_z_controller(void)
         // set alt target to current height on transition. This
         // starts the Z controller off with the right values
         gcs().send_text(MAV_SEVERITY_INFO, "Reset alt target to %.1f", (double)inertial_nav.get_altitude());
-        pos_control->set_alt_target(inertial_nav.get_altitude());
+        set_alt_target_current();
         pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
 
         // initialize vertical speeds and leash lengths
@@ -776,7 +776,7 @@ void QuadPlane::init_hover(void)
     pos_control->set_accel_z(pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control->set_alt_target(inertial_nav.get_altitude());
+    set_alt_target_current();
     pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     init_throttle_wait();
@@ -840,7 +840,7 @@ void QuadPlane::init_loiter(void)
     pos_control->set_accel_z(pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control->set_alt_target(inertial_nav.get_altitude());
+    set_alt_target_current();
     pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
 
     init_throttle_wait();
@@ -1793,7 +1793,7 @@ void QuadPlane::vtol_position_controller(void)
                                                      plane.auto_state.wp_proportion,
                                                      0, 1);
             }
-            pos_control->set_alt_target(target_altitude - plane.home.alt);
+            adjust_alt_target(target_altitude - plane.home.alt);
         } else {
             pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
         }
@@ -1941,8 +1941,6 @@ void QuadPlane::control_qrtl(void)
         // change target altitude to home alt
         plane.next_WP_loc.alt = plane.home.alt;
         verify_vtol_land();
-    } else {
-        pos_control->set_alt_target(qrtl_alt*100UL);
     }
 }
 
@@ -1980,7 +1978,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
     pos_control->set_accel_z(pilot_accel_z);
 
     // initialise position and desired velocity
-    pos_control->set_alt_target(inertial_nav.get_altitude());
+    set_alt_target_current();
     pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     
     // also update nav_controller for status output
@@ -2019,7 +2017,7 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
     target.x = diff2d.x * 100;
     target.y = diff2d.y * 100;
     target.z = plane.next_WP_loc.alt - origin.alt;
-    pos_control->set_alt_target(inertial_nav.get_altitude());
+    set_alt_target_current();
     
     // also update nav_controller for status output
     plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
@@ -2039,7 +2037,7 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
     }
     transition_state = TRANSITION_AIRSPEED_WAIT;
     plane.TECS_controller.set_pitch_max_limit(transition_pitch_max);
-    pos_control->set_alt_target(inertial_nav.get_altitude());
+    set_alt_target_current();
 
     plane.complete_auto_takeoff();
     
@@ -2114,7 +2112,7 @@ bool QuadPlane::verify_vtol_land(void)
     float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
     if (poscontrol.state == QPOS_LAND_DESCEND && height_above_ground < land_final_alt) {
         poscontrol.state = QPOS_LAND_FINAL;
-        pos_control->set_alt_target(inertial_nav.get_altitude());
+        set_alt_target_current();
 
         // cut IC engine if enabled
         if (land_icengine_cut != 0) {
@@ -2324,3 +2322,22 @@ bool QuadPlane::guided_mode_enabled(void)
     }
     return guided_mode != 0;
 }
+
+/*
+  set altitude target to current altitude
+ */
+void QuadPlane::set_alt_target_current(void)
+{
+    pos_control->set_alt_target(inertial_nav.get_altitude());
+}
+
+/*
+  adjust the altitude target to the given target, moving it slowly
+ */
+void QuadPlane::adjust_alt_target(float altitude_cm)
+{
+    float current_alt = inertial_nav.get_altitude();
+    // don't let it get beyond 50cm from current altitude
+    float target_cm = constrain_float(altitude_cm, current_alt-50, current_alt+50);
+    pos_control->set_alt_target(target_cm);
+}
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 3ca4166bf4..23592457b5 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -399,6 +399,12 @@ private:
 
     void afs_terminate(void);
     bool guided_mode_enabled(void);
+
+    // set altitude target to current altitude
+    void set_alt_target_current(void);
+    
+    // adjust altitude target smoothly
+    void adjust_alt_target(float target_cm);
     
 public:
     void motor_test_output();