From 401c8b5f867f4f786224ae693ebda03a17036fc5 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Wed, 20 Feb 2019 14:13:33 +0100 Subject: [PATCH 1/3] AP_Compass: probe for RM3100 --- libraries/AP_Compass/AP_Compass.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 5089cc529f9b4..60c7b2d832a62 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -617,7 +617,7 @@ void Compass::_probe_external_i2c_compasses(void) all_external, all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL)); } - + #if !HAL_MINIMIZE_FEATURES // AK09916 on ICM20948 FOREACH_I2C_EXTERNAL(i) { @@ -693,6 +693,17 @@ void Compass::_probe_external_i2c_compasses(void) ADD_BACKEND(DRIVER_IST8308, AP_Compass_IST8308::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8308_I2C_ADDR), true, ROTATION_NONE)); } + + // external i2c bus + FOREACH_I2C_EXTERNAL(i) { + ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, HAL_COMPASS_RM3100_I2C_ADDR), + true, ROTATION_NONE)); + } + FOREACH_I2C_INTERNAL(i) { + ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, HAL_COMPASS_RM3100_I2C_ADDR), + all_external, ROTATION_NONE)); + } + #endif // HAL_MINIMIZE_FEATURES } From 97db19eb09cfe3cc6fa9d35de772f545188504df Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Wed, 20 Feb 2019 14:24:01 +0100 Subject: [PATCH 2/3] AP_Compass: add corection for CMM configuration and correct debuging msg --- libraries/AP_Compass/AP_Compass_RM3100.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp index 20cc649b869fe..80bc5ea3412ec 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.cpp +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -117,13 +117,15 @@ bool AP_Compass_RM3100::init() ccy1 != CCP1_DEFAULT || ccy0 != CCP0_DEFAULT || ccz1 != CCP1_DEFAULT || ccz0 != CCP0_DEFAULT) { // couldn't read one of the cycle count registers or didn't recognize the default cycle count values - goto fail; + // hal.console->printf("RM3100: Unable to read the register for whoami test\n"); + dev->get_semaphore()->give(); + return false; } dev->setup_checked_registers(8); - dev->write_register(RM3100_TMRC_REG, TMRC, true); // cycle count z - dev->write_register(RM3100_CMM_REG, CMM, false); // CMM configuration + 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 @@ -141,7 +143,7 @@ bool AP_Compass_RM3100::init() /* register the compass instance in the frontend */ compass_instance = register_compass(); - printf("Found a RM3100 at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance); + hal.console->printf("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), compass_instance); set_rotation(compass_instance, rotation); @@ -157,10 +159,6 @@ bool AP_Compass_RM3100::init() FUNCTOR_BIND_MEMBER(&AP_Compass_RM3100::timer, void)); return true; - -fail: - dev->get_semaphore()->give(); - return false; } void AP_Compass_RM3100::timer() From 1eae546a19f8cb429a948b9e37465fbc249eeead Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 22 Feb 2019 11:41:59 +0100 Subject: [PATCH 3/3] AP_Compass: RM3100, don't touch TMRC register to use defualt CMM update rate --- libraries/AP_Compass/AP_Compass_RM3100.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp index 80bc5ea3412ec..4307273c3620b 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.cpp +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -124,7 +124,6 @@ bool AP_Compass_RM3100::init() 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