From 4a8a24a1a21052bcdcc61f0ef9afc20d0e17f33a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 21 Oct 2015 13:22:24 +1100 Subject: [PATCH] AP_Compass: don't update last_update_usec for raw fields this fixes a problem where the EKF gets compass samples at 50Hz instead of the expected 10Hz --- libraries/AP_Compass/AP_Compass_Backend.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index b4822ad6d1..4fc29279d5 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -44,7 +44,11 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint32_t time_us Compass::mag_state &state = _compass._state[instance]; state.last_update_ms = hal.scheduler->millis(); - state.last_update_usec = hal.scheduler->micros(); + + // 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.raw_field = mag; state.raw_meas_time_us = time_us; state.updated_raw_field = true;