diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 7dcaee83b9..c275c66aae 100644 --- a/ArduCopter/control_auto.pde +++ b/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); } diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index ba4cb0a5d5..0ea684f681 100644 --- a/ArduCopter/control_land.pde +++ b/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); diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index fc99ba6663..bb626e4a4b 100644 --- a/ArduCopter/control_rtl.pde +++ b/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);