AP_NavEKF2/3: learn biases for inactive IMUs

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -150,7 +150,7 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
     Vector3f delta_angle;
     const AP_InertialSensor &_ins = AP::ins();
     for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
-        if (_ins.get_gyro_health(i) && healthy_count < 2) {
+        if (_ins.use_gyro(i) && healthy_count < 2) {
             Vector3f dangle;
             if (_ins.get_delta_angle(i, dangle)) {
                 healthy_count++;
@@ -638,8 +638,9 @@ AP_AHRS_DCM::drift_correction(float deltat)
     const AP_InertialSensor &_ins = AP::ins();
 
     // rotate accelerometer values into the earth frame
+    uint8_t healthy_count = 0;
     for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
-        if (_ins.get_accel_health(i)) {
+        if (_ins.use_accel(i) && healthy_count < 2) {
             /*
               by using get_delta_velocity() instead of get_accel() the
               accel value is sampled over the right time delta for
@@ -653,6 +654,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
                 // integrate the accel vector in the earth frame between GPS readings
                 _ra_sum[i] += _accel_ef[i] * deltat;
             }
+            healthy_count++;
         }
     }
 

libraries/AP_HAL_ChibiOS/Scheduler.cpp

@@ -163,12 +163,12 @@ void Scheduler::boost_end(void)
  */
 void Scheduler::delay_microseconds_boost(uint16_t usec)
 {
-    if (in_main_thread()) {
+    if (!_priority_boosted && in_main_thread()) {
         set_high_priority();
         _priority_boosted = true;
+        _called_boost = true;
     }
     delay_microseconds(usec); //Suspends Thread for desired microseconds
-    _called_boost = true;
 }
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1401,25 +1401,64 @@ void AP_InertialSensor::wait_for_sample(void)
 
 check_sample:
     if (!_hil_mode) {
-        // we also wait for at least one backend to have a sample of both
-        // accel and gyro. This normally completes immediately.
-        bool gyro_available = false;
-        bool accel_available = false;
+        // now we wait until we have the gyro and accel samples we need
+        uint8_t gyro_available_mask = 0;
+        uint8_t accel_available_mask = 0;
+        uint32_t wait_counter = 0;
+
         while (true) {
             for (uint8_t i=0; i<_backend_count; i++) {
+                // this is normally a nop, but can be used by backends
+                // that don't accumulate samples on a timer
                 _backends[i]->accumulate();
             }
 
-            for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
-                gyro_available |= _new_gyro_data[i];
-                accel_available |= _new_accel_data[i];
+            for (uint8_t i=0; i<_gyro_count; i++) {
+                if (_new_gyro_data[i]) {
+                    const uint8_t imask = (1U<<i);
+                    gyro_available_mask |= imask;
+                    if (_use[i]) {
+                        _gyro_wait_mask |= imask;
+                    } else {
+                        _gyro_wait_mask &= ~imask;
+                    }
+                }
+            }
+            for (uint8_t i=0; i<_accel_count; i++) {
+                if (_new_accel_data[i]) {
+                    const uint8_t imask = (1U<<i);
+                    accel_available_mask |= imask;
+                    if (_use[i]) {
+                        _accel_wait_mask |= imask;
+                    } else {
+                        _accel_wait_mask &= ~imask;
+                    }
+                }
             }
 
-            if (gyro_available && accel_available) {
-                break;
+            // we wait for up to 800us to get all of the required
+            // accel and gyro samples. After that we accept at least
+            // one of each
+            if (wait_counter < 7) {
+                if (gyro_available_mask &&
+                    ((gyro_available_mask & _gyro_wait_mask) == _gyro_wait_mask) &&
+                    accel_available_mask &&
+                    ((accel_available_mask & _accel_wait_mask) == _accel_wait_mask)) {
+                    break;
+                }
+            } else {
+                if (gyro_available_mask && accel_available_mask) {
+                    // reset the wait mask so we don't keep delaying
+                    // for a dead IMU on the next loop. As soon as it
+                    // comes back we will start waiting on it again
+                    _gyro_wait_mask &= gyro_available_mask;
+                    _accel_wait_mask &= accel_available_mask;
+                    break;
+                }
             }
 
-            hal.scheduler->delay_microseconds(100);
+            hal.scheduler->delay_microseconds_boost(100);
+            wait_counter++;
         }
     }
 
@@ -2006,6 +2045,31 @@ AP_InertialSensor::Gyro_Calibration_Timing AP_InertialSensor::gyro_calibration_t
     return (Gyro_Calibration_Timing)_gyro_cal_timing.get();
 }
 
+#if !HAL_MINIMIZE_FEATURES
+/*
+  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);
+    }
+}
+#endif // HAL_MINIMIZE_FEATURES
+
 
 namespace AP {
 

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -274,6 +274,9 @@ class AP_InertialSensor : AP_AccelCal_Client
     // 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,
@@ -495,6 +498,11 @@ class AP_InertialSensor : AP_AccelCal_Client
     uint8_t _primary_gyro;
     uint8_t _primary_accel;
 
+    // mask of accels and gyros which we will be actively using
+    // and this should wait for in wait_for_sample()
+    uint8_t _gyro_wait_mask;
+    uint8_t _accel_wait_mask;
+
     // bitmask bit which indicates if we should log raw accel and gyro data
     uint32_t _log_raw_bit;
 
@@ -579,6 +587,8 @@ class AP_InertialSensor : AP_AccelCal_Client
     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,11 +147,16 @@ 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],
                         _imu._gyro_raw_sample_rates[instance]);
 
+    uint64_t last_sample_us = _imu._gyro_last_sample_us[instance];
+
     /*
       we have two classes of sensors. FIFO based sensors produce data
       at a very predictable overall rate, but the data comes in
@@ -159,24 +167,26 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
      */
     if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) {
         dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6f;
+        _imu._gyro_last_sample_us[instance] = sample_us;
     } else {
         // don't accept below 100Hz
         if (_imu._gyro_raw_sample_rates[instance] < 100) {
             return;
         }
 
         dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
+        _imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
     }
-    _imu._gyro_last_sample_us[instance] = sample_us;
 
 #if AP_MODULE_SUPPORTED
     // call gyro_sample hook if any
     AP_Module::call_hook_gyro_sample(instance, dt, gyro);
 #endif
 
     // push gyros if optical flow present
-    if (hal.opticalflow)
+    if (hal.opticalflow) {
         hal.opticalflow->push_gyro(gyro.x, gyro.y, dt);
+    }
 
     // compute delta angle
     Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt;
@@ -193,6 +203,16 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 
     {
         WITH_SEMAPHORE(_sem);
+        uint64_t now = AP_HAL::micros64();
+
+        if (now - last_sample_us > 100000U) {
+            // zero accumulator if sensor was unhealthy for 0.1s
+            _imu._delta_angle_acc[instance].zero();
+            _imu._delta_angle_acc_dt[instance] = 0;
+            dt = 0;
+            delta_angle.zero();
+        }
+
         // integrate delta angle accumulator
         // the angles and coning corrections are accumulated separately in the
         // referenced paper, but in simulation little difference was found between
@@ -255,6 +275,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,11 +311,16 @@ 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],
                         _imu._accel_raw_sample_rates[instance]);
 
+    uint64_t last_sample_us = _imu._accel_last_sample_us[instance];
+
     /*
       we have two classes of sensors. FIFO based sensors produce data
       at a very predictable overall rate, but the data comes in
@@ -303,15 +331,16 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
      */
     if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) {
         dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6f;
+        _imu._accel_last_sample_us[instance] = sample_us;
     } else {
         // don't accept below 100Hz
         if (_imu._accel_raw_sample_rates[instance] < 100) {
             return;
         }
 
         dt = 1.0f / _imu._accel_raw_sample_rates[instance];
+        _imu._accel_last_sample_us[instance] = AP_HAL::micros64();
     }
-    _imu._accel_last_sample_us[instance] = sample_us;
 
 #if AP_MODULE_SUPPORTED
     // call accel_sample hook if any
@@ -323,6 +352,15 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     {
         WITH_SEMAPHORE(_sem);
 
+        uint64_t now = AP_HAL::micros64();
+
+        if (now - last_sample_us > 100000U) {
+            // zero accumulator if sensor was unhealthy for 0.1s
+            _imu._delta_velocity_acc[instance].zero();
+            _imu._delta_velocity_acc_dt[instance] = 0;
+            dt = 0;
+        }
+
         // delta velocity
         _imu._delta_velocity_acc[instance] += accel * dt;
         _imu._delta_velocity_acc_dt[instance] += dt;
@@ -428,6 +466,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 +484,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 +515,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;

libraries/AP_Mount/SoloGimbal.cpp

@@ -229,7 +229,7 @@ void SoloGimbal::readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
 void SoloGimbal::update_fast() {
     const AP_InertialSensor &ins = AP::ins();
 
-    if (ins.get_gyro_health(0) && ins.get_gyro_health(1)) {
+    if (ins.use_gyro(0) && ins.use_gyro(1)) {
         // dual gyro mode - average first two gyros
         Vector3f dAng;
         readVehicleDeltaAngle(0, dAng);