Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

TECS: in _calcPitchControlOutput guard against invalid airspeed inputs

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-12-11 12:10:12 +01:00
committed by Matthias Grob
parent 9db86f7f0a
commit 91ab09ebd5
1 changed files with 8 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+8 -1
View File
@@ -481,8 +481,15 @@ void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const Co
float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param, float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
const Flag &flag) const const Flag &flag) const
{ {
float airspeed_for_seb_rate = param.equivalent_airspeed_trim;
// avoid division by zero by checking if airspeed is finite and greater than zero
if (flag.airspeed_enabled && PX4_ISFINITE(input.tas) && input.tas > FLT_EPSILON) {
airspeed_for_seb_rate = input.tas;
}
// Calculate derivative from change in climb angle to rate of change of specific energy balance // Calculate derivative from change in climb angle to rate of change of specific energy balance
const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G; const float climb_angle_to_SEB_rate = airspeed_for_seb_rate * CONSTANTS_ONE_G;
// Calculate a specific energy correction that doesn't include the integrator contribution // Calculate a specific energy correction that doesn't include the integrator contribution
float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain + float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain +