From 39d744769572a90fa7d60902358caf83dffb29dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Dec 2021 09:31:48 +1100 Subject: [PATCH] Plane: reset VTOL takeoff if not armed we need to reset the takeoff target position while disarmed so we don't use spurious position information from before we get good GPS lock. also remove the "Resetting previous waypoint" message as it doesn't provide useful information and is just a distraction (it would be printed continuously while waiting for arming with this PR) --- ArduPlane/commands.cpp | 1 - ArduPlane/quadplane.cpp | 5 +++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index dc746ccfd0..6242660f41 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -51,7 +51,6 @@ void Plane::set_next_WP(const struct Location &loc) // location as the previous waypoint, to prevent immediately // considering the waypoint complete if (current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) { - gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint"); prev_WP_loc = current_loc; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c6cb1e56d2..e19140a70f 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3421,9 +3421,10 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) const uint32_t now = millis(); - // reset takeoff start time if we aren't armed, as we won't have made any progress + // reset takeoff if we aren't armed if (!hal.util->get_soft_armed()) { - takeoff_start_time_ms = now; + do_vtol_takeoff(cmd); + return false; } if (now - takeoff_start_time_ms < 3000 &&