AP_InertialSensor: Rework notch filter allocation and sampling

https://github.com/ArduPilot/ardupilot/issues/11346

Allocate a notch filter per-IMU.
Update the notch filters in the backend at the sensor sample rate.
Allow raw logging of post-filtered gyro and accel values.

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -439,7 +439,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
 
     // @Group: NOTCH_
     // @Path: ../Filter/NotchFilter.cpp
-    AP_SUBGROUPINFO(_notch_filter, "NOTCH_",  37, AP_InertialSensor, NotchFilterVector3fParam),
+    AP_SUBGROUPINFO(_notch_filter, "NOTCH_",  37, AP_InertialSensor, NotchFilterParams),
 
     // @Group: LOG_
     // @Path: ../AP_InertialSensor/BatchSampler.cpp
@@ -646,8 +646,6 @@ AP_InertialSensor::init(uint16_t sample_rate)
 
     _sample_period_usec = 1000*1000UL / _sample_rate;
 
-    _notch_filter.init(sample_rate);
-    
     // establish the baseline time between samples
     _delta_time = 0;
     _next_sample_usec = 0;
@@ -1339,9 +1337,6 @@ void AP_InertialSensor::update(void)
         }
     }
 
-    // apply notch filter to primary gyro
-    _gyro[_primary_gyro] = _notch_filter.apply(_gyro[_primary_gyro]);
-    
     _last_update_usec = AP_HAL::micros();
     
     _have_sample = false;

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -294,6 +294,7 @@ public:
         void periodic();
 
         bool doing_sensor_rate_logging() const { return _doing_sensor_rate_logging; }
+        bool doing_post_filter_logging() const { return _doing_post_filter_logging; }
 
         // class level parameters
         static const struct AP_Param::GroupInfo var_info[];
@@ -320,6 +321,7 @@ public:
 
         enum batch_opt_t {
             BATCH_OPT_SENSOR_RATE = (1<<0),
+            BATCH_OPT_POST_FILTER = (1<<1),
         };
 
         void rotate_to_next_sensor();
@@ -333,6 +335,7 @@ public:
         bool initialised : 1;
         bool isbh_sent : 1;
         bool _doing_sensor_rate_logging : 1;
+        bool _doing_post_filter_logging : 1;
         uint8_t instance : 3; // instance we are sending data for
         AP_InertialSensor::IMU_SENSOR_TYPE type : 1;
         uint16_t isb_seqnum;
@@ -402,7 +405,8 @@ private:
     bool _new_gyro_data[INS_MAX_INSTANCES];
 
     // optional notch filter on gyro
-    NotchFilterVector3fParam _notch_filter;
+    NotchFilterParams _notch_filter;
+    NotchFilterVector3f _gyro_notch_filter[INS_MAX_INSTANCES];
 
     // Most recent gyro reading
     Vector3f _gyro[INS_MAX_INSTANCES];

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -204,14 +204,25 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
         _imu._last_delta_angle[instance] = delta_angle;
         _imu._last_raw_gyro[instance] = gyro;
 
+        // apply the low pass filter
         _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
+        // apply the notch filter
+        if (_gyro_notch_enabled()) {
+            _imu._gyro_filtered[instance] = _imu._gyro_notch_filter[instance].apply(gyro);
+        }
         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._new_gyro_data[instance] = true;
     }
 
-    log_gyro_raw(instance, sample_us, gyro);
+    if (!_imu.batchsampler.doing_post_filter_logging()) {
+        log_gyro_raw(instance, sample_us, gyro);
+    }
+    else {
+        log_gyro_raw(instance, sample_us, _imu._gyro_filtered[instance]);
+    }
 }
 
 void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gyro)
@@ -326,7 +337,11 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
         _imu._new_accel_data[instance] = true;
     }
 
-    log_accel_raw(instance, sample_us, accel);
+    if (!_imu.batchsampler.doing_post_filter_logging()) {
+        log_accel_raw(instance, sample_us, accel);
+    } else {
+        log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
+    }
 }
 
 void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel)
@@ -438,6 +453,15 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
         _imu._gyro_filter[instance].set_cutoff_frequency(_gyro_raw_sample_rate(instance), _gyro_filter_cutoff());
         _last_gyro_filter_hz[instance] = _gyro_filter_cutoff();
     }
+    // possily update the notch filter parameters
+    if (_last_notch_center_freq_hz[instance] != _gyro_notch_center_freq_hz() ||
+        _last_notch_bandwidth_hz[instance] != _gyro_notch_bandwidth_hz() ||
+        !is_equal(_last_notch_attenuation_dB[instance], _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_bandwidth_hz[instance] = _gyro_notch_bandwidth_hz();
+        _last_notch_attenuation_dB[instance] = _gyro_notch_attenuation_dB();
+    }
 }
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -232,6 +232,17 @@ protected:
     // return the requested sample rate in Hz
     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(); }
+
+    // return the notch filter bandwidth in Hz for the sample rate
+    uint8_t _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(); }
+
     // common gyro update function for all backends
     void update_gyro(uint8_t instance);
 
@@ -239,8 +250,11 @@ protected:
     void update_accel(uint8_t instance);
 
     // support for updating filter at runtime
-    int8_t _last_accel_filter_hz[INS_MAX_INSTANCES];
-    int8_t _last_gyro_filter_hz[INS_MAX_INSTANCES];
+    uint8_t _last_accel_filter_hz[INS_MAX_INSTANCES];
+    uint8_t _last_gyro_filter_hz[INS_MAX_INSTANCES];
+    uint16_t _last_notch_center_freq_hz[INS_MAX_INSTANCES];
+    uint8_t _last_notch_bandwidth_hz[INS_MAX_INSTANCES];
+    float _last_notch_attenuation_dB[INS_MAX_INSTANCES];
 
     void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
         _imu._gyro_orientation[instance] = rotation;

libraries/AP_InertialSensor/BatchSampler.cpp

@@ -22,7 +22,7 @@ const AP_Param::GroupInfo AP_InertialSensor::BatchSampler::var_info[] = {
     // @Param: BAT_OPT
     // @DisplayName: Batch Logging Options Mask
     // @Description: Options for the BatchSampler
-    // @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP)
+    // @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP), 1: Sample post-filtering
     // @User: Advanced
     AP_GROUPINFO("BAT_OPT",  3, AP_InertialSensor::BatchSampler, _batch_options_mask, 0),
 
@@ -87,8 +87,14 @@ void AP_InertialSensor::BatchSampler::periodic()
 
 void AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging()
 {
+    if (((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_POST_FILTER)
+        && type == IMU_SENSOR_TYPE_GYRO) {
+        _doing_post_filter_logging = true;
+        return;
+    }
     if (!((batch_opt_t)(_batch_options_mask.get()) & BATCH_OPT_SENSOR_RATE)) {
         _doing_sensor_rate_logging = false;
+        _doing_post_filter_logging = false;
         return;
     }
     const uint8_t bit = (1<<instance);