Merge remote-tracking branch 'origin/master' into geo

1 files changed, 32 insertions, 17 deletions

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 3730b1920..a57a0481a 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 
 	} else {
 		// Calculate gain scaler from specific energy error to throttle
-		float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
+		float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min));
 
 		// Calculate feed-forward throttle
 		float ff_throttle = 0;
@@ -310,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
 
 		if (STEdot_dem >= 0) {
-			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
+			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr);
 
 		} else {
 			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
@@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 			_throttle_dem = constrain(_throttle_dem,
 						  _last_throttle_dem - thrRateIncr,
 						  _last_throttle_dem + thrRateIncr);
-			_last_throttle_dem = _throttle_dem;
 		}
 
+		// Ensure _last_throttle_dem is always initialized properly
+		// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
+		_last_throttle_dem = _throttle_dem;
+
 
 		// Calculate integrator state upper and lower limits
 		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
@@ -551,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	// Calculate pitch demand
 	_update_pitch();
 
-//    // Write internal variables to the log_tuning structure. This
-//    // structure will be logged in dataflash at 10Hz
-	// log_tuning.hgt_dem  = _hgt_dem_adj;
-	// log_tuning.hgt      = _integ3_state;
-	// log_tuning.dhgt_dem = _hgt_rate_dem;
-	// log_tuning.dhgt     = _integ2_state;
-	// log_tuning.spd_dem  = _TAS_dem_adj;
-	// log_tuning.spd      = _integ5_state;
-	// log_tuning.dspd     = _vel_dot;
-	// log_tuning.ithr     = _integ6_state;
-	// log_tuning.iptch    = _integ7_state;
-	// log_tuning.thr      = _throttle_dem;
-	// log_tuning.ptch     = _pitch_dem;
-	// log_tuning.dspd_dem = _TAS_rate_dem;
+	_tecs_state.timestamp = now;
+
+	if (_underspeed) {
+		_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
+	} else if (_badDescent) {
+		_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
+	} else if (_climbOutDem) {
+		_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
+	} else {
+		// If no error flag applies, conclude normal
+		_tecs_state.mode = ECL_TECS_MODE_NORMAL;
+	}
+
+	_tecs_state.hgt_dem  = _hgt_dem_adj;
+	_tecs_state.hgt      = _integ3_state;
+	_tecs_state.dhgt_dem = _hgt_rate_dem;
+	_tecs_state.dhgt     = _integ2_state;
+	_tecs_state.spd_dem  = _TAS_dem_adj;
+	_tecs_state.spd      = _integ5_state;
+	_tecs_state.dspd     = _vel_dot;
+	_tecs_state.ithr     = _integ6_state;
+	_tecs_state.iptch    = _integ7_state;
+	_tecs_state.thr      = _throttle_dem;
+	_tecs_state.ptch     = _pitch_dem;
+	_tecs_state.dspd_dem = _TAS_rate_dem;
+
 }