Plane: display VTOL pos1 state change when approach disabled

This commit is contained in:
Andrew Tridgell 2021-11-23 09:06:30 +11:00 committed by Randy Mackay
parent b6d852ac13
commit 38a5feefb6
1 changed files with 2 additions and 1 deletions

View File

@ -2586,13 +2586,14 @@ void QuadPlane::run_xy_controller(void)
*/
void QuadPlane::poscontrol_init_approach(void)
{
const float dist = plane.current_loc.get_distance(plane.next_WP_loc);
if ((options & OPTION_DISABLE_APPROACH) != 0) {
// go straight to QPOS_POSITION1
poscontrol.set_state(QPOS_POSITION1);
gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist);
} else if (poscontrol.get_state() != QPOS_APPROACH) {
// check if we are close to the destination. We don't want to
// do a full approach when very close
const float dist = plane.current_loc.get_distance(plane.next_WP_loc);
if (dist < transition_threshold()) {
if (is_tailsitter() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f", dist);