From f7279aa13fa41c6c233b7360577070e2f6f7e5cf Mon Sep 17 00:00:00 2001
From: Mike McCauley <mikem@airspayce.com>
Date: Wed, 22 Jan 2014 08:57:03 +1000
Subject: [PATCH] AP_InertialSensor: AP_InertialSensor_Flymaple implement
 get_gyro_health and get_accel_health.

---
 .../AP_InertialSensor_Flymaple.cpp            | 30 +++++++++++++++++++
 .../AP_InertialSensor_Flymaple.h              |  3 ++
 2 files changed, 33 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
index a04d9698ec..ccc481c086 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
@@ -224,6 +224,36 @@ bool AP_InertialSensor_Flymaple::update(void)
     return true;
 }
 
+bool AP_InertialSensor_Flymaple::get_gyro_health(void) const
+{
+    if (_last_gyro_timestamp == 0) {
+        // not initialised yet, show as healthy to prevent scary GCS
+        // warnings
+        return true;
+    }
+    uint64_t now = hal.scheduler->micros();
+    if ((now - _last_gyro_timestamp) >= (2 * raw_sample_interval_us)) {
+        // gyros have not updated
+        return false;
+    }
+    return true;
+}
+
+bool AP_InertialSensor_Flymaple::get_accel_health(void) const
+{
+    if (_last_accel_timestamp == 0) {
+        // not initialised yet, show as healthy to prevent scary GCS
+        // warnings
+        return true;
+    }
+    uint64_t now = hal.scheduler->micros();
+    if ((now - _last_accel_timestamp) >= (2 * raw_sample_interval_us)) {
+        // gyros have not updated
+        return false;
+    }
+    return true;
+}
+
 float AP_InertialSensor_Flymaple::get_delta_time(void) const
 {
     return _delta_time;
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
index 2d546b06ac..6cb7273e23 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
@@ -22,6 +22,9 @@ public:
     float        	get_delta_time() const;
     float           get_gyro_drift_rate();
     bool            wait_for_sample(uint16_t timeout_ms);
+    bool            get_gyro_health(void) const;
+    bool            get_accel_health(void) const;
+    bool            healthy(void) const { return get_gyro_health() && get_accel_health(); } // testing
 
 private:
     uint16_t        _init_sensor( Sample_rate sample_rate );