From bcd5dff774168752fbf26515708641fb82159c17 Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Mon, 17 Aug 2015 13:44:46 -0300 Subject: [PATCH] AP_Baro: MS56XX: do not change change state on error If we have an error in the SPI or I2C transaction we should not change the state. Otherwise we might read a temperature when the sensor is reporting pressure and vice-versa. --- libraries/AP_Baro/AP_Baro_MS5611.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 87e39e40fd..f5f50e8ae4 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -282,9 +282,11 @@ void AP_Baro_MS56XX::_timer(void) _s_D2 >>= 1; _d2_count = 16; } + + if (_serial->write(CMD_CONVERT_D1_OSR4096)) { // Command to read pressure + _state++; + } } - _state++; - _serial->write(CMD_CONVERT_D1_OSR4096); // Command to read pressure } else { uint32_t d1 = _serial->read_24bits(0);; if (d1 != 0) { @@ -301,13 +303,16 @@ void AP_Baro_MS56XX::_timer(void) } // Now a new reading exists _updated = true; - } - _state++; - if (_state == 5) { - _serial->write(CMD_CONVERT_D2_OSR4096); // Command to read temperature - _state = 0; - } else { - _serial->write(CMD_CONVERT_D1_OSR4096); // Command to read pressure + + if (_state == 4) { + if (_serial->write(CMD_CONVERT_D2_OSR4096)) { // Command to read temperature + _state = 0; + } + } else { + if (_serial->write(CMD_CONVERT_D1_OSR4096)) { // Command to read pressure + _state++; + } + } } }