diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 3104446a15..5755c41bd2 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -153,6 +153,9 @@ public: return _spdWeight; } + // this will force TECS to reinitialize all states + void reinitialize_states() {_states_initalized = false;} + enum ECL_TECS_MODE { ECL_TECS_MODE_NORMAL = 0, ECL_TECS_MODE_UNDERSPEED, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c746e08812..1663e82999 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -237,6 +237,7 @@ private: float _roll; float _pitch; float _yaw; + bool _reinitialize_tecs; ///< indicates if the TECS states should be reinitialized (used for VTOL) ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -565,6 +566,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_last_received(0), _groundspeed_undershoot(0.0f), _global_pos_valid(false), + _reinitialize_tecs(true), _l1_control(), _mTecs(), _control_mode_current(FW_POSCTRL_MODE_OTHER) @@ -2154,11 +2156,27 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, unsigned mode, bool pitch_max_special) { - /* do not run tecs if we are not in air */ - if (_vehicle_status.condition_landed) { + bool run_tecs = true; + + // do not run TECS if we are not in air + run_tecs &= !_vehicle_status.condition_landed; + + // do not run TECS if vehicle is a VTOL and we are in rotary wing mode + if (_vehicle_status.is_vtol) { + run_tecs &= !_vehicle_status.is_rotary_wing; + } + + if (!run_tecs) { + // next time we run TECS we should reinitialize states + _reinitialize_tecs = true; return; } + if (_reinitialize_tecs) { + _tecs.reinitialize_states(); + _reinitialize_tecs = false; + } + if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f;