sensors: add 2nd static notch and migrate existing parameters

- new static notch filter configured via IMU_GYRO_NF1_FRQ and IMU_GYRO_NF1_BW
 - existing notch parameters IMU_GYRO_NF_FREQ and IMU_GYRO_NF_BW become
IMU_GYRO_NF0_FRQ and IMU_GYRO_NF0_BW

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

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;
 }

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(&param_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)

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -132,7 +132,8 @@ private:
 
 	// angular velocity filters
 	math::LowPassFilter2p<float> _lp_filter_velocity[3] {};
-	math::NotchFilter<float> _notch_filter_velocity[3] {};
+	math::NotchFilter<float> _notch_filter0_velocity[3] {};
+	math::NotchFilter<float> _notch_filter1_velocity[3] {};
 
 #if !defined(CONSTRAINED_FLASH)
 
@@ -188,8 +189,10 @@ private:
 		(ParamFloat<px4::params::IMU_GYRO_DNF_BW>) _param_imu_gyro_dnf_bw,
 #endif // !CONSTRAINED_FLASH
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
-		(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
-		(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
+		(ParamFloat<px4::params::IMU_GYRO_NF0_FRQ>) _param_imu_gyro_nf0_frq,
+		(ParamFloat<px4::params::IMU_GYRO_NF0_BW>) _param_imu_gyro_nf0_bw,
+		(ParamFloat<px4::params::IMU_GYRO_NF1_FRQ>) _param_imu_gyro_nf1_frq,
+		(ParamFloat<px4::params::IMU_GYRO_NF1_BW>) _param_imu_gyro_nf1_bw,
 		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
 		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
 	)

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