AP_TECS: fixed descent or lack of climb bug

this fixes a state where we either cannot climb or descend in an
uncontrolled manner in a TECS controlled mode

the conditions under which this happened were:

- _use_synthetic_airspeed_once was true due to quadplane takeoff
- we left _thr_clip_status as MAX from previous use of synthetic airspeed
- then run without airspeed

note that this can also impact users with an airspeed sensor if they
disable it or it fails in flight, particularly during a climb
This commit is contained in:
Andrew Tridgell 2023-09-28 12:52:04 +10:00 committed by Peter Barker
parent e2895c08ae
commit 4a181ede62
1 changed files with 4 additions and 0 deletions

View File

@ -831,6 +831,9 @@ float AP_TECS::_get_i_gain(void)
*/
void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge)
{
// reset clip status after possible use of synthetic airspeed
_thr_clip_status = clipStatus::NONE;
// Calculate throttle demand by interpolating between pitch and throttle limits
float nomThr;
//If landing and we don't have an airspeed sensor and we have a non-zero
@ -1088,6 +1091,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_lag_comp_hgt_offset = 0.0f;
_post_TO_hgt_offset = 0.0f;
_takeoff_start_ms = 0;
_use_synthetic_airspeed_once = false;
_flags.underspeed = false;
_flags.badDescent = false;