diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 8bbd44c98c..bb9d55b1e0 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/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) { diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index 2d87e86d8a..7791d0820c 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -132,6 +133,9 @@ private: hrt_abstime _last_run{0}; + static constexpr float _kAirspeedFilterTimeConstant{1.f}; + AlphaFilter _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); };