Copter: Fix LAND overshoot issue

ArduCopter/control_auto.pde

@@ -323,10 +323,14 @@ static void auto_land_run()
     // run loiter controller
     wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
 
+    float cmb_rate = get_land_descent_speed();
+    
     // call z-axis position controller
-    pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt, true);
+    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
     pos_control.update_z_controller();
 
+    desired_climb_rate = cmb_rate;
+
     // roll & pitch from waypoint controller, yaw rate from pilot
     attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
 }

ArduCopter/control_land.pde

@@ -108,9 +108,11 @@ static void land_gps_run()
         cmb_rate = 0;
     } else {
         land_pause = false;
-        cmb_rate = get_throttle_land();
+        cmb_rate = get_land_descent_speed();
     }
 
+    desired_climb_rate = cmb_rate;
+
     // update altitude target and call position controller
     pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
     pos_control.update_z_controller();
@@ -166,18 +168,20 @@ static void land_nogps_run()
         cmb_rate = 0;
     } else {
         land_pause = false;
-        cmb_rate = get_throttle_land();
+        cmb_rate = get_land_descent_speed();
     }
 
+    desired_climb_rate = cmb_rate;
+
     // call position controller
     pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
     pos_control.update_z_controller();
 }
 
-// get_throttle_land - high level landing logic
+// get_land_descent_speed - high level landing logic
 //      returns climb rate (in cm/s) which should be passed to the position controller
 //      should be called at 100hz or higher
-static float get_throttle_land()
+static float get_land_descent_speed()
 {
 #if CONFIG_SONAR == ENABLED
     bool sonar_ok = sonar_enabled && sonar.healthy();
@@ -185,7 +189,7 @@ static float get_throttle_land()
     bool sonar_ok = false;
 #endif
     // if we are above 10m and the sonar does not sense anything perform regular alt hold descent
-    if (current_loc.alt >= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
+    if (pos_control.get_pos_target().z >= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
         return pos_control.get_speed_down();
     }else{
         return -abs(g.land_speed);

ArduCopter/control_rtl.pde

@@ -376,10 +376,12 @@ static void rtl_land_run()
     wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
 
     // call z-axis position controller
-    float cmb_rate = get_throttle_land();
+    float cmb_rate = get_land_descent_speed();
     pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
     pos_control.update_z_controller();
 
+    desired_climb_rate = cmb_rate;
+
     // roll & pitch from waypoint controller, yaw rate from pilot
     attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);