Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: protect against low airspeed in reset

This commit is contained in:
Iampete1 2023-02-19 17:13:36 +00:00 committed by Randy Mackay
parent ac395450cd
commit 969ee8be2c
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_TECS/AP_TECS.cpp
View File

@ -383,9 +383,13 @@ void AP_TECS::_update_speed(float load_factor)
_TASmin = _TAS_dem;
}
// limit the airspeed to a minimum of 3 m/s
const float min_airspeed = 3.0;
// Reset states of time since last update is too large
if (DT > 1.0f) {
_TAS_state = (_EAS * EAS2TAS);
_TAS_state = MAX(_TAS_state, min_airspeed);
_integDTAS_state = 0.0f;
DT = 0.1f; // when first starting TECS, use a
// small time constant
@ -411,8 +415,7 @@ void AP_TECS::_update_speed(float load_factor)
_integDTAS_state = _integDTAS_state + integDTAS_input * DT;
float TAS_input = _integDTAS_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
_TAS_state = _TAS_state + TAS_input * DT;
// limit the airspeed to a minimum of 3 m/s
_TAS_state = MAX(_TAS_state, 3.0f);
_TAS_state = MAX(_TAS_state, min_airspeed);
}