Airspeed Filter: add filter to airspeed for scaling (#25908)

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -150,7 +150,7 @@ FixedwingRateControl::vehicle_land_detected_poll()
 	}
 }
 
-float FixedwingRateControl::get_airspeed_and_update_scaling()
+float FixedwingRateControl::get_airspeed_and_update_scaling(float dt)
 {
 	_airspeed_validated_sub.update();
 	const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s)
@@ -163,6 +163,13 @@ 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);
 
+		if (dt > 1.f) {
+			_airspeed_filter_for_torque_scaling.reset(airspeed);
+
+		} else {
+			airspeed = _airspeed_filter_for_torque_scaling.update(airspeed, dt);
+		}
+
 	} 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
@@ -274,7 +281,7 @@ void FixedwingRateControl::Run()
 
 		if (_vcontrol_mode.flag_control_rates_enabled) {
 
-			const float airspeed = get_airspeed_and_update_scaling();
+			const float airspeed = get_airspeed_and_update_scaling(dt);
 
 			/* reset integrals where needed */
 			if (_rates_sp.reset_integral) {

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>
@@ -132,6 +133,9 @@ private:
 
 	hrt_abstime _last_run{0};
 
+	static constexpr float _kAirspeedFilterTimeConstant{1.f};
+	AlphaFilter<float> _airspeed_filter_for_torque_scaling{_kAirspeedFilterTimeConstant};
+
 	float _airspeed_scaling{1.0f};
 
 	bool _landed{true};
@@ -219,5 +223,5 @@ private:
 	void		vehicle_manual_poll();
 	void		vehicle_land_detected_poll();
 
-	float 		get_airspeed_and_update_scaling();
+	float 		get_airspeed_and_update_scaling(float dt);
 };