From b16226dddf33f0754f3644444ba2195006e2a5c0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 13 Nov 2016 17:08:06 +1100 Subject: [PATCH] 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 --- libraries/AP_Baro/AP_Baro_MS5611.cpp | 13 ++++++++++++- libraries/AP_Baro/AP_Baro_MS5611.h | 2 ++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 4cd81c67a1..6aa2dad73c 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -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; } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 6af5c591f6..6bb81f8a62 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -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