mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-08 22:53:57 -04:00
AP_Compass: when I2c fails, don't retry for 1s
This commit is contained in:
parent
9fb3b13af3
commit
60185509f4
@ -246,11 +246,20 @@ AP_Compass_HMC5843::init()
|
||||
// Read Sensor data
|
||||
bool AP_Compass_HMC5843::read()
|
||||
{
|
||||
if (!healthy && !re_initialise()) {
|
||||
return false;
|
||||
if (!healthy) {
|
||||
if (millis() < _retry_time) {
|
||||
return false;
|
||||
}
|
||||
if (!re_initialise()) {
|
||||
_retry_time = millis() + 1000;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (!read_raw()) {
|
||||
// try again in 1 second, and set I2c clock speed slower
|
||||
_retry_time = millis() + 1000;
|
||||
I2c.setSpeed(false);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -53,6 +53,7 @@ class AP_Compass_HMC5843 : public Compass
|
||||
virtual bool re_initialise(void);
|
||||
bool read_register(uint8_t address, uint8_t *value);
|
||||
bool write_register(uint8_t address, byte value);
|
||||
uint32_t _retry_time; // when unhealthy the millis() value to retry at
|
||||
|
||||
public:
|
||||
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
|
||||
|
Loading…
Reference in New Issue
Block a user