diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index bfad75c831..5cff49b5c6 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -159,6 +159,9 @@ bool AP_Compass_HMC5843::init() return false; } + // high retries for init + _bus->set_retries(10); + if (!_bus->configure()) { hal.console->printf("HMC5843: Could not configure the bus\n"); goto errout; @@ -185,6 +188,9 @@ bool AP_Compass_HMC5843::init() _initialised = true; + // lower retries for run + _bus->set_retries(3); + bus_sem->give(); // perform an initial read diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index b2c44532e3..3197bb0b43 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -89,6 +89,8 @@ public: // return 24 bit bus identifier virtual uint32_t get_bus_id(void) const = 0; + + virtual void set_retries(uint8_t retries) {} }; class AP_HMC5843_BusDriver_HALDevice : public AP_HMC5843_BusDriver @@ -113,6 +115,10 @@ public: uint32_t get_bus_id(void) const override { return _dev->get_bus_id(); } + + void set_retries(uint8_t retries) override { + return _dev->set_retries(retries); + } private: AP_HAL::OwnPtr _dev; diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp index 9abaaf4c13..12c7381364 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp @@ -86,6 +86,9 @@ bool AP_Compass_LIS3MDL::init() dev->set_read_flag(0xC0); } + // high retries for init + dev->set_retries(10); + uint8_t whoami; if (!dev->read_registers(ADDR_WHO_AM_I, &whoami, 1) || whoami != ID_WHO_AM_I) { @@ -100,6 +103,9 @@ bool AP_Compass_LIS3MDL::init() dev->write_register(ADDR_CTRL_REG3, 0, true); // continuous dev->write_register(ADDR_CTRL_REG4, 0x0C, true); // z-axis ultra high perf dev->write_register(ADDR_CTRL_REG5, 0x40, true); // block-data-update + + // lower retries for run + dev->set_retries(3); dev->get_semaphore()->give();