From 6a17a605d4681d7c9e26e4799fa5aeb73129e25c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Sep 2023 12:18:00 +1000 Subject: [PATCH] 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. --- libraries/AP_TECS/AP_TECS.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index febc8db56b..843e008c8f 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -1160,8 +1160,18 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, float hgt_afe, float load_factor) { - // Calculate time in seconds since last update 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 = MAX(_DT, 0.001f); _update_pitch_throttle_last_usec = now;