diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering index b122fd389c..b64c8cc5d9 100644 --- a/posix-configs/SITL/init/test/test_imu_filtering +++ b/posix-configs/SITL/init/test/test_imu_filtering @@ -28,8 +28,8 @@ param set IMU_GYRO_DNF_EN 3 # test values param set IMU_GYRO_CUTOFF 60 param set IMU_DGYRO_CUTOFF 40 -#param set IMU_GYRO_NF_FREQ 60 -#param set IMU_GYRO_NF_BW 5 +#param set IMU_GYRO_NF0_FRQ 60 +#param set IMU_GYRO_NF0_BW 5 # log nearly everything param set SDLOG_PROFILE 859 diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 196bbba508..d024a303b9 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -163,5 +163,20 @@ bool param_modify_on_import(bson_node_t node) } } + // 2022-03-15: translate notch filter IMU_GYRO_NF_FREQ to IMU_GYRO_NF0_FRQ and IMU_GYRO_NF_BW -> IMU_GYRO_NF0_BW + { + if (strcmp("IMU_GYRO_NF_FREQ", node->name) == 0) { + strcpy(node->name, "IMU_GYRO_NF0_FRQ"); + PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_FREQ", "IMU_GYRO_NF0_FRQ"); + return true; + } + + if (strcmp("IMU_GYRO_NF_BW", node->name) == 0) { + strcpy(node->name, "IMU_GYRO_NF0_BW"); + PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_BW", "IMU_GYRO_NF0_BW"); + return true; + } + } + return false; } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 3d8262fbf9..4f52197f6f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -167,10 +167,15 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us) _lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get()); _lp_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis)); - // angular velocity notch - _notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(), - _param_imu_gyro_nf_bw.get()); - _notch_filter_velocity[axis].reset(); + // angular velocity notch 0 + _notch_filter0_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf0_frq.get(), + _param_imu_gyro_nf0_bw.get()); + _notch_filter0_velocity[axis].reset(); + + // angular velocity notch 1 + _notch_filter1_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf1_frq.get(), + _param_imu_gyro_nf1_bw.get()); + _notch_filter1_velocity[axis].reset(); // angular acceleration low pass if ((_param_imu_dgyro_cutoff.get() > 0.f) @@ -360,11 +365,13 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); - const bool nf_enabled_prev = (_param_imu_gyro_nf_freq.get() > 0.f) && (_param_imu_gyro_nf_bw.get() > 0.f); + const bool nf0_enabled_prev = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f); + const bool nf1_enabled_prev = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f); updateParams(); - const bool nf_enabled = (_param_imu_gyro_nf_freq.get() > 0.f) && (_param_imu_gyro_nf_bw.get() > 0.f); + const bool nf0_enabled = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f); + const bool nf1_enabled = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f); _calibration.ParametersUpdate(); @@ -376,12 +383,23 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) } } - // gyro notch filter frequency or bandwidth changed - for (auto &nf : _notch_filter_velocity) { - const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f); - const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f); + // gyro notch filter 0 frequency or bandwidth changed + for (auto &nf : _notch_filter0_velocity) { + const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf0_frq.get()) > 0.01f); + const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf0_bw.get()) > 0.01f); - if ((nf_enabled_prev != nf_enabled) || (nf_enabled && (nf_freq_changed || nf_bw_changed))) { + if ((nf0_enabled_prev != nf0_enabled) || (nf0_enabled && (nf_freq_changed || nf_bw_changed))) { + _reset_filters = true; + break; + } + } + + // gyro notch filter 1 frequency or bandwidth changed + for (auto &nf : _notch_filter1_velocity) { + const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf1_frq.get()) > 0.01f); + const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf1_bw.get()) > 0.01f); + + if ((nf1_enabled_prev != nf1_enabled) || (nf1_enabled && (nf_freq_changed || nf_bw_changed))) { _reset_filters = true; break; } @@ -695,9 +713,14 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int #endif // !CONSTRAINED_FLASH - // Apply general notch filter (IMU_GYRO_NF_FREQ) - if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) { - _notch_filter_velocity[axis].applyArray(data, N); + // Apply general notch filter 0 (IMU_GYRO_NF0_FRQ) + if (_notch_filter0_velocity[axis].getNotchFreq() > 0.f) { + _notch_filter0_velocity[axis].applyArray(data, N); + } + + // Apply general notch filter 1 (IMU_GYRO_NF1_FRQ) + if (_notch_filter1_velocity[axis].getNotchFreq() > 0.f) { + _notch_filter1_velocity[axis].applyArray(data, N); } // Apply general low-pass filter (IMU_GYRO_CUTOFF) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 66b1191a29..45805de73f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -132,7 +132,8 @@ private: // angular velocity filters math::LowPassFilter2p _lp_filter_velocity[3] {}; - math::NotchFilter _notch_filter_velocity[3] {}; + math::NotchFilter _notch_filter0_velocity[3] {}; + math::NotchFilter _notch_filter1_velocity[3] {}; #if !defined(CONSTRAINED_FLASH) @@ -188,8 +189,10 @@ private: (ParamFloat) _param_imu_gyro_dnf_bw, #endif // !CONSTRAINED_FLASH (ParamFloat) _param_imu_gyro_cutoff, - (ParamFloat) _param_imu_gyro_nf_freq, - (ParamFloat) _param_imu_gyro_nf_bw, + (ParamFloat) _param_imu_gyro_nf0_frq, + (ParamFloat) _param_imu_gyro_nf0_bw, + (ParamFloat) _param_imu_gyro_nf1_frq, + (ParamFloat) _param_imu_gyro_nf1_bw, (ParamInt) _param_imu_gyro_ratemax, (ParamFloat) _param_imu_dgyro_cutoff ) diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index 708b8971e6..8564503d54 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -38,7 +38,7 @@ * This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency. * This only affects the signal sent to the controllers, not the estimators. * Applies to both angular velocity and angular acceleration sent to the controllers. -* See "IMU_GYRO_NF_BW" to set the bandwidth of the filter. +* See "IMU_GYRO_NF0_BW" to set the bandwidth of the filter. * * A value of 0 disables the filter. * @@ -48,13 +48,13 @@ * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f); +PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_FRQ, 0.0f); /** * Notch filter bandwidth for gyro * * The frequency width of the stop band for the 2nd order notch filter on the primary gyro. -* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency. +* See "IMU_GYRO_NF0_FRQ" to activate the filter and to set the notch frequency. * Applies to both angular velocity and angular acceleration sent to the controllers. * * @min 0 @@ -63,7 +63,42 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f); +PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_BW, 20.0f); + +/** +* Notch filter 2 frequency for gyro +* +* The center frequency for the 2nd order notch filter on the primary gyro. +* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency. +* This only affects the signal sent to the controllers, not the estimators. +* Applies to both angular velocity and angular acceleration sent to the controllers. +* See "IMU_GYRO_NF1_BW" to set the bandwidth of the filter. +* +* A value of 0 disables the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f); + +/** +* Notch filter 1 bandwidth for gyro +* +* The frequency width of the stop band for the 2nd order notch filter on the primary gyro. +* See "IMU_GYRO_NF1_FRQ" to activate the filter and to set the notch frequency. +* Applies to both angular velocity and angular acceleration sent to the controllers. +* +* @min 0 +* @max 100 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f); + /** * Low pass filter cutoff frequency for gyro