fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/fw_rate_control/FixedwingRateControl.cpp
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();
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) {

6
src/modules/fw_rate_control/FixedwingRateControl.hpp
View File

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