From 9ecd889e9d88a3f78eab65547873e57b921dcd06 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 30 Oct 2013 08:54:03 +1100
Subject: [PATCH] AP_InertialSensor: detect bad MPU6000 SPI transactions and
 lower bus speed

this uses bad data or bad INT_STATUS values from the MPU6000 to detect
the sensor running too fast and lower bus speed
---
 libraries/AP_InertialSensor/AP_InertialSensor.h     |  3 +++
 .../AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 13 +++++++------
 .../AP_InertialSensor/AP_InertialSensor_MPU6000.h   |  3 +++
 3 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 11109afc44..e7d282b551 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -139,6 +139,9 @@ public:
         }
     }
 
+    virtual uint16_t error_count(void) const { return 0; }
+    virtual bool healthy(void) const { return true; }
+
 protected:
 
     // sensor specific init to be overwritten by descendant classes
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 6aafb29d6a..af09400902 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -351,12 +351,17 @@ void AP_InertialSensor_MPU6000::_read_data_transaction() {
     } rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, };
     
     _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
-    
+
+    /*
+      detect a bad SPI bus transaction by looking for all 14 bytes
+      zero, or the wrong INT_STATUS register value. This is used to
+      detect a too high SPI bus speed.
+     */
     uint8_t i;
     for (i=0; i<14; i++) {
         if (rx.v[i] != 0) break;
     }
-    if (rx.int_status != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i != 14) {
+    if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
         // likely a bad bus transaction
         if (++_error_count > 4) {
             _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
@@ -534,10 +539,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate)
     // clear interrupt on any read, and hold the data ready pin high
     // until we clear the interrupt
     _register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN);
-    hal.scheduler->delay(1);
-
-    // read INT status to clear the initial bits
-    _register_read(MPUREG_INT_STATUS);
 
     // now that we have initialised, we set the SPI bus speed to high
     // (8MHz on APM2)
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index e34a8e7d35..e9c7dbc6ea 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -33,6 +33,9 @@ public:
     // get_delta_time returns the time period in seconds overwhich the sensor data was collected
     float            	get_delta_time();
 
+    uint16_t error_count(void) const { return _error_count; }
+    bool healthy(void) const { return _error_count <= 4; }
+
 protected:
     uint16_t                    _init_sensor( Sample_rate sample_rate );