diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 90ea928bfb..a594d02bd5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index b7c1d6e22f..e9525d39e8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index af8e9e4cc0..73f443f670 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/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(); + } } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 7189cad0e6..e28457264f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/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; diff --git a/libraries/AP_InertialSensor/BatchSampler.cpp b/libraries/AP_InertialSensor/BatchSampler.cpp index 69c3636b21..b570c04df6 100644 --- a/libraries/AP_InertialSensor/BatchSampler.cpp +++ b/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<