From 23977dc0b544632197e8a3334ca89a1cf805a0be Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 16 May 2024 11:47:53 +1000 Subject: [PATCH] AP_Compass: do self test after cycle count setup and also turn it off --- libraries/AP_Compass/AP_Compass_RM3100.cpp | 25 ++++++++++++---------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp index bcc889ddfa44a0..7f8e9d354f25d9 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.cpp +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -66,7 +66,7 @@ #define TMRC 0x94 // Update rate 150Hz #define CMM 0x71 // read 3 axes and set data ready if 3 axes are ready -#define RUN_SELF_TEST 0xFA +#define RUN_SELF_TEST 0xFF extern const AP_HAL::HAL &hal; AP_Compass_Backend *AP_Compass_RM3100::probe(AP_HAL::OwnPtr dev, @@ -127,6 +127,17 @@ bool AP_Compass_RM3100::init() return false; } + dev->setup_checked_registers(8); + + dev->write_register(RM3100_TMRC_REG, TMRC, true); // CMM data rate + dev->write_register(RM3100_CMM_REG, CMM, true); // CMM configuration + dev->write_register(RM3100_CCX1_REG, CCP1, true); // cycle count x + dev->write_register(RM3100_CCX0_REG, CCP0, true); // cycle count x + dev->write_register(RM3100_CCY1_REG, CCP1, true); // cycle count y + dev->write_register(RM3100_CCY0_REG, CCP0, true); // cycle count y + dev->write_register(RM3100_CCZ1_REG, CCP1, true); // cycle count z + dev->write_register(RM3100_CCZ0_REG, CCP0, true); // cycle count z + uint8_t bist; // do a self test of Coils dev->write_register(RM3100_BIST_REG, RUN_SELF_TEST); @@ -141,16 +152,8 @@ bool AP_Compass_RM3100::init() return false; } - dev->setup_checked_registers(8); - - dev->write_register(RM3100_TMRC_REG, TMRC, true); // CMM data rate - dev->write_register(RM3100_CMM_REG, CMM, true); // CMM configuration - dev->write_register(RM3100_CCX1_REG, CCP1, true); // cycle count x - dev->write_register(RM3100_CCX0_REG, CCP0, true); // cycle count x - dev->write_register(RM3100_CCY1_REG, CCP1, true); // cycle count y - dev->write_register(RM3100_CCY0_REG, CCP0, true); // cycle count y - dev->write_register(RM3100_CCZ1_REG, CCP1, true); // cycle count z - dev->write_register(RM3100_CCZ0_REG, CCP0, true); // cycle count z + // turn off BIST + dev->write_register(RM3100_BIST_REG, 0x00); _scaler = (1 / GAIN_CC200) * UTESLA_TO_MGAUSS; // has to be changed if using a different cycle count