Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: protect against low airspeed in reset

Browse Source
This commit is contained in:
Iampete1 2023-02-19 17:13:36 +00:00 committed by Andrew Tridgell
parent 5accaeb0bf
commit f61d482fde
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; _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 // Reset states of time since last update is too large
if (DT > 1.0f) { if (DT > 1.0f) {
_TAS_state = (_EAS * EAS2TAS); _TAS_state = (_EAS * EAS2TAS);
_TAS_state = MAX(_TAS_state, min_airspeed);
_integDTAS_state = 0.0f; _integDTAS_state = 0.0f;
DT = 0.1f; // when first starting TECS, use a DT = 0.1f; // when first starting TECS, use a
// small time constant // small time constant
@ -411,8 +415,7 @@ void AP_TECS::_update_speed(float load_factor)
_integDTAS_state = _integDTAS_state + integDTAS_input * DT; _integDTAS_state = _integDTAS_state + integDTAS_input * DT;
float TAS_input = _integDTAS_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f; float TAS_input = _integDTAS_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
_TAS_state = _TAS_state + TAS_input * DT; _TAS_state = _TAS_state + TAS_input * DT;
// limit the airspeed to a minimum of 3 m/s _TAS_state = MAX(_TAS_state, min_airspeed);
_TAS_state = MAX(_TAS_state, 3.0f);
} }