diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0d0f9bfad4..85113beabb 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2614,6 +2614,10 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) } state = s; last_state_change_ms = AP_HAL::millis(); + + // we consider setting the state to be equivalent to running to + // prevent code from overriding the state as stale + last_run_ms = last_state_change_ms; } /* @@ -2636,6 +2640,10 @@ void QuadPlane::vtol_position_controller(void) check_attitude_relax(); + if (hal.util->get_soft_armed()) { + poscontrol.last_run_ms = now_ms; + } + // horizontal position control switch (poscontrol.get_state()) { @@ -3207,8 +3215,14 @@ void QuadPlane::control_auto(void) case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_LOITER_TO_ALT: + case MAV_CMD_NAV_LOITER_TO_ALT: { + const uint32_t now = AP_HAL::millis(); + if (now - poscontrol.last_run_ms > 100) { + // ensure that poscontrol is reset + poscontrol.set_state(QPOS_POSITION1); + } vtol_position_controller(); + } break; default: waypoint_controller(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 24d2567d11..ef5bf6be4e 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -486,6 +486,7 @@ private: uint32_t thrust_loss_start_ms; uint32_t last_log_ms; bool reached_wp_speed; + uint32_t last_run_ms; private: uint32_t last_state_change_ms; enum position_control_state state;