1 files changed, 72 insertions, 21 deletions

diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 5cafb1c79..eb45237b7 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -28,6 +28,28 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
+		_tecs_state {},
+		_update_50hz_last_usec(0),
+		_update_speed_last_usec(0),
+		_update_pitch_throttle_last_usec(0),
+		// TECS tuning parameters
+		_hgtCompFiltOmega(0.0f),
+		_spdCompFiltOmega(0.0f),
+		_maxClimbRate(2.0f),
+		_minSinkRate(1.0f),
+		_maxSinkRate(2.0f),
+		_timeConst(5.0f),
+		_timeConstThrot(8.0f),
+		_ptchDamp(0.0f),
+		_thrDamp(0.0f),
+		_integGain(0.0f),
+		_vertAccLim(0.0f),
+		_rollComp(0.0f),
+		_spdWeight(0.5f),
+		_heightrate_p(0.0f),
+		_heightrate_ff(0.0f),
+		_speedrate_p(0.0f),
+		_throttle_dem(0.0f),
 		_pitch_dem(0.0f),
 		_integ1_state(0.0f),
 		_integ2_state(0.0f),
@@ -45,6 +67,9 @@ public:
 		_hgt_dem_prev(0.0f),
 		_TAS_dem_adj(0.0f),
 		_STEdotErrLast(0.0f),
+		_underspeed(false),
+		_detect_underspeed_enabled(true),
+		_badDescent(false),
 		_climbOutDem(false),
 		_SPE_dem(0.0f),
 		_SKE_dem(0.0f),
@@ -100,29 +125,42 @@ public:
 		return _spdWeight;
 	}
 
-	// log data on internal state of the controller. Called at 10Hz
-	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
-
-	// struct PACKED log_TECS_Tuning {
-	// 	LOG_PACKET_HEADER;
-	// 	float hgt;
-	// 	float dhgt;
-	// 	float hgt_dem;
-	// 	float dhgt_dem;
-	// 	float spd_dem;
-	// 	float spd;
-	// 	float dspd;
-	// 	float ithr;
-	// 	float iptch;
-	// 	float thr;
-	// 	float ptch;
-	// 	float dspd_dem;
-	// } log_tuning;
+	enum ECL_TECS_MODE {
+		ECL_TECS_MODE_NORMAL = 0,
+		ECL_TECS_MODE_UNDERSPEED,
+		ECL_TECS_MODE_BAD_DESCENT,
+		ECL_TECS_MODE_CLIMBOUT
+	};
+
+	struct tecs_state {
+		uint64_t timestamp;
+		float hgt;
+		float dhgt;
+		float hgt_dem;
+		float dhgt_dem;
+		float spd_dem;
+		float spd;
+		float dspd;
+		float ithr;
+		float iptch;
+		float thr;
+		float ptch;
+		float dspd_dem;
+		enum ECL_TECS_MODE mode;
+	};
+
+	void get_tecs_state(struct tecs_state& state) {
+		state = _tecs_state;
+	}
 
 	void set_time_const(float time_const) {
 		_timeConst = time_const;
 	}
 
+	void set_time_const_throt(float time_const_throt) {
+		_timeConstThrot = time_const_throt;
+	}
+
 	void set_min_sink_rate(float rate) {
 		_minSinkRate = rate;
 	}
@@ -183,11 +221,22 @@ public:
 		_heightrate_p = heightrate_p;
 	}
 
+	void set_heightrate_ff(float heightrate_ff) {
+		_heightrate_ff = heightrate_ff;
+	}
+
 	void set_speedrate_p(float speedrate_p) {
 		_speedrate_p = speedrate_p;
 	}
 
+	void set_detect_underspeed_enabled(bool enabled) {
+		_detect_underspeed_enabled = enabled;
+	}
+
 private:
+
+	struct tecs_state _tecs_state;
+
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
 
@@ -204,6 +253,7 @@ private:
 	float _minSinkRate;
 	float _maxSinkRate;
 	float _timeConst;
+	float _timeConstThrot;
 	float _ptchDamp;
 	float _thrDamp;
 	float _integGain;
@@ -211,6 +261,7 @@ private:
 	float _rollComp;
 	float _spdWeight;
 	float _heightrate_p;
+	float _heightrate_ff;
 	float _speedrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
@@ -285,15 +336,15 @@ private:
 	// Underspeed condition
 	bool _underspeed;
 
+	// Underspeed detection enabled
+	bool _detect_underspeed_enabled;
+
 	// Bad descent condition caused by unachievable airspeed demand
 	bool _badDescent;
 
 	// climbout mode
 	bool _climbOutDem;
 
-	// throttle demand before limiting
-	float _throttle_dem_unc;
-
 	// pitch demand before limiting
 	float _pitch_dem_unc;