gyro filtering: apply IMU_GYRO_CUTOFF also to D-term

Greatly improves the noise on the d-term on a Kopis 2. Apparently the order
of filtering vs differentiating matters.

Also disables the DGYRO filter by default, as the D-term now already has
a default filter of 30Hz applied.

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -282,19 +282,22 @@ void VehicleAngularVelocity::Run()
 			// correct for in-run bias errors
 			const Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias;
 
-			// Differentiate angular velocity (after notch filter)
+			// Gyro filtering:
+			// - Apply general notch filter (IMU_GYRO_NF_FREQ)
+			// - Apply general low-pass filter (IMU_GYRO_CUTOFF)
+			// - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
+
 			const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};
-			const Vector3f angular_acceleration_raw = (angular_velocity_notched - _angular_velocity_prev) / dt;
 
-			_angular_velocity_prev = angular_velocity_notched;
+			const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
+
+			const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
+			_angular_velocity_prev = angular_velocity;
 			_angular_acceleration_prev = angular_acceleration_raw;
+			const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
 
 			CheckFilters();
 
-			// Filter: apply low-pass
-			const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
-			const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
-
 			// publish once all new samples are processed
 			sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();
 

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
 *
 * The cutoff frequency for the 2nd order butterworth filter on the primary gyro.
 * This only affects the angular velocity sent to the controllers, not the estimators.
-* Doesn't apply to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF.
+* It applies also to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF.
 *
 * A value of 0 disables the filter.
 *
@@ -109,7 +109,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
 * the time derivative of the measured angular velocity, also known as
 * the D-term filter in the rate controller. The D-term uses the derivative of
 * the rate and thus is the most susceptible to noise. Therefore, using
-* a D-term filter allows to decrease the driver-level filtering, which
+* a D-term filter allows to increase IMU_GYRO_CUTOFF, which
 * leads to reduced control latency and permits to increase the P gains.
 *
 * A value of 0 disables the filter.
@@ -120,4 +120,4 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
 * @reboot_required true
 * @group Sensors
 */
-PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f);
+PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 0.0f);