AP_InertialSensor: support two full harmonic notch filters

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -548,11 +548,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
 
     // @Group: HNTCH_
     // @Path: ../Filter/HarmonicNotchFilter.cpp
-    AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_",  41, AP_InertialSensor, HarmonicNotchFilterParams),
+    AP_SUBGROUPINFO(_harmonic_notch_filter[0], "HNTCH_",  41, AP_InertialSensor, HarmonicNotchFilterParams),
 
+#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1
     // @Group: HNTC2_
     // @Path: ../Filter/HarmonicNotchFilter.cpp
-    AP_SUBGROUPINFO(_notch_filter, "HNTC2_",  53, AP_InertialSensor, HarmonicNotchFilterParams),
+    AP_SUBGROUPINFO(_harmonic_notch_filter[1], "HNTC2_",  53, AP_InertialSensor, HarmonicNotchFilterParams),
+#endif
 
     // @Param: GYRO_RATE
     // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled
@@ -874,45 +876,44 @@ AP_InertialSensor::init(uint16_t loop_rate)
     // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
     // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
     // configured value
-    _calculated_harmonic_notch_freq_hz[0] = _harmonic_notch_filter.center_freq_hz();
-    _num_calculated_harmonic_notch_frequencies = 1;
-    _num_dynamic_harmonic_notches = 1;
-
     uint8_t num_filters = 0;
-    const bool double_notch = _harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
+    bool double_notch[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS] {};
+    for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
+        _calculated_harmonic_notch_freq_hz[i][0] = _harmonic_notch_filter[i].center_freq_hz();
+        _num_calculated_harmonic_notch_frequencies[i] = 1;
+        _num_dynamic_harmonic_notches[i] = 1;
+        double_notch[i] = _harmonic_notch_filter[i].hasOption(HarmonicNotchFilterParams::Options::DoubleNotch);
 #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
-    if (_harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
+        if (_harmonic_notch_filter[i].hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
 #if HAL_WITH_DSP
-        if (get_gyro_harmonic_notch_tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
+        if (get_gyro_harmonic_notch_tracking_mode(i) == HarmonicNotchDynamicMode::UpdateGyroFFT) {
-            _num_dynamic_harmonic_notches = AP_HAL::DSP::MAX_TRACKED_PEAKS; // only 3 peaks supported currently
+            _num_dynamic_harmonic_notches[i] = AP_HAL::DSP::MAX_TRACKED_PEAKS; // only 3 peaks supported currently
         } else
 #endif
         {
             AP_Motors *motors = AP::motors();
-            _num_dynamic_harmonic_notches = __builtin_popcount(motors->get_motor_mask());
+            if (motors != nullptr) {
+                _num_dynamic_harmonic_notches[i] = __builtin_popcount(motors->get_motor_mask());
+            }
         }
         // avoid harmonics unless actually configured by the user
-        _harmonic_notch_filter.set_default_harmonics(1);
+        _harmonic_notch_filter[i].set_default_harmonics(1);
-    }
+        }
 #endif
+    }
     // count number of used sensors
     uint8_t sensors_used = 0;
     for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
         sensors_used += _use[i];
     }
 
-    // calculate number of notches we might want to use for harmonic notch
+    for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
-    if (_harmonic_notch_filter.enabled()) {
+        // calculate number of notches we might want to use for harmonic notch
-        num_filters = __builtin_popcount( _harmonic_notch_filter.harmonics())
+        if (_harmonic_notch_filter[i].enabled()) {
-            * _num_dynamic_harmonic_notches * (double_notch ? 2 : 1)
+            num_filters += __builtin_popcount( _harmonic_notch_filter[i].harmonics())
-            * sensors_used;
+                * _num_dynamic_harmonic_notches[i] * (double_notch[i] ? 2 : 1)
-    }
+                * sensors_used;
-    // add filters used by static notch
+        }
-    if (_notch_filter.enabled()) {
-        _notch_filter.set_default_harmonics(1);
-        num_filters +=  __builtin_popcount(_notch_filter.harmonics())
-            * (_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch) ? 2 : 1)
-            * sensors_used;
     }
 
     if (num_filters > HAL_HNF_MAX_FILTERS) {
@@ -923,18 +924,14 @@ AP_InertialSensor::init(uint16_t loop_rate)
     for (uint8_t i=0; i<get_gyro_count(); i++) {
         // only allocate notches for IMUs in use
         if (_use[i]) {
-            if (_harmonic_notch_filter.enabled()) {
+            for (uint8_t j=0; j<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; j++) {
-                _gyro_harmonic_notch_filter[i].allocate_filters(_num_dynamic_harmonic_notches,
+                if (_harmonic_notch_filter[j].enabled()) {
-                    _harmonic_notch_filter.harmonics(), double_notch);
+                    _gyro_harmonic_notch_filter[j][i].allocate_filters(_num_dynamic_harmonic_notches[j],
-                // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
+                                                                       _harmonic_notch_filter[j].harmonics(), double_notch[j]);
-                _gyro_harmonic_notch_filter[i].init(_gyro_raw_sample_rates[i], _calculated_harmonic_notch_freq_hz[0],
+                    // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
-                    _harmonic_notch_filter.bandwidth_hz(), _harmonic_notch_filter.attenuation_dB());
+                    _gyro_harmonic_notch_filter[j][i].init(_gyro_raw_sample_rates[i], _calculated_harmonic_notch_freq_hz[j][0],
-            }
+                                                           _harmonic_notch_filter[j].bandwidth_hz(), _harmonic_notch_filter[j].attenuation_dB());
-            if (_notch_filter.enabled()) {
+                }
-                _gyro_notch_filter[i].allocate_filters(1, _notch_filter.harmonics(),
-                    _notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch));
-                _gyro_notch_filter[i].init(_gyro_raw_sample_rates[i], _notch_filter.center_freq_hz(),
-                    _notch_filter.bandwidth_hz(), _notch_filter.attenuation_dB());
             }
         }
     }
@@ -2007,24 +2004,30 @@ void AP_InertialSensor::acal_update()
 }
 
 // Update the harmonic notch frequency
-void AP_InertialSensor::update_harmonic_notch_freq_hz(float scaled_freq) {
+void AP_InertialSensor::update_harmonic_notch_freq_hz(uint8_t idx, float scaled_freq) {
+    if (idx >= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) {
+        return;
+    }
     // protect against zero as the scaled frequency
     if (is_positive(scaled_freq)) {
-        _calculated_harmonic_notch_freq_hz[0] = scaled_freq;
+        _calculated_harmonic_notch_freq_hz[idx][0] = scaled_freq;
     }
-    _num_calculated_harmonic_notch_frequencies = 1;
+    _num_calculated_harmonic_notch_frequencies[idx] = 1;
 }
 
 // Update the harmonic notch frequency
-void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) {
+void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]) {
+    if (idx >= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) {
+        return;
+    }
     // protect against zero as the scaled frequency
     for (uint8_t i = 0; i < num_freqs; i++) {
         if (is_positive(scaled_freq[i])) {
-            _calculated_harmonic_notch_freq_hz[i] = scaled_freq[i];
+            _calculated_harmonic_notch_freq_hz[idx][i] = scaled_freq[i];
         }
     }
     // any uncalculated frequencies will float at the previous value or the initialized freq if none
-    _num_calculated_harmonic_notch_frequencies = num_freqs;
+    _num_calculated_harmonic_notch_frequencies[idx] = num_freqs;
 }
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -35,6 +35,9 @@
 #define HAL_INS_TEMPERATURE_CAL_ENABLE !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
 #endif
 
+#ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
+#define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2
+#endif
 
 #include <stdint.h>
 
@@ -241,9 +244,9 @@ public:
     uint8_t get_primary_gyro(void) const { return _primary_gyro; }
 
     // Update the harmonic notch frequency
-    void update_harmonic_notch_freq_hz(float scaled_freq);
+    void update_harmonic_notch_freq_hz(uint8_t idx, float scaled_freq);
     // Update the harmonic notch frequencies
-    void update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
+    void update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]);
 
     // get the gyro filter rate in Hz
     uint16_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; }
@@ -252,32 +255,32 @@ public:
     uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
 
     // harmonic notch current center frequency
-    float get_gyro_dynamic_notch_center_freq_hz(void) const { return _calculated_harmonic_notch_freq_hz[0]; }
+    float get_gyro_dynamic_notch_center_freq_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_calculated_harmonic_notch_freq_hz[idx][0]:0; }
 
     // number of dynamic harmonic notches
-    uint8_t get_num_gyro_dynamic_notches(void) const { return _num_dynamic_harmonic_notches; }
+    uint8_t get_num_gyro_dynamic_notches(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_num_dynamic_harmonic_notches[idx]:0; }
 
     // set of harmonic notch current center frequencies
-    const float* get_gyro_dynamic_notch_center_frequencies_hz(void) const { return _calculated_harmonic_notch_freq_hz; }
+    const float* get_gyro_dynamic_notch_center_frequencies_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_calculated_harmonic_notch_freq_hz[idx]:nullptr; }
 
     // number of harmonic notch current center frequencies
-    uint8_t get_num_gyro_dynamic_notch_center_frequencies(void) const { return _num_calculated_harmonic_notch_frequencies; }
+    uint8_t get_num_gyro_dynamic_notch_center_frequencies(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_num_calculated_harmonic_notch_frequencies[idx]:0; }
 
     // harmonic notch reference center frequency
-    float get_gyro_harmonic_notch_center_freq_hz(void) const { return _harmonic_notch_filter.center_freq_hz(); }
+    float get_gyro_harmonic_notch_center_freq_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].center_freq_hz():0; }
 
     // harmonic notch reference scale factor
-    float get_gyro_harmonic_notch_reference(void) const { return _harmonic_notch_filter.reference(); }
+    float get_gyro_harmonic_notch_reference(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].reference():0; }
 
     // harmonic notch tracking mode
-    HarmonicNotchDynamicMode get_gyro_harmonic_notch_tracking_mode(void) const { return _harmonic_notch_filter.tracking_mode(); }
+    HarmonicNotchDynamicMode get_gyro_harmonic_notch_tracking_mode(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].tracking_mode():HarmonicNotchDynamicMode::Fixed; }
 
     // harmonic notch harmonics
-    uint8_t get_gyro_harmonic_notch_harmonics(void) const { return _harmonic_notch_filter.harmonics(); }
+    uint8_t get_gyro_harmonic_notch_harmonics(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].harmonics():0; }
 
     // harmonic notch options
-    bool has_harmonic_option(HarmonicNotchFilterParams::Options option) {
+    bool has_harmonic_option(uint8_t idx, HarmonicNotchFilterParams::Options option) {
-        return _harmonic_notch_filter.hasOption(option);
+        return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].hasOption(option):false;
     }
 
     // write out harmonic notch log messages
@@ -307,7 +310,7 @@ public:
     bool gyro_notch_enabled(void) const { return _notch_filter.enabled(); }
 
     // return true if harmonic notch enabled
-    bool gyro_harmonic_notch_enabled(void) const { return _harmonic_notch_filter.enabled(); }
+    bool gyro_harmonic_notch_enabled(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_harmonic_notch_filter[idx].enabled():false; }
 
     AuxiliaryBus *get_auxiliary_bus(int16_t backend_id) { return get_auxiliary_bus(backend_id, 0); }
     AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance);
@@ -509,18 +512,14 @@ private:
     bool _new_accel_data[INS_MAX_INSTANCES];
     bool _new_gyro_data[INS_MAX_INSTANCES];
 
-    // optional notch filter on gyro
-    HarmonicNotchFilterParams _notch_filter;
-    HarmonicNotchFilterVector3f _gyro_notch_filter[INS_MAX_INSTANCES];
-
     // optional harmonic notch filter on gyro
-    HarmonicNotchFilterParams _harmonic_notch_filter;
+    HarmonicNotchFilterParams _harmonic_notch_filter[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
-    HarmonicNotchFilterVector3f _gyro_harmonic_notch_filter[INS_MAX_INSTANCES];
+    HarmonicNotchFilterVector3f _gyro_harmonic_notch_filter[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS][INS_MAX_INSTANCES];
     // number of independent notches in the filter
-    uint8_t _num_dynamic_harmonic_notches;
+    uint8_t _num_dynamic_harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
     // the current center frequency for the notch
-    float _calculated_harmonic_notch_freq_hz[INS_MAX_NOTCHES];
+    float _calculated_harmonic_notch_freq_hz[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS][INS_MAX_NOTCHES];
-    uint8_t _num_calculated_harmonic_notch_frequencies;
+    uint8_t _num_calculated_harmonic_notch_frequencies[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
 
     // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -274,14 +274,11 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 #endif
         Vector3f gyro_filtered = gyro;
 
-        // apply the notch filter
-        if (_gyro_notch_enabled()) {
-            gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);
-        }
-
         // apply the harmonic notch filter
-        if (gyro_harmonic_notch_enabled()) {
+        for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
-            gyro_filtered = _imu._gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
+            if (gyro_harmonic_notch_enabled(i)) {
+                gyro_filtered = _imu._gyro_harmonic_notch_filter[i][instance].apply(gyro_filtered);
+            }
         }
 
         // apply the low pass filter last to attentuate any notch induced noise
@@ -290,8 +287,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         // if the filtering failed in any way then reset the filters and keep the old value
         if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) {
             _imu._gyro_filter[instance].reset();
-            _imu._gyro_notch_filter[instance].reset();
+            for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
-            _imu._gyro_harmonic_notch_filter[instance].reset();
+                _imu._gyro_harmonic_notch_filter[i][instance].reset();
+            }
         } else {
             _imu._gyro_filtered[instance] = gyro_filtered;
         }
@@ -395,14 +393,11 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
 #endif
         Vector3f gyro_filtered = gyro;
 
-        // apply the notch filter
+        // apply the harmonic notch filters
-        if (_gyro_notch_enabled()) {
+        for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
-            gyro_filtered = _imu._gyro_notch_filter[instance].apply(gyro_filtered);
+            if (gyro_harmonic_notch_enabled(i)) {
-        }
+                gyro_filtered = _imu._gyro_harmonic_notch_filter[i][instance].apply(gyro_filtered);
-
+            }
-        // apply the harmonic notch filter
-        if (gyro_harmonic_notch_enabled()) {
-            gyro_filtered = _imu._gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
         }
 
         // apply the low pass filter last to attentuate any notch induced noise
@@ -411,8 +406,9 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
         // if the filtering failed in any way then reset the filters and keep the old value
         if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) {
             _imu._gyro_filter[instance].reset();
-            _imu._gyro_notch_filter[instance].reset();
+            for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
-            _imu._gyro_harmonic_notch_filter[instance].reset();
+                _imu._gyro_harmonic_notch_filter[i][instance].reset();
+            }
         } else {
             _imu._gyro_filtered[instance] = gyro_filtered;
         }
@@ -734,30 +730,25 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
     }
 
     // possily update the harmonic notch filter parameters
-    if (!is_equal(_last_harmonic_notch_bandwidth_hz, gyro_harmonic_notch_bandwidth_hz()) ||
+    for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
-        !is_equal(_last_harmonic_notch_attenuation_dB, gyro_harmonic_notch_attenuation_dB()) ||
+        if (!is_equal(_last_harmonic_notch_bandwidth_hz[i], gyro_harmonic_notch_bandwidth_hz(i)) ||
-        sensors_converging()) {
+            !is_equal(_last_harmonic_notch_attenuation_dB[i], gyro_harmonic_notch_attenuation_dB(i)) ||
-        _imu._gyro_harmonic_notch_filter[instance].init(_gyro_raw_sample_rate(instance), gyro_harmonic_notch_center_freq_hz(), gyro_harmonic_notch_bandwidth_hz(), gyro_harmonic_notch_attenuation_dB());
+            sensors_converging()) {
-        _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
+            _imu._gyro_harmonic_notch_filter[i][instance].init(_gyro_raw_sample_rate(instance),
-        _last_harmonic_notch_bandwidth_hz = gyro_harmonic_notch_bandwidth_hz();
+                                                               gyro_harmonic_notch_center_freq_hz(i),
-        _last_harmonic_notch_attenuation_dB = gyro_harmonic_notch_attenuation_dB();
+                                                               gyro_harmonic_notch_bandwidth_hz(i),
-    } else if (!is_equal(_last_harmonic_notch_center_freq_hz, gyro_harmonic_notch_center_freq_hz())) {
+                                                               gyro_harmonic_notch_attenuation_dB(i));
-        if (num_gyro_harmonic_notch_center_frequencies() > 1) {
+            _last_harmonic_notch_center_freq_hz[i] = gyro_harmonic_notch_center_freq_hz(i);
-            _imu._gyro_harmonic_notch_filter[instance].update(num_gyro_harmonic_notch_center_frequencies(), gyro_harmonic_notch_center_frequencies_hz());
+            _last_harmonic_notch_bandwidth_hz[i] = gyro_harmonic_notch_bandwidth_hz(i);
-        } else {
+            _last_harmonic_notch_attenuation_dB[i] = gyro_harmonic_notch_attenuation_dB(i);
-            _imu._gyro_harmonic_notch_filter[instance].update(gyro_harmonic_notch_center_freq_hz());
+        } else if (!is_equal(_last_harmonic_notch_center_freq_hz[i], gyro_harmonic_notch_center_freq_hz(i))) {
+            if (num_gyro_harmonic_notch_center_frequencies(i) > 1) {
+                _imu._gyro_harmonic_notch_filter[i][instance].update(num_gyro_harmonic_notch_center_frequencies(i), gyro_harmonic_notch_center_frequencies_hz(i));
+            } else {
+                _imu._gyro_harmonic_notch_filter[i][instance].update(gyro_harmonic_notch_center_freq_hz(i));
+            }
+            _last_harmonic_notch_center_freq_hz[i] = gyro_harmonic_notch_center_freq_hz(i);
         }
-        _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
-    }
-    // possily update the notch filter parameters
-    if (!is_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) ||
-        !is_equal(_last_notch_bandwidth_hz, _gyro_notch_bandwidth_hz()) ||
-        !is_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB()) ||
-        sensors_converging()) {
-        _imu._gyro_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_notch_center_freq_hz(), _gyro_notch_bandwidth_hz(), _gyro_notch_attenuation_dB());
-        _last_notch_center_freq_hz = _gyro_notch_center_freq_hz();
-        _last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz();
-        _last_notch_attenuation_dB = _gyro_notch_attenuation_dB();
     }
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -257,33 +257,22 @@ protected:
         return (uint16_t)_imu._loop_rate;
     }
 
-    // return the notch filter center in Hz for the sample rate
-    float _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); }
-
-    // return the notch filter bandwidth in Hz for the sample rate
-    float _gyro_notch_bandwidth_hz(void) const { return _imu._notch_filter.bandwidth_hz(); }
-
-    // return the notch filter attenuation in dB for the sample rate
-    float _gyro_notch_attenuation_dB(void) const { return _imu._notch_filter.attenuation_dB(); }
-
-    bool _gyro_notch_enabled(void) const { return _imu._notch_filter.enabled(); }
-
     // return the harmonic notch filter center in Hz for the sample rate
-    float gyro_harmonic_notch_center_freq_hz() const { return _imu.get_gyro_dynamic_notch_center_freq_hz(); }
+    float gyro_harmonic_notch_center_freq_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu.get_gyro_dynamic_notch_center_freq_hz(idx):0; }
 
     // set of harmonic notch current center frequencies
-    const float* gyro_harmonic_notch_center_frequencies_hz(void) const { return _imu.get_gyro_dynamic_notch_center_frequencies_hz(); }
+    const float* gyro_harmonic_notch_center_frequencies_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu.get_gyro_dynamic_notch_center_frequencies_hz(idx):nullptr; }
 
     // number of harmonic notch current center frequencies
-    uint8_t num_gyro_harmonic_notch_center_frequencies(void) const { return _imu.get_num_gyro_dynamic_notch_center_frequencies(); }
+    uint8_t num_gyro_harmonic_notch_center_frequencies(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu.get_num_gyro_dynamic_notch_center_frequencies(idx):0; }
 
     // return the harmonic notch filter bandwidth in Hz for the sample rate
-    float gyro_harmonic_notch_bandwidth_hz(void) const { return _imu._harmonic_notch_filter.bandwidth_hz(); }
+    float gyro_harmonic_notch_bandwidth_hz(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu._harmonic_notch_filter[idx].bandwidth_hz():0; }
 
     // return the harmonic notch filter attenuation in dB for the sample rate
-    float gyro_harmonic_notch_attenuation_dB(void) const { return _imu._harmonic_notch_filter.attenuation_dB(); }
+    float gyro_harmonic_notch_attenuation_dB(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu._harmonic_notch_filter[idx].attenuation_dB():0; }
 
-    bool gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); }
+    bool gyro_harmonic_notch_enabled(uint8_t idx) const { return idx<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS?_imu._harmonic_notch_filter[idx].enabled():false; }
 
     // common gyro update function for all backends
     void update_gyro(uint8_t instance) __RAMFUNC__; /* front end */
@@ -294,14 +283,11 @@ protected:
     // support for updating filter at runtime
     uint16_t _last_accel_filter_hz;
     uint16_t _last_gyro_filter_hz;
-    float _last_notch_center_freq_hz;
-    float _last_notch_bandwidth_hz;
-    float _last_notch_attenuation_dB;
 
     // support for updating harmonic filter at runtime
-    float _last_harmonic_notch_center_freq_hz;
+    float _last_harmonic_notch_center_freq_hz[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
-    float _last_harmonic_notch_bandwidth_hz;
+    float _last_harmonic_notch_bandwidth_hz[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
-    float _last_harmonic_notch_attenuation_dB;
+    float _last_harmonic_notch_attenuation_dB[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
 
     void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
         _imu._gyro_orientation[instance] = rotation;

libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

@@ -132,6 +132,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
 // @LoggerMessage: FTN
 // @Description: Filter Tuning Messages
 // @Field: TimeUS: microseconds since system startup
+// @Field: I: instance
 // @Field: NDn: number of active dynamic harmonic notches
 // @Field: NF1: dynamic harmonic notch centre frequency for motor 1
 // @Field: NF2: dynamic harmonic notch centre frequency for motor 2
@@ -147,10 +148,21 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
 // @Field: NF12: dynamic harmonic notch centre frequency for motor 12
 void AP_InertialSensor::write_notch_log_messages() const
 {
-    const float* notches = get_gyro_dynamic_notch_center_frequencies_hz();
+    for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
-    AP::logger().WriteStreaming(
+        if (!gyro_harmonic_notch_enabled(i)) {
-        "FTN", "TimeUS,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s-zzzzzzzzzzzz", "F-------------", "QBffffffffffff", AP_HAL::micros64(), get_num_gyro_dynamic_notch_center_frequencies(),
+            continue;
+        }
+        const float* notches = get_gyro_dynamic_notch_center_frequencies_hz(i);
+        if (notches == nullptr) {
+            continue;
+        }
+        AP::logger().WriteStreaming(
+            "FTN", "TimeUS,I,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s#-zzzzzzzzzzzz", "F--------------", "QBBffffffffffff",
+            AP_HAL::micros64(),
+            i,
+            get_num_gyro_dynamic_notch_center_frequencies(i),
             notches[0], notches[1], notches[2], notches[3],
             notches[4], notches[5], notches[6], notches[7],
             notches[8], notches[9], notches[10], notches[11]);
+    }
 }