From 80f8f8b14e32b3bfb9241a872f1cfb5c5a48d825 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 19 Feb 2022 22:21:52 +0000 Subject: [PATCH] Plane: quadplane: double log QPOS state change --- ArduPlane/quadplane.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 60ce00ce94..f405fc2c8b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2139,6 +2139,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; @@ -2163,14 +2164,17 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) // reset throttle descent control qp.thr_ctrl_land = false; } + // 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; } /*