From 55f73f4163b39a7e41dbb9924e63f3bedb6649f0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 21 Dec 2017 15:04:13 +1100
Subject: [PATCH] AP_Airspeed: fixed timing errors in MS5525 driver

this fixes issue #7188, where we could read from the MS5525 sensor too
soon after requesting data and the result coming back would be garbage
---
 libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp | 43 ++++++++++++++++----
 libraries/AP_Airspeed/AP_Airspeed_MS5525.h   |  3 ++
 2 files changed, 37 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
index 3f41da615f..b93562fef5 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
@@ -90,6 +90,7 @@ bool AP_Airspeed_MS5525::init()
     uint8_t reg = REG_CONVERT_TEMPERATURE;
     dev->transfer(&reg, 1, nullptr, 0);
     state = 0;
+    command_send_us = AP_HAL::micros();
 
     // drop to 2 retries for runtime
     dev->set_retries(2);
@@ -226,30 +227,54 @@ void AP_Airspeed_MS5525::calculate(void)
 // 50Hz timer
 void AP_Airspeed_MS5525::timer()
 {
+    if (AP_HAL::micros() - command_send_us < 10000) {
+        // we should avoid trying to read the ADC too soon after
+        // sending the command
+        return;
+    }
+    
     uint32_t adc_val = read_adc();
 
+    if (adc_val == 0) {
+        // we have either done a read too soon after sending the
+        // request or we have tried to read the same sample twice. We
+        // re-send the command now as we don't know what state the
+        // sensor is in, and we do want to trigger it sending a value,
+        // which we will discard
+        if (dev->transfer(&cmd_sent, 1, nullptr, 0)) {
+            command_send_us = AP_HAL::micros();
+        }
+        // when we get adc_val == 0 then then both the current value and
+        // the next value we read from the sensor are invalid
+        ignore_next = true;
+        return;
+    }
+    
     /*
      * If read fails, re-initiate a read command for current state or we are
      * stuck
      */
-    uint8_t next_state = state;
-    if (adc_val != 0) {
-        next_state = (state + 1) % 5;
-
-        if (state == 0) {
+    if (!ignore_next) {
+        if (cmd_sent == REG_CONVERT_TEMPERATURE) {
             D2 = adc_val;
-        } else {
+        } else if (cmd_sent == REG_CONVERT_PRESSURE) {
             D1 = adc_val;
             calculate();
         }
     }
 
-    uint8_t next_cmd = next_state == 0 ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE;
-    if (!dev->transfer(&next_cmd, 1, nullptr, 0)) {
+    ignore_next = false;
+    
+    cmd_sent = (state == 0) ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE;
+    if (!dev->transfer(&cmd_sent, 1, nullptr, 0)) {
+        // we don't know for sure what state the sensor is in when we
+        // fail to send the command, so ignore the next response
+        ignore_next = true;
         return;
     }
+    command_send_us = AP_HAL::micros();
 
-    state = next_state;
+    state = (state + 1) % 5;
 }
 
 // return the current differential_pressure in Pascal
diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h
index e5e227a4f7..0cf9bbc708 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h
+++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h
@@ -64,6 +64,9 @@ private:
     uint8_t state;
     int32_t D1;
     int32_t D2;
+    uint32_t command_send_us;
+    bool ignore_next;
+    uint8_t cmd_sent;
     
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
 };