AP_InertialSensor: added RC switch for killing IMUs

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -2045,6 +2045,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 {
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -274,6 +274,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 {

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -131,6 +131,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;
 
@@ -144,6 +147,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],
@@ -255,6 +261,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;
 
@@ -288,6 +297,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],
@@ -428,6 +440,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*/
@@ -443,6 +458,9 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
 {    
     WITH_SEMAPHORE(_sem);
 
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
     if (_imu._new_gyro_data[instance]) {
         _publish_gyro(instance, _imu._gyro_filtered[instance]);
         _imu._new_gyro_data[instance] = false;
@@ -471,6 +489,9 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance)
 {    
     WITH_SEMAPHORE(_sem);
 
+    if ((1U<<instance) & _imu.imu_kill_mask) {
+        return;
+    }
     if (_imu._new_accel_data[instance]) {
         _publish_accel(instance, _imu._accel_filtered[instance]);
         _imu._new_accel_data[instance] = false;