From 7fc5d693c2c7c3cc077e417ca4140b7aa2be024b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 1 Sep 2014 20:20:27 +0900
Subject: [PATCH] INS: add get_accel_health_all and get_gyro_health_all

Returns true only if all available accels or gyros are healthy
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 24 +++++++++++++++++++
 .../AP_InertialSensor/AP_InertialSensor.h     |  2 ++
 2 files changed, 26 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 3cb39d5b35..1f99d3079a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -294,6 +294,30 @@ AP_InertialSensor::init_gyro()
     _save_parameters();
 }
 
+// get_gyro_health_all - return true if all gyros are healthy
+bool AP_InertialSensor::get_gyro_health_all(void) const
+{
+    for (uint8_t i=0; i<get_gyro_count(); i++) {
+        if (!get_gyro_health(i)) {
+            return false;
+        }
+    }
+    // return true if we have at least one gyro
+    return (get_gyro_count() > 0);
+}
+
+// get_accel_health_all - return true if all accels are healthy
+bool AP_InertialSensor::get_accel_health_all(void) const
+{
+    for (uint8_t i=0; i<get_accel_count(); i++) {
+        if (!get_accel_health(i)) {
+            return false;
+        }
+    }
+    // return true if we have at least one accel
+    return (get_accel_count() > 0);
+}
+
 void
 AP_InertialSensor::_init_accel()
 {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 78f13baafc..70611c94d9 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -118,10 +118,12 @@ public:
     // multi-device interface
     virtual bool get_gyro_health(uint8_t instance) const { return true; }
     bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); }
+    bool get_gyro_health_all(void) const;
     virtual uint8_t get_gyro_count(void) const { return 1; };
 
     virtual bool get_accel_health(uint8_t instance) const { return true; }
     bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); }
+    bool get_accel_health_all(void) const;
     virtual uint8_t get_accel_count(void) const { return 1; };
 
     // get accel offsets in m/s/s