From 6856ee0a672ffe7aa4d99855106a8e5e5034b9c7 Mon Sep 17 00:00:00 2001
From: mahima-yoga <mahima@auterion.com>
Date: Tue, 15 Jul 2025 10:56:29 +0200
Subject: [PATCH] fw-rate-ctrl: filter airspeed before scaling servo torques

---
 .../fw_rate_control/FixedwingRateControl.cpp        | 13 +++++++++++++
 .../fw_rate_control/FixedwingRateControl.hpp        |  3 +++
 2 files changed, 16 insertions(+)

diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp
index d7de4e4741..474bc1597a 100644
--- a/src/modules/fw_rate_control/FixedwingRateControl.cpp
+++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp
@@ -163,6 +163,19 @@ float FixedwingRateControl::get_airspeed_and_update_scaling()
 		/* prevent numerical drama by requiring 0.5 m/s minimal speed */
 		airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);
 
+		const float timestamp = _airspeed_validated_sub.get().timestamp;
+		const float dt = static_cast<float>(timestamp - _last_airspeed_update) * 1e-6f;
+
+		_last_airspeed_update = timestamp;
+
+		if (dt < FLT_EPSILON || dt > 1.f) {
+			_airspeed_filter_for_torque_scaling.reset(airspeed);
+
+		} else {
+			_airspeed_filter_for_torque_scaling.setParameters(dt, 1.f);
+			_airspeed_filter_for_torque_scaling.update(airspeed);
+		}
+
 	} else {
 		// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
 		// this assumption is good as long as the vehicle is not hovering in a headwind which is much larger
diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp
index 2d87e86d8a..ce2d4b324a 100644
--- a/src/modules/fw_rate_control/FixedwingRateControl.hpp
+++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp
@@ -37,6 +37,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/slew_rate/SlewRate.hpp>
@@ -131,7 +132,9 @@ private:
 	perf_counter_t _loop_perf;
 
 	hrt_abstime _last_run{0};
+	hrt_abstime _last_airspeed_update{0};
 
+	AlphaFilter<float> _airspeed_filter_for_torque_scaling;
 	float _airspeed_scaling{1.0f};
 
 	bool _landed{true};