From 8ecb76aba22a54e803edfedb9145290b92b3544f Mon Sep 17 00:00:00 2001
From: Mathieu Bresciani <brescianimathieu@gmail.com>
Date: Wed, 15 Jan 2025 14:02:28 +0100
Subject: [PATCH] [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        | 10 ++++++++++
 .../mc_rate_control/MulticopterRateControl.cpp     |  7 ++++++-
 .../mc_rate_control/MulticopterRateControl.hpp     |  4 ++++
 .../mc_rate_control/mc_rate_control_params.c       | 14 ++++++++++++++
 4 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp
index b7a6e86953a8..b80e2a73f593 100644
--- a/src/lib/mathlib/math/filter/AlphaFilter.hpp
+++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp
@@ -89,6 +89,16 @@ class AlphaFilter
 		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
 	 *
diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp
index fea041022e3e..39311252892c 100644
--- a/src/modules/mc_rate_control/MulticopterRateControl.cpp
+++ b/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);
diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp
index 0f402c12c3e3..1a2427da18e1 100644
--- a/src/modules/mc_rate_control/MulticopterRateControl.hpp
+++ b/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 @@ class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public
 	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 @@ class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public
 		(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,
diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c
index 2571a70c1f40..b37c52e746ed 100644
--- a/src/modules/mc_rate_control/mc_rate_control_params.c
+++ b/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);