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)
This commit is contained in:
Andrew Tridgell 2021-12-04 07:14:04 +11:00
parent 7ab343dd66
commit e7c7cdd653
2 changed files with 3 additions and 3 deletions

View File

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

View File

@ -2823,9 +2823,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 &&