5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

AP_Compass: fixed field orientation for MMC3416

the sensor is not FRD, and needs Y axis reversed to follow ArduPilot
FRD convention for magnetometers
This commit is contained in:
Andrew Tridgell 2023-01-23 17:15:58 +11:00 committed by Peter Barker
parent 0f62f8e4a8
commit f203cf321f

View File

@ -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);