From e5615ec34920e092145ba543354e5e5d1411ceb2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 1 Aug 2015 15:40:40 +0900
Subject: [PATCH] INS: add USE parameters

---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 48 ++++++++++++++++++-
 .../AP_InertialSensor/AP_InertialSensor.h     |  5 ++
 2 files changed, 51 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 7e8b2be0d7..918255ad2a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -265,6 +265,30 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("ACCEL_FILTER", 19, AP_InertialSensor, _accel_filter_cutoff,  DEFAULT_ACCEL_FILTER),
 
+    // @Param: USE
+    // @DisplayName: Use first IMU for attitude, velocity and position estimates
+    // @Description: Use first IMU for attitude, velocity and position estimates
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("USE", 20, AP_InertialSensor, _use[0],  1),
+
+#if INS_MAX_INSTANCES > 2
+    // @Param: USE2
+    // @DisplayName: Use second IMU for attitude, velocity and position estimates
+    // @Description: Use second IMU for attitude, velocity and position estimates
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("USE2", 21, AP_InertialSensor, _use[1],  1),
+#endif
+#if INS_MAX_INSTANCES > 3
+    // @Param: USE3
+    // @DisplayName: Use third IMU for attitude, velocity and position estimates
+    // @Description: Use third IMU for attitude, velocity and position estimates
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("USE3", 21, AP_InertialSensor, _use[2],  0),
+#endif
+
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
@@ -680,6 +704,16 @@ bool AP_InertialSensor::gyro_calibrated_ok_all() const
     return (get_gyro_count() > 0);
 }
 
+// return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
+bool AP_InertialSensor::use_gyro(uint8_t instance) const
+{
+    if (instance >= INS_MAX_INSTANCES) {
+        return false;
+    }
+
+    return (get_gyro_health(instance) && _use[instance]);
+}
+
 // get_accel_health_all - return true if all accels are healthy
 bool AP_InertialSensor::get_accel_health_all(void) const
 {
@@ -784,6 +818,16 @@ bool AP_InertialSensor::accel_calibrated_ok_all() const
     return true;
 }
 
+// return true if accel instance should be used (must be healthy and have it's use parameter set to 1)
+bool AP_InertialSensor::use_accel(uint8_t instance) const
+{
+    if (instance >= INS_MAX_INSTANCES) {
+        return false;
+    }
+
+    return (get_accel_health(instance) && _use[instance]);
+}
+
 void
 AP_InertialSensor::_init_gyro()
 {
@@ -1181,13 +1225,13 @@ void AP_InertialSensor::update(void)
 
         // set primary to first healthy accel and gyro
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
-            if (_gyro_healthy[i]) {
+            if (_gyro_healthy[i] && _use[i]) {
                 _primary_gyro = i;
                 break;
             }
         }
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
-            if (_accel_healthy[i]) {
+            if (_accel_healthy[i] && _use[i]) {
                 _primary_accel = i;
                 break;
             }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 44fd904022..f5f3535625 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -144,12 +144,14 @@ public:
     uint8_t get_gyro_count(void) const { return _gyro_count; }
     bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
     bool gyro_calibrated_ok_all() const;
+    bool use_gyro(uint8_t instance) const;
 
     bool get_accel_health(uint8_t instance) const { return (instance<_accel_count) ? _accel_healthy[instance] : false; }
     bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
     bool get_accel_health_all(void) const;
     uint8_t get_accel_count(void) const { return _accel_count; };
     bool accel_calibrated_ok_all() const;
+    bool use_accel(uint8_t instance) const;
 
     // get accel offsets in m/s/s
     const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
@@ -304,6 +306,9 @@ private:
     AP_Int8     _accel_filter_cutoff;
     AP_Int8     _gyro_filter_cutoff;
 
+    // use for attitude, velocity, position estimates
+    AP_Int8     _use[INS_MAX_INSTANCES];
+
     // board orientation from AHRS
     enum Rotation _board_orientation;