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

tecs: zero guard ste rates and airspeed setpoint rates

Browse Source
This commit is contained in:
Thomas Stastny 2022-07-12 13:56:30 +02:00 committed by Daniel Agar
parent 63e4ea23b7
commit 1782f9cd3e
2 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -168,9 +168,8 @@ void TECS::_update_speed_setpoint()
// Calculate limits for the demanded rate of change of speed based on physical performance limits
// with a 50% margin to allow the total energy controller to correct for errors.
// NOTE: at zero airspeed, the true airspeed rate setpoint is unbounded
const float max_tas_rate_sp = 0.5f * _STE_rate_max / _tas_state;
const float min_tas_rate_sp = 0.5f * _STE_rate_min / _tas_state;
const float max_tas_rate_sp = 0.5f * _STE_rate_max / math::max(_tas_state, FLT_EPSILON);
const float min_tas_rate_sp = 0.5f * _STE_rate_min / math::max(_tas_state, FLT_EPSILON);
_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
@ -548,10 +547,10 @@ void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altit
void TECS::_update_STE_rate_lim()
{
// Calculate the specific total energy upper rate limits from the max throttle climb rate
_STE_rate_max = _max_climb_rate * CONSTANTS_ONE_G;
_STE_rate_max = math::max(_max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
// Calculate the specific total energy lower rate limits from the min throttle sink rate
_STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G;
_STE_rate_min = - math::max(_min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
}
void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,

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

@ -291,8 +291,8 @@ private:
// vehicle physical limits
float _pitch_setpoint_unc{0.0f}; ///< pitch demand before limiting (rad)
float _STE_rate_max{0.0f}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
float _STE_rate_min{0.0f}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
float _STE_rate_max{FLT_EPSILON}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
float _STE_rate_min{-FLT_EPSILON}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
float _throttle_setpoint_max{0.0f}; ///< normalised throttle upper limit
float _throttle_setpoint_min{0.0f}; ///< normalised throttle lower limit
float _throttle_trim{0.0f}; ///< throttle required to fly level at _EAS_setpoint, compensated for air density and vehicle weight