dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

tecs: convert tas error and soft bounds to percentage of trim airspeed

This commit is contained in:
Thomas Stastny 2022-07-12 14:18:20 +02:00 committed by Daniel Agar
parent 1782f9cd3e
commit a12e40b1d8
2 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/lib/tecs/TECS.cpp
View File

@ -216,8 +216,17 @@ void TECS::_detect_underspeed()
return;
}
const float tas_fully_undersped = math::max(_TAS_min - TAS_ERROR_BOUND - TAS_UNDERSPEED_SOFT_BOUND, 0.0f);
const float tas_starting_to_underspeed = math::max(_TAS_min - TAS_ERROR_BOUND, tas_fully_undersped);
// this is the expected (something like standard) deviation from the airspeed setpoint that we allow the airspeed
// to vary in before ramping in underspeed mitigation
const float tas_error_bound = kTASErrorPercentage * _equivalent_airspeed_trim;
// this is the soft boundary where underspeed mitigation is ramped in
// NOTE: it's currently the same as the error bound, but separated here to indicate these values do not in general
// need to be the same
const float tas_underspeed_soft_bound = kTASErrorPercentage * _equivalent_airspeed_trim;
const float tas_fully_undersped = math::max(_TAS_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f);
const float tas_starting_to_underspeed = math::max(_TAS_min - tas_error_bound, tas_fully_undersped);
_percent_undersped = 1.0f - math::constrain((_tas_state - tas_fully_undersped) /
math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);

8
src/lib/tecs/TECS.hpp
View File

@ -219,12 +219,8 @@ public:
private:
// [m/s] bound on expected (safe) true airspeed tracking errors, including TAS = TAS_min - TAS_ERROR_BOUND
static constexpr float TAS_ERROR_BOUND = 2.0f;
// [m/s] true airspeed soft boundary region below the accepted TAS error region (below TAS_min - TAS_ERROR_BOUND)
// underspeed mitigation measures are ramped in from zero to full within this region
static constexpr float TAS_UNDERSPEED_SOFT_BOUND = 1.5f;
// [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
static constexpr float kTASErrorPercentage = 0.15;
static constexpr float _jerk_max =
1000.0f;