From 176d3c0b2fbf9a449d3490c76225c2284e3fe300 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 6 Feb 2020 15:43:16 +1100 Subject: [PATCH] AP_TECS: added reset() method used at end of VTOL takeoff --- libraries/AP_TECS/AP_TECS.cpp | 3 ++- libraries/AP_TECS/AP_TECS.h | 10 +++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 5f0c60a46f..6821df6a70 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -911,7 +911,7 @@ void AP_TECS::_update_pitch(void) void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) { // Initialise states and variables if DT > 1 second or in climbout - if (_DT > 1.0f) + if (_DT > 1.0f || _need_reset) { _integTHR_state = 0.0f; _integSEB_state = 0.0f; @@ -927,6 +927,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _flags.reached_speed_takeoff = false; _DT = 0.1f; // when first starting TECS, use a // small time constant + _need_reset = false; } else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 2f91f33656..29803886fc 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -116,7 +116,12 @@ public: void use_synthetic_airspeed(void) { _use_synthetic_airspeed_once = true; } - + + // reset on next loop + void reset(void) override { + _need_reset = true; + } + // this supports the TECS_* user settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -320,6 +325,9 @@ private: float _land_pitch_min = -90; + // need to reset on next loop + bool _need_reset; + // internal variables to be logged struct { float SKE_weighting;