[Multirotor] add yaw torque low pass filter (#24173)

co-authored-by: danielmellinger <107884356+danielmellinger@users.noreply.github.com>
co-authored-by: Eric Katzfey <eric.katzfey@modalai.com>

src/lib/mathlib/math/filter/AlphaFilter.hpp

@@ -89,6 +89,16 @@ public:
 		return true;
 	}
 
+	void setCutoffFreq(float cutoff_freq)
+	{
+		if (cutoff_freq > FLT_EPSILON) {
+			_time_constant = 1.f / (M_TWOPI_F * cutoff_freq);
+
+		} else {
+			_time_constant = 0.f;
+		}
+	}
+
 	/**
 	 * Set filter parameter alpha directly without time abstraction
 	 *

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -95,6 +95,8 @@ MulticopterRateControl::parameters_updated()
 	// manual rate control acro mode rate limits
 	_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
 				  radians(_param_mc_acro_y_max.get()));
+
+	_output_lpf_yaw.setCutoffFreq(_param_mc_yaw_tq_cutoff.get());
 }
 
 void
@@ -214,9 +216,12 @@ MulticopterRateControl::Run()
 			}
 
 			// run rate controller
-			const Vector3f torque_setpoint =
+			Vector3f torque_setpoint =
 				_rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed);
 
+			// apply low-pass filtering on yaw axis to reduce high frequency torque caused by rotor acceleration
+			torque_setpoint(2) = _output_lpf_yaw.update(torque_setpoint(2), dt);
+
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
 			_rate_control.getRateControlStatus(rate_ctrl_status);

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -34,6 +34,7 @@
 #pragma once
 
 #include <lib/rate_control/rate_control.hpp>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
@@ -129,6 +130,8 @@ private:
 	float _energy_integration_time{0.0f};
 	float _control_energy[4] {};
 
+	AlphaFilter<float> _output_lpf_yaw;
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
 		(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
@@ -150,6 +153,7 @@ private:
 		(ParamFloat<px4::params::MC_YAWRATE_D>) _param_mc_yawrate_d,
 		(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
 		(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
+		(ParamFloat<px4::params::MC_YAW_TQ_CUTOFF>) _param_mc_yaw_tq_cutoff,
 
 		(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
 		(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,

src/modules/mc_rate_control/mc_rate_control_params.c

@@ -292,3 +292,17 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f);
  * @group Multicopter Rate Control
  */
 PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
+
+/**
+ * Low pass filter cutoff frequency for yaw torque setpoint
+ *
+ * Reduces vibrations by lowering high frequency torque caused by rotor acceleration.
+ * 0 disables the filter
+ *
+ * @min 0
+ * @max 10
+ * @unit Hz
+ * @decimal 3
+ * @group Multicopter Rate Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAW_TQ_CUTOFF, 2.f);