fw-rate-ctrl: filter airspeed before scaling servo torques

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

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