From f203cf321ffc8a37254eefecb663161564bfbf68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 23 Jan 2023 17:15:58 +1100 Subject: [PATCH] AP_Compass: fixed field orientation for MMC3416 the sensor is not FRD, and needs Y axis reversed to follow ArduPilot FRD convention for magnetometers --- libraries/AP_Compass/AP_Compass_MMC3416.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp index d7ba223e16..5177a74be6 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -247,6 +247,10 @@ void AP_Compass_MMC3416::timer() #endif last_sample_ms = AP_HAL::millis(); + + // sensor is not FRD + field.y = -field.y; + accumulate_sample(field, compass_instance); if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { @@ -273,6 +277,9 @@ void AP_Compass_MMC3416::timer() field *= -counts_to_milliGauss; field += offset; + // sensor is not FRD + field.y = -field.y; + last_sample_ms = AP_HAL::millis(); accumulate_sample(field, compass_instance);