mirror of https://github.com/ArduPilot/ardupilot
AP_Baro: fixed a bug with corrupted conversions in MS5611 driver
the datasheet says that if you get back zero in an ADC read that the next value can be corrupt. I have seen this happen on the FMUv1, leading to bad altitude readings
This commit is contained in:
parent
ac4b100ae4
commit
b16226dddf
|
@ -225,10 +225,21 @@ bool AP_Baro_MS56XX::_timer(void)
|
|||
|
||||
next_cmd = next_state == 0 ? ADDR_CMD_CONVERT_TEMPERATURE
|
||||
: ADDR_CMD_CONVERT_PRESSURE;
|
||||
_dev->transfer(&next_cmd, 1, nullptr, 0);
|
||||
if (!_dev->transfer(&next_cmd, 1, nullptr, 0)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
/* if we had a failed read we are all done */
|
||||
if (adc_val == 0) {
|
||||
// a failed read can mean the returned value is corrupt, we
|
||||
// must discard it
|
||||
_discard_next = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (_discard_next) {
|
||||
_discard_next = false;
|
||||
_state = next_state;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -51,6 +51,8 @@ protected:
|
|||
struct {
|
||||
uint16_t c1, c2, c3, c4, c5, c6;
|
||||
} _cal_reg;
|
||||
|
||||
bool _discard_next;
|
||||
};
|
||||
|
||||
class AP_Baro_MS5611 : public AP_Baro_MS56XX
|
||||
|
|
Loading…
Reference in New Issue