AP_InertialSensor: keep a record of the priamry gyro and use it

add callbacks for when gyrp/accel primary changes

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1959,16 +1959,27 @@ void AP_InertialSensor::update(void)
             }
         }
 
+#if AP_AHRS_ENABLED
+        // ask AHRS for the true primary, might just be us though
+        _primary_gyro = AP::ahrs().get_primary_gyro_index();
+        _primary_accel = AP::ahrs().get_primary_accel_index();
+#endif
         // set primary to first healthy accel and gyro
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             if (_gyro_healthy[i] && _use(i)) {
                 _first_usable_gyro = i;
+#if !AP_AHRS_ENABLED
+                _primary_gyro = _first_usable_gyro;
+#endif
                 break;
             }
         }
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             if (_accel_healthy[i] && _use(i)) {
                 _first_usable_accel = i;
+#if !AP_AHRS_ENABLED
+                _primary_accel = _first_usable_accel;
+#endif
                 break;
             }
         }

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -659,10 +659,14 @@ private:
     bool _gyro_cal_ok[INS_MAX_INSTANCES];
     bool _accel_id_ok[INS_MAX_INSTANCES];
 
-    // primary accel and gyro
+    // first usable gyro and accel
     uint8_t _first_usable_gyro;
     uint8_t _first_usable_accel;
 
+    // primary accel and gyro
+    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;

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -222,7 +222,6 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
     save_gyro_window(instance, gyro, filter_phase++);
 
     Vector3f gyro_filtered = gyro;
-
 #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     // apply the harmonic notch filters
     for (auto &notch : _imu.harmonic_notches) {
@@ -230,15 +229,13 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
             continue;
         }
         bool inactive = notch.is_inactive();
-#if AP_AHRS_ENABLED
         // by default we only run the expensive notch filters on the
         // currently active IMU we reset the inactive notch filters so
         // that if we switch IMUs we're not left with old data
         if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
-            instance != AP::ahrs().get_primary_gyro_index()) {
+            instance != _imu._primary_gyro) {
             inactive = true;
         }
-#endif
         if (inactive) {
             // while inactive we reset the filter so when it activates the first output
             // will be the first input sample
@@ -371,6 +368,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 
     // 5us
     log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
+    update_primary_gyro();
 }
 
 /*
@@ -458,6 +456,7 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
     }
 
     log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
+    update_primary_gyro();
 }
 
 void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &raw_gyro, const Vector3f &filtered_gyro)
@@ -615,6 +614,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
     // assume we're doing pre-filter logging:
     log_accel_raw(instance, sample_us, accel);
 #endif
+    update_primary_accel();
 }
 
 /*
@@ -692,6 +692,7 @@ void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, con
     // assume we're doing pre-filter logging
     log_accel_raw(instance, sample_us, accel);
 #endif
+    update_primary_accel();
 }
 
 
@@ -807,6 +808,17 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
     update_gyro_filters(instance);
 }
 
+void AP_InertialSensor_Backend::update_primary_gyro()
+{
+    // timing changes need to be made in the bus thread in order to take effect which is
+    // why they are actioned here
+    const bool is_new_primary = gyro_instance == _imu._primary_gyro;
+    if (is_primary_gyro != is_new_primary) {
+        set_primary_gyro(is_new_primary);
+        is_primary_gyro = is_new_primary;
+    }
+}
+
 /*
   propagate filter changes from front end to backend
  */
@@ -850,6 +862,16 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
     update_accel_filters(instance);
 }
 
+void AP_InertialSensor_Backend::update_primary_accel()
+{
+    // timing changes need to be made in the bus thread in order to take effect which is
+    // why they are actioned here
+    const bool is_new_primary = accel_instance == _imu._primary_accel;
+    if (is_primary_accel != is_new_primary) {
+        set_primary_accel(is_new_primary);
+        is_primary_accel = is_new_primary;
+    }
+}
 
 /*
   propagate filter changes from front end to backend

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -160,7 +160,9 @@ protected:
 
     // instance numbers of accel and gyro data
     uint8_t gyro_instance;
+    bool is_primary_gyro = true;
     uint8_t accel_instance;
+    bool is_primary_accel = true;
 
     void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
     void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
@@ -292,6 +294,12 @@ protected:
     void update_accel(uint8_t instance) __RAMFUNC__; /* front end */
     void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */
 
+    // catch updates to the primary gyro and accel
+    void update_primary_gyro() __RAMFUNC__; /* backend */
+    void update_primary_accel() __RAMFUNC__; /* backend */
+    virtual void set_primary_gyro(bool is_primary) {}
+    virtual void set_primary_accel(bool is_primary) {}
+
     // support for updating filter at runtime
     uint16_t _last_accel_filter_hz;
     uint16_t _last_gyro_filter_hz;

libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

@@ -196,8 +196,7 @@ void AP_InertialSensor::write_notch_log_messages() const
 
         // ask the HarmonicNotchFilter object for primary gyro to
         // log the actual notch centers
-        const uint8_t primary_gyro = AP::ahrs().get_primary_gyro_index();
-        notch.filter[primary_gyro].log_notch_centers(i, now_us);
+        notch.filter[_primary_gyro].log_notch_centers(i, now_us);
     }
 }
 #endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED