From 12e1f67326d83966a6a056f838e969d8436e4f8d Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 11 May 2021 13:45:27 +0200 Subject: [PATCH] AP_Compass: only update last_update_ms on raw_field update if on calibration --- libraries/AP_Compass/AP_Compass_Backend.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index a43d4c08f4..ae41108221 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -45,15 +45,14 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance) { - Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; - // note that we do not set last_update_usec here as otherwise the // EKF and DCM would end up consuming compass data at the full // sensor rate. We want them to consume only the filtered fields - state.last_update_ms = AP_HAL::millis(); #if COMPASS_CAL_ENABLED auto& cal = _compass._calibrator[_compass._get_priority(Compass::StateIndex(instance))]; if (cal != nullptr) { + Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)]; + state.last_update_ms = AP_HAL::millis(); cal->new_sample(mag); } #endif