fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 18:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Nick
2025-11-26 12:38:17 +01:00
committed by GitHub
parent d97a8d7d3b
commit 93d767ab51
2 changed files with 14 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_rate_control/FixedwingRateControl.cpp
+9 -2
View File
@@ -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(); _airspeed_validated_sub.update();
const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s) 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 */ /* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s); 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 { } else {
// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible // 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 // 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) { 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 */ /* reset integrals where needed */
if (_rates_sp.reset_integral) { if (_rates_sp.reset_integral) {
src/modules/fw_rate_control/FixedwingRateControl.hpp
+5 -1
View File
@@ -37,6 +37,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <lib/parameters/param.h> #include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <lib/slew_rate/SlewRate.hpp> #include <lib/slew_rate/SlewRate.hpp>
@@ -132,6 +133,9 @@ private:
hrt_abstime _last_run{0}; hrt_abstime _last_run{0};
static constexpr float _kAirspeedFilterTimeConstant{1.f};
AlphaFilter<float> _airspeed_filter_for_torque_scaling{_kAirspeedFilterTimeConstant};
float _airspeed_scaling{1.0f}; float _airspeed_scaling{1.0f};
bool _landed{true}; bool _landed{true};
@@ -219,5 +223,5 @@ private:
void vehicle_manual_poll(); void vehicle_manual_poll();
void vehicle_land_detected_poll(); void vehicle_land_detected_poll();
float get_airspeed_and_update_scaling(); float get_airspeed_and_update_scaling(float dt);
}; };