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

AP_TECS: ensure good TECS state before running update_pitch_throttle 

update_pitch_throttle can be called when update_50hz hasn't run in a very long time, or ever.  This requires a main loop rate >50Hz, and for the mode change to occur in the same loop that update_50Hz doesn't run but update_pitch_throttle does.
This commit is contained in:
Peter Barker 2023-09-08 12:18:00 +10:00 committed by Andrew Tridgell
parent bfc9b6e389
commit 6a17a605d4
1 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_TECS/AP_TECS.cpp
View File

@ -1160,8 +1160,18 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
float hgt_afe, float hgt_afe,
float load_factor) float load_factor)
{ {
// Calculate time in seconds since last update
uint64_t now = AP_HAL::micros64(); uint64_t now = AP_HAL::micros64();
// check how long since we last did the 50Hz update; do nothing in
// this loop if that hasn't run for some signficant period of
// time. Notably, it may never have run, leaving _TAS_state as
// zero and subsequently division-by-zero errors.
const float _DT_for_update_50hz = (now - _update_50hz_last_usec) * 1.0e-6f;
if (_update_50hz_last_usec == 0 || _DT_for_update_50hz > 1.0) {
// more than 1 second since it was run, don't do anything yet:
return;
}
// Calculate time in seconds since last update
_DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f; _DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
_DT = MAX(_DT, 0.001f); _DT = MAX(_DT, 0.001f);
_update_pitch_throttle_last_usec = now; _update_pitch_throttle_last_usec = now;