From a5e3cd664b2c1db34705f061706bc7e5282e5617 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 18 Apr 2019 14:24:01 +1000
Subject: [PATCH] AP_InertialSensor: added RC switch for killing IMUs

---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 23 +++++++++++++++++++
 .../AP_InertialSensor/AP_InertialSensor.h     |  5 ++++
 .../AP_InertialSensor_Backend.cpp             | 23 +++++++++++++++++++
 3 files changed, 51 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index eb2b2d232f..cb04d35d7c 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -2043,6 +2043,29 @@ AP_InertialSensor::Gyro_Calibration_Timing AP_InertialSensor::gyro_calibration_t
     return (Gyro_Calibration_Timing)_gyro_cal_timing.get();
 }
 
+/*
+  update IMU kill mask, used for testing IMU failover
+ */
+void AP_InertialSensor::kill_imu(uint8_t imu_idx, bool kill_it)
+{
+    if (kill_it) {
+        uint8_t new_kill_mask = imu_kill_mask | (1U<<imu_idx);
+        // don't allow the last IMU to be killed
+        bool all_dead = true;
+        for (uint8_t i=0; i<MIN(_gyro_count, _accel_count); i++) {
+            if (use_gyro(i) && use_accel(i) && !(new_kill_mask & (1U<<i))) {
+                // we have at least one healthy IMU left
+                all_dead = false;
+            }
+        }
+        if (!all_dead) {
+            imu_kill_mask = new_kill_mask;
+        }
+    } else {
+        imu_kill_mask &= ~(1U<<imu_idx);
+    }
+}
+
 
 namespace AP {
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index f9dc8de86a..7f996f373e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -275,6 +275,9 @@ public:
     // return time in microseconds of last update() call
     uint32_t get_last_update_usec(void) const { return _last_update_usec; }
 
+    // for killing an IMU for testing purposes
+    void kill_imu(uint8_t imu_idx, bool kill_it);
+
     enum IMU_SENSOR_TYPE {
         IMU_SENSOR_TYPE_ACCEL = 0,
         IMU_SENSOR_TYPE_GYRO = 1,
@@ -584,6 +587,8 @@ private:
     uint32_t _gyro_startup_error_count[INS_MAX_INSTANCES];
     bool _startup_error_counts_set;
     uint32_t _startup_ms;
+
+    uint8_t imu_kill_mask;
 };
 
 namespace AP {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index cd763270de..f81a8ccee8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -132,6 +132,9 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
  */
 void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro)
 {
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
     _imu._gyro[instance] = gyro;
     _imu._gyro_healthy[instance] = true;
 
@@ -145,6 +148,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
                                                             const Vector3f &gyro,
                                                             uint64_t sample_us)
 {
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
     float dt;
 
     _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
@@ -245,6 +251,9 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa
  */
 void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel)
 {
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
     _imu._accel[instance] = accel;
     _imu._accel_healthy[instance] = true;
 
@@ -278,6 +287,9 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
                                                              uint64_t sample_us,
                                                              bool fsync_set)
 {
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
     float dt;
 
     _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
@@ -413,6 +425,9 @@ uint16_t AP_InertialSensor_Backend::get_sample_rate_hz(void) const
  */
 void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float temperature)
 {
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
     _imu._temperature[instance] = temperature;
 
     /* give the temperature to the control loop in order to keep it constant*/
@@ -430,6 +445,10 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
         return;
     }
 
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        _sem->give();
+        return;
+    }
     if (_imu._new_gyro_data[instance]) {
         _publish_gyro(instance, _imu._gyro_filtered[instance]);
         _imu._new_gyro_data[instance] = false;
@@ -453,6 +472,10 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance)
         return;
     }
 
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        _sem->give();
+        return;
+    }
     if (_imu._new_accel_data[instance]) {
         _publish_accel(instance, _imu._accel_filtered[instance]);
         _imu._new_accel_data[instance] = false;