diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3e8fd6ca54..b48a4316c8 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2162,6 +2162,7 @@ void QuadPlane::log_QPOS(void) */ void QuadPlane::PosControlState::set_state(enum position_control_state s) { + const uint32_t now = AP_HAL::millis(); if (state != s) { auto &qp = plane.quadplane; pilot_correction_done = false; @@ -2183,14 +2184,17 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) Vector2f rpos; last_pos_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos); } + // double log to capture the state change + qp.log_QPOS(); + state = s; qp.log_QPOS(); + last_log_ms = now; } - state = s; - last_state_change_ms = AP_HAL::millis(); + last_state_change_ms = now; // 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; + last_run_ms = now; } /*