diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a826f39c70..ccfbdfe9da 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2088,8 +2088,20 @@ void QuadPlane::poscontrol_init_approach(void) poscontrol.set_state(QPOS_POSITION1); gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist); } else if (poscontrol.get_state() != QPOS_APPROACH) { - gcs().send_text(MAV_SEVERITY_INFO,"VTOL approach d=%.1f", dist); - poscontrol.set_state(QPOS_APPROACH); + // check if we are close to the destination. We don't want to + // do a full approach when very close + if (dist < transition_threshold()) { + if (tailsitter.enabled() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { + gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist); + poscontrol.set_state(QPOS_POSITION1); + } else { + gcs().send_text(MAV_SEVERITY_INFO,"VTOL short d=%.1f", dist); + poscontrol.set_state(QPOS_AIRBRAKE); + } + } else { + gcs().send_text(MAV_SEVERITY_INFO,"VTOL approach d=%.1f", dist); + poscontrol.set_state(QPOS_APPROACH); + } poscontrol.thrust_loss_start_ms = 0; } } @@ -3442,12 +3454,30 @@ bool QuadPlane::in_transition(void) const /* calculate current stopping distance for a quadplane in fixed wing flight */ -float QuadPlane::stopping_distance(void) +float QuadPlane::stopping_distance(float ground_speed_squared) { // use v^2/(2*accel). This is only quite approximate as the drag // varies with pitch, but it gives something for the user to // control the transition distance in a reasonable way - return plane.ahrs.groundspeed_vector().length_squared() / (2 * transition_decel); + return ground_speed_squared / (2 * transition_decel); +} + +/* + calculate current stopping distance for a quadplane in fixed wing flight + */ +float QuadPlane::stopping_distance(void) +{ + return stopping_distance(plane.ahrs.groundspeed_vector().length_squared()); +} + +/* + distance below which we don't do approach, based on stopping + distance for cruise speed + */ +float QuadPlane::transition_threshold(void) +{ + // 1.5 times stopping distance for cruise speed + return 1.5 * stopping_distance(sq(plane.aparm.airspeed_cruise_cm*0.01)); } #define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index e8aa1053d6..5aef021cd6 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -267,8 +267,13 @@ private: void setup_defaults(void); // calculate a stopping distance for fixed-wing to vtol transitions + float stopping_distance(float ground_speed_squared); float stopping_distance(void); - + + // distance below which we don't do approach, based on stopping + // distance for cruise speed + float transition_threshold(void); + AP_Int16 transition_time_ms; // transition deceleration, m/s/s