From 46785e8ecfade10f857fc897e94a28008a60882f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 9 Nov 2016 19:53:55 +1100
Subject: [PATCH] AP_InertialSensor: improved method for FIFO integrity
 checking

check temperature every 255 samples against FIFO data
---
 .../AP_InertialSensor_MPU6000.cpp             | 44 ++++++++++++-------
 .../AP_InertialSensor_MPU6000.h               |  2 +
 .../AP_InertialSensor_MPU9250.cpp             |  2 +-
 3 files changed, 31 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index e2613bfd82..52f376bcd3 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -492,14 +492,6 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
 
         float temp = int16_val(data, 3);
         temp = temp/340 + 36.53;
-
-        if (fabsf(_last_temp - temp) > 10 && !is_zero(_last_temp)) {
-            // a 10 degree change in one sample is a highly likely
-            // sign of a FIFO alignment error
-            _last_temp = 0;
-            _fifo_reset();
-            return;
-        }
         _last_temp = temp;
         
         gyro = Vector3f(int16_val(data, 5),
@@ -532,14 +524,6 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
 
         float temp = int16_val(data, 3);
         temp = temp/340 + 36.53;
-
-        if (fabsf(_last_temp - temp) > 10 && !is_zero(_last_temp)) {
-            // a 10 degree change in one sample is a highly likely
-            // sign of a FIFO alignment error
-            _last_temp = 0;
-            _fifo_reset();
-            return;
-        }
         _last_temp = temp;
     }
 
@@ -556,6 +540,29 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
     _notify_new_gyro_raw_sample(_gyro_instance, gyro);
 }
 
+/*
+ * check the FIFO integrity by cross-checking the temperature against
+ * the last FIFO reading
+ */
+void AP_InertialSensor_MPU6000::_check_temperature(void)
+{
+    uint8_t rx[2];
+    
+    if (!_block_read(MPUREG_TEMP_OUT_H, rx, 2)) {
+        return;
+    }
+    float temp = int16_val(rx, 0) / 340 + 36.53;
+
+    if (fabsf(_last_temp - temp) > 2 && !is_zero(_last_temp)) {
+        // a 2 degree change in one sample is a highly likely
+        // sign of a FIFO alignment error
+        printf("FIFO temperature reset: %.2f %.2f\n",
+               (double)temp, (double)_last_temp);
+        _last_temp = temp;
+        _fifo_reset();
+    }
+}
+
 void AP_InertialSensor_MPU6000::_read_fifo()
 {
     uint8_t n_samples;
@@ -595,6 +602,11 @@ void AP_InertialSensor_MPU6000::_read_fifo()
     } else {
         _accumulate(rx, n_samples);
     }
+
+    if (_temp_counter++ == 255) {
+        // check FIFO integrity every 0.25s
+        _check_temperature();
+    }
 }
 
 bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf,
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index a7d8e033e4..e187acbdd4 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -87,6 +87,7 @@ private:
 
     void _accumulate(uint8_t *samples, uint8_t n_samples);
     void _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
+    void _check_temperature(void);
 
     // instance numbers of accel and gyro data
     uint8_t _gyro_instance;
@@ -112,6 +113,7 @@ private:
     
     // last temperature reading, used to detect FIFO errors
     float _last_temp;
+    uint8_t _temp_counter;
 
     // buffer for fifo read
     uint8_t *_fifo_buffer;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index 45b064277b..c90dd22f82 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -487,7 +487,7 @@ void AP_InertialSensor_MPU9250::_check_temperature(void)
         // a 2 degree change in one sample is a highly likely
         // sign of a FIFO alignment error
         printf("FIFO temperature reset: %.2f %.2f\n",
-               temp, _last_temp);
+               (double)temp, (double)_last_temp);
         _last_temp = temp;
         _fifo_reset();
     }