From 1da686b125803ee546d6f76f8cef158a22c80e1d Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Tue, 2 Feb 2016 11:51:55 +0100
Subject: [PATCH] TECS: - do not run TECS for VTO which are in rotary wing mode
 - reinitialise TECS the first time we start using it again

---
 src/lib/external_lgpl/tecs/tecs.h             |  3 +++
 .../fw_pos_control_l1_main.cpp                | 22 +++++++++++++++++--
 2 files changed, 23 insertions(+), 2 deletions(-)

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;