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 Tom Pittenger
parent f7317966dc
commit 6d552a32d7
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

@ -430,9 +430,13 @@ void AP_TECS::_update_speed(float DT)
_EAS = constrain_float(0.01f * (float)aparm.airspeed_cruise_cm.get(), (float)aparm.airspeed_min.get(), (float)aparm.airspeed_max.get());
}
// 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 (_flags.reset) {
_TAS_state = (_EAS * EAS2TAS);
_TAS_state = MAX(_TAS_state, min_airspeed);
_integDTAS_state = 0.0f;
return;
}
@ -449,8 +453,7 @@ void AP_TECS::_update_speed(float DT)
_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);
}