Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: improve target airspeed in landing approach 

if the user hasn't set TECS_LAND_ARSPD then we can use an airspeed
between ARSPD_FBW_MIN and TRIM_ARSPD_CM when on approach

this also fixes landing_desired_closing_velocity() to never go above
the landing target speed, so we don't try to speed up if we are
starting the landing sequence too early
This commit is contained in:
Andrew Tridgell 2022-07-30 18:38:59 +10:00 committed by Randy Mackay
parent 0c3276da42
commit 9485a9a62c
1 changed files with 35 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
ArduPlane/quadplane.cpp
View File

@ -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;