AP_Baro: when I2c fails, don't retry for 1s

This commit is contained in:
Andrew Tridgell 2012-01-04 16:35:16 +11:00
parent 880c7e6411
commit 9fb3b13af3
2 changed files with 13 additions and 0 deletions

View File

@ -152,7 +152,13 @@ void AP_Baro_BMP085::ReadPress()
{
uint8_t buf[3];
if (!healthy && millis() < _retry_time) {
return;
}
if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
_retry_time = millis() + 1000;
I2c.setSpeed(false);
healthy = false;
return;
}
@ -197,7 +203,13 @@ void AP_Baro_BMP085::ReadTemp()
{
uint8_t buf[2];
if (!healthy && millis() < _retry_time) {
return;
}
if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
_retry_time = millis() + 1000;
I2c.setSpeed(false);
healthy = false;
return;
}

View File

@ -50,6 +50,7 @@ class AP_Baro_BMP085 : public AP_Baro
uint8_t _temp_index;
uint8_t _press_index;
uint32_t _retry_time;
void Command_ReadPress();
void Command_ReadTemp();