diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index ef8fe7de17..a1178c3bcb 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3923,6 +3923,16 @@ Vector2f QuadPlane::landing_desired_closing_velocity() // base target speed based on sqrt of distance float target_speed = safe_sqrt(2*transition_decel*dist); + + // don't let the target speed go above landing approach speed + const float eas2tas = plane.ahrs.get_EAS2TAS(); + float land_speed = plane.aparm.airspeed_cruise_cm * 0.01; + float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed(); + if (is_positive(tecs_land_airspeed)) { + land_speed = tecs_land_airspeed; + } + target_speed = MIN(target_speed, eas2tas * land_speed); + Vector2f target_speed_xy = diff_wp.normalized() * target_speed; return target_speed_xy; @@ -3933,26 +3943,38 @@ Vector2f QuadPlane::landing_desired_closing_velocity() */ float QuadPlane::get_land_airspeed(void) { - if (poscontrol.get_state() == QPOS_APPROACH || + const auto qstate = poscontrol.get_state(); + if (qstate == QPOS_APPROACH || plane.control_mode == &plane.mode_rtl) { - float land_airspeed = plane.TECS_controller.get_land_airspeed(); - if (!is_positive(land_airspeed)) { - land_airspeed = plane.aparm.airspeed_cruise_cm * 0.01; + const float cruise_speed = plane.aparm.airspeed_cruise_cm * 0.01; + float approach_speed = cruise_speed; + float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed(); + if (is_positive(tecs_land_airspeed)) { + approach_speed = tecs_land_airspeed; + } else { + if (qstate == QPOS_APPROACH) { + // default to half way between min airspeed and cruise + // airspeed when on the approach + approach_speed = 0.5*(cruise_speed+plane.aparm.airspeed_min); + } else { + // otherwise cruise + approach_speed = cruise_speed; + } } - float cruise_airspeed = plane.aparm.airspeed_cruise_cm * 0.01; - float time_to_landing = plane.auto_state.wp_distance / MAX(land_airspeed, 5); + const float time_to_landing = plane.auto_state.wp_distance / MAX(approach_speed, 5); /* slow down to landing approach speed as we get closer to landing - */ - land_airspeed = linear_interpolate(land_airspeed, cruise_airspeed, - time_to_landing, - 20, 60); - return land_airspeed; + */ + approach_speed = linear_interpolate(approach_speed, cruise_speed, + time_to_landing, + 20, 60); + return approach_speed; } - Vector2f vel = landing_desired_closing_velocity(); - const float eas2tas = plane.ahrs.get_EAS2TAS(); + // calculate speed based on landing desired velocity + Vector2f vel = landing_desired_closing_velocity(); const Vector3f wind = plane.ahrs.wind_estimate(); + const float eas2tas = plane.ahrs.get_EAS2TAS(); vel.x -= wind.x; vel.y -= wind.y; vel /= eas2tas;