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;