AP_InertialSensor: add harmonic notch filter to gyro filter chain

Allow dynamic updates to the calculated frequency. Convert bandwidth and frequency to floats.
backend variables do not need to be indexed per-backed

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -453,6 +453,10 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
     // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
     AP_GROUPINFO("ENABLE_MASK",  40, AP_InertialSensor, _enable_mask, 0x7F),
 
+    // @Group: HNTCH_
+    // @Path: ../Filter/HarmonicNotchFilter.cpp
+    AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_",  41, AP_InertialSensor, HarmonicNotchFilterParams),
+
     /*
       NOTE: parameter indexes have gaps above. When adding new
       parameters check for conflicts carefully
@@ -471,6 +475,7 @@ AP_InertialSensor::AP_InertialSensor() :
     }
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
+
     for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
         _gyro_cal_ok[i] = true;
         _accel_max_abs_offsets[i] = 3.5f;
@@ -654,6 +659,15 @@ AP_InertialSensor::init(uint16_t sample_rate)
 
     // initialise IMU batch logging
     batchsampler.init();
+
+    // 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 = _harmonic_notch_filter.center_freq_hz();
+
+    for (uint8_t i=0; i<get_gyro_count(); i++) {
+        _gyro_harmonic_notch_filter[i].create(_harmonic_notch_filter.harmonics());
+    }
 }
 
 bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
@@ -1709,6 +1723,14 @@ void AP_InertialSensor::acal_update()
     }
 }
 
+// Update the harmonic notch frequency
+void AP_InertialSensor::update_harmonic_notch_freq_hz(float scaled_freq) {
+    // When disarmed, throttle is zero
+    if (is_positive(scaled_freq)) {
+        _calculated_harmonic_notch_freq_hz = scaled_freq;
+    }
+}
+
 /*
     set and save accelerometer bias along with trim calculation
 */

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -25,6 +25,7 @@
 #include <Filter/LowPassFilter2p.h>
 #include <Filter/LowPassFilter.h>
 #include <Filter/NotchFilter.h>
+#include <Filter/HarmonicNotchFilter.h>
 
 class AP_InertialSensor_Backend;
 class AuxiliaryBus;
@@ -202,6 +203,9 @@ public:
     uint8_t get_primary_accel(void) const { return _primary_accel; }
     uint8_t get_primary_gyro(void) const { return _primary_gyro; }
 
+    // Update the harmonic notch frequency
+    void update_harmonic_notch_freq_hz(float scaled_freq);
+
     // enable HIL mode
     void set_hil_mode(void) { _hil_mode = true; }
 
@@ -211,6 +215,15 @@ public:
     // get the accel filter rate in Hz
     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; }
+
+    // harmonic notch reference center frequency
+    float get_gyro_harmonic_notch_center_freq_hz(void) const { return _harmonic_notch_filter.center_freq_hz(); }
+
+    // harmonic notch reference scale factor
+    float get_gyro_harmonic_notch_reference(void) const { return _harmonic_notch_filter.reference(); }
+
     // indicate which bit in LOG_BITMASK indicates raw logging enabled
     void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
 
@@ -411,6 +424,12 @@ private:
     NotchFilterParams _notch_filter;
     NotchFilterVector3f _gyro_notch_filter[INS_MAX_INSTANCES];
 
+    // optional harmonic notch filter on gyro
+    HarmonicNotchFilterParams _harmonic_notch_filter;
+    HarmonicNotchFilterVector3f _gyro_harmonic_notch_filter[INS_MAX_INSTANCES];
+    // the current center frequency for the notch
+    float _calculated_harmonic_notch_freq_hz;
+
     // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];
     Vector3f _delta_angle[INS_MAX_INSTANCES];

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -226,6 +226,12 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 
         // apply the low pass filter
         _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
+
+        // apply the harmonic notch filter
+        if (_gyro_harmonic_notch_enabled()) {
+            _imu._gyro_filtered[instance] = _imu._gyro_harmonic_notch_filter[instance].apply(_imu._gyro_filtered[instance]);
+        }
+
         // apply the notch filter
         if (_gyro_notch_enabled()) {
             _imu._gyro_filtered[instance] = _imu._gyro_notch_filter[instance].apply(_imu._gyro_filtered[instance]);
@@ -233,6 +239,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) {
             _imu._gyro_filter[instance].reset();
             _imu._gyro_notch_filter[instance].reset();
+            _imu._gyro_harmonic_notch_filter[instance].reset();
         }
         _imu._new_gyro_data[instance] = true;
     }
@@ -493,18 +500,31 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
     }
 
     // possibly update filter frequency
-    if (_last_gyro_filter_hz[instance] != _gyro_filter_cutoff()) {
+    if (_last_gyro_filter_hz != _gyro_filter_cutoff()) {
         _imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
-        _last_gyro_filter_hz[instance] = _gyro_filter_cutoff();
+        _last_gyro_filter_hz = _gyro_filter_cutoff();
+    }
+
+    // possily update the harmonic notch filter parameters
+    if (!is_equal(_last_harmonic_notch_bandwidth_hz, _gyro_harmonic_notch_bandwidth_hz()) ||
+        !is_equal(_last_harmonic_notch_attenuation_dB, _gyro_harmonic_notch_attenuation_dB())) {
+        _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());
+        _last_harmonic_notch_center_freq_hz = _gyro_harmonic_notch_center_freq_hz();
+        _last_harmonic_notch_bandwidth_hz = _gyro_harmonic_notch_bandwidth_hz();
+        _last_harmonic_notch_attenuation_dB = _gyro_harmonic_notch_attenuation_dB();
+    }
+    else if (!is_equal(_last_harmonic_notch_center_freq_hz, _gyro_harmonic_notch_center_freq_hz())) {
+        _imu._gyro_harmonic_notch_filter[instance].update(_gyro_harmonic_notch_center_freq_hz());
+        _last_harmonic_notch_center_freq_hz = _gyro_harmonic_notch_center_freq_hz();
     }
     // possily update the notch filter parameters
-    if (_last_notch_center_freq_hz[instance] != _gyro_notch_center_freq_hz() ||
+    if (!is_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) ||
-        _last_notch_bandwidth_hz[instance] != _gyro_notch_bandwidth_hz() ||
+        !is_equal(_last_notch_bandwidth_hz, _gyro_notch_bandwidth_hz()) ||
-        !is_equal(_last_notch_attenuation_dB[instance], _gyro_notch_attenuation_dB())) {
+        !is_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB())) {
         _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[instance] = _gyro_notch_center_freq_hz();
+        _last_notch_center_freq_hz = _gyro_notch_center_freq_hz();
-        _last_notch_bandwidth_hz[instance] = _gyro_notch_bandwidth_hz();
+        _last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz();
-        _last_notch_attenuation_dB[instance] = _gyro_notch_attenuation_dB();
+        _last_notch_attenuation_dB = _gyro_notch_attenuation_dB();
     }
 }
 
@@ -524,9 +544,9 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance)
     }
     
     // possibly update filter frequency
-    if (_last_accel_filter_hz[instance] != _accel_filter_cutoff()) {
+    if (_last_accel_filter_hz != _accel_filter_cutoff()) {
         _imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());
-        _last_accel_filter_hz[instance] = _accel_filter_cutoff();
+        _last_accel_filter_hz = _accel_filter_cutoff();
     }
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -75,7 +75,7 @@ public:
 
     // notify of a fifo reset
     void notify_fifo_reset(void);
-    
+
     /*
       device driver IDs. These are used to fill in the devtype field
       of the device ID, which shows up as INS*ID* parameters to
@@ -179,7 +179,7 @@ protected:
 
     // update the sensor rate for FIFO sensors
     void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const;
-    
+
     // set accelerometer max absolute offset for calibration
     void _set_accel_max_abs_offset(uint8_t instance, float offset);
 
@@ -233,16 +233,27 @@ protected:
     uint16_t get_sample_rate_hz(void) const;
 
     // return the notch filter center in Hz for the sample rate
-    uint16_t _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); }
+    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
-    uint16_t _gyro_notch_bandwidth_hz(void) const { return _imu._notch_filter.bandwidth_hz(); }
+    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(); }
 
     uint8_t _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._calculated_harmonic_notch_freq_hz; }
+
+    // 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(); }
+
+    // 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(); }
+
+    uint8_t _gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); }
+
     // common gyro update function for all backends
     void update_gyro(uint8_t instance);
 
@@ -250,12 +261,17 @@ protected:
     void update_accel(uint8_t instance);
 
     // support for updating filter at runtime
-    uint16_t _last_accel_filter_hz[INS_MAX_INSTANCES];
+    uint16_t _last_accel_filter_hz;
-    uint16_t _last_gyro_filter_hz[INS_MAX_INSTANCES];
+    uint16_t _last_gyro_filter_hz;
-    uint16_t _last_notch_center_freq_hz[INS_MAX_INSTANCES];
+    float _last_notch_center_freq_hz;
-    uint16_t _last_notch_bandwidth_hz[INS_MAX_INSTANCES];
+    float _last_notch_bandwidth_hz;
-    float _last_notch_attenuation_dB[INS_MAX_INSTANCES];
+    float _last_notch_attenuation_dB;
-
+
+    // support for updating harmonic filter at runtime
+    float _last_harmonic_notch_center_freq_hz;
+    float _last_harmonic_notch_bandwidth_hz;
+    float _last_harmonic_notch_attenuation_dB;
+    
     void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
         _imu._gyro_orientation[instance] = rotation;
     }