From b339050e4594b6f93518ff3e41daa3d903f6a121 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Fri, 24 Feb 2017 18:16:45 -0800
Subject: [PATCH] AP_Compass: IST8310: account for errors in measurment
 requests

If we don't recover for errors in the request for new sample, we may get
stuck with no sample anymore. Recover from bad transfers.
---
 libraries/AP_Compass/AP_Compass_IST8310.cpp | 50 ++++++++++++++++-----
 libraries/AP_Compass/AP_Compass_IST8310.h   |  6 +++
 2 files changed, 44 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp
index f85e61e93b..b78e88ac0c 100644
--- a/libraries/AP_Compass/AP_Compass_IST8310.cpp
+++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp
@@ -42,6 +42,8 @@
 #define PDCNTL_REG 0x42
 #define NORMAL_PULSE_DURATION 0xC0
 
+#define SAMPLING_PERIOD_USEC (10 * USEC_PER_MSEC)
+
 extern const AP_HAL::HAL &hal;
 
 AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
@@ -110,9 +112,13 @@ bool AP_Compass_IST8310::init()
     _dev->set_device_type(DEVTYPE_IST8310);
     set_dev_id(_instance, _dev->get_bus_id());
 
-    _dev->register_periodic_callback(10 * USEC_PER_MSEC,
+    _dev->register_periodic_callback(SAMPLING_PERIOD_USEC,
                                      FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, void));
 
+    _perf_xfer_err = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "IST8310_xfer_err");
+    _perf_not_ready = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "IST8310_not_ready");
+    _perf_restart = hal.util->perf_alloc(AP_HAL::Util::PC_COUNT, "IST8310_restart");
+
     return true;
 
 fail:
@@ -122,13 +128,21 @@ fail:
 
 void AP_Compass_IST8310::start_conversion()
 {
-    _dev->write_register(CNTL1_REG,
-            SINGLE_MEASUREMENT_MODE);
+    if (!_dev->write_register(CNTL1_REG, SINGLE_MEASUREMENT_MODE)) {
+        hal.util->perf_count(_perf_xfer_err);
+        _need_start = true;
+        return;
+    }
+
+    _need_start = false;
 }
 
 void AP_Compass_IST8310::timer()
 {
-    bool ret = false;
+    if (_need_start) {
+        start_conversion();
+        return;
+    }
 
     struct PACKED {
         uint8_t status;
@@ -137,20 +151,34 @@ void AP_Compass_IST8310::timer()
         le16_t rz;
     } buffer;
 
-
-    ret = _dev->read_registers(STAT1_REG, (uint8_t *) &buffer, sizeof(buffer));
+    bool ret = _dev->read_registers(STAT1_REG, (uint8_t *) &buffer, sizeof(buffer));
     if (!ret) {
-        /* We're going to be back on the next iteration either way */
+        hal.util->perf_count(_perf_xfer_err);
         return;
     }
 
     auto status = buffer.status;
 
+    uint32_t now = AP_HAL::micros();
+
     if (!(status & 0x01)) {
-        /* We're not ready yet */
-       return;
+        hal.util->perf_count(_perf_not_ready);
+
+        /*
+         * Sensor is in a wrong state or something went really wrong: try to
+         * request a sample again
+         */
+        if (now > _last_measurement_usec + 2 * SAMPLING_PERIOD_USEC) {
+            hal.util->perf_count(_perf_restart);
+            start_conversion();
+        }
+
+        return;
     }
 
+    _last_measurement_usec = now;
+    start_conversion();
+
     auto x = static_cast<int16_t>(le16toh(buffer.rx));
     auto y = static_cast<int16_t>(le16toh(buffer.ry));
     auto z = static_cast<int16_t>(le16toh(buffer.rz));
@@ -162,7 +190,7 @@ void AP_Compass_IST8310::timer()
     rotate_field(field, _instance);
 
     /* publish raw_field (uncorrected point sample) for calibration use */
-    publish_raw_field(field, AP_HAL::micros(), _instance);
+    publish_raw_field(field, now, _instance);
 
     /* correct raw_field for known errors */
     correct_field(field, _instance);
@@ -172,8 +200,6 @@ void AP_Compass_IST8310::timer()
         _accum_count++;
         _sem->give();
     }
-
-    start_conversion();
 }
 
 void AP_Compass_IST8310::read()
diff --git a/libraries/AP_Compass/AP_Compass_IST8310.h b/libraries/AP_Compass/AP_Compass_IST8310.h
index 2ee0cf0c5c..13b7149f8d 100644
--- a/libraries/AP_Compass/AP_Compass_IST8310.h
+++ b/libraries/AP_Compass/AP_Compass_IST8310.h
@@ -47,8 +47,14 @@ private:
 
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
 
+    AP_HAL::Util::perf_counter_t _perf_xfer_err;
+    AP_HAL::Util::perf_counter_t _perf_not_ready;
+    AP_HAL::Util::perf_counter_t _perf_restart;
+
     Vector3f _accum = Vector3f();
     uint32_t _accum_count = 0;
+    uint32_t _last_measurement_usec;
     enum Rotation _rotation;
     uint8_t _instance;
+    bool _need_start;
 };