Merge branch 'fw_autoland_att_tecs' into fw_autoland_att_tecs_navigator

Conflicts:
	src/lib/external_lgpl/tecs/tecs.cpp
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

2 files changed, 53 insertions, 34 deletions

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index cfb505194..d089be080 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -169,35 +169,45 @@ void TECS::_update_speed_demand(void)
 	// calculate velocity rate limits based on physical performance limits
 	// provision to use a different rate limit if bad descent or underspeed condition exists
 	// Use 50% of maximum energy rate to allow margin for total energy contgroller
-	float velRateMax;
-	float velRateMin;
-
-	if ((_badDescent) || (_underspeed)) {
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-
-	} else {
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-	}
-
-	// Apply rate limit
-	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
-		_TAS_rate_dem = velRateMax;
-
-	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
-		_TAS_rate_dem = velRateMin;
+//	float velRateMax;
+//	float velRateMin;
+//
+//	if ((_badDescent) || (_underspeed)) {
+//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+//
+//	} else {
+//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+//	}
+//
+//	// Apply rate limit
+//	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+//		_TAS_rate_dem = velRateMax;
+//
+//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+//		_TAS_rate_dem = velRateMin;
+//
+//	} else {
+//		_TAS_dem_adj = _TAS_dem;
+//
+//
+//	_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+//	}
 
-	} else {
-		_TAS_dem_adj = _TAS_dem;
-		_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
-	}
+	_TAS_dem_adj = _TAS_dem;
+	_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
 
 	// Constrain speed demand again to protect against bad values on initialisation.
 	_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
-	_TAS_dem_last = _TAS_dem;
+//	_TAS_dem_last = _TAS_dem;
+
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f,  _badDescent %u , _underspeed %u",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state,  _badDescent , _underspeed);
 }
 
 void TECS::_update_height_demand(float demand, float state)
@@ -240,7 +250,7 @@ void TECS::_update_height_demand(float demand, float state)
 		_hgt_rate_dem = -_maxSinkRate;
 	}
 
-	// warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
+	//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
 }
 
 void TECS::_detect_underspeed(void)
@@ -300,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
 		// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
 		// drag increase during turns.
 		float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
-		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
+		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);
@@ -368,14 +378,18 @@ void TECS::_detect_bad_descent(void)
 	// 1) Underspeed protection not active
 	// 2) Specific total energy error > 0
 	// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
-	float STEdot = _SPEdot + _SKEdot;
-
-	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
-		_badDescent = true;
+//	float STEdot = _SPEdot + _SKEdot;
+//
+//	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+//
+//		warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
+//		_badDescent = true;
+//
+//	} else {
+//		_badDescent = false;
+//	}
 
-	} else {
-		_badDescent = false;
-	}
+	_badDescent = false;
 }
 
 void TECS::_update_pitch(void)
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 06e1c8ad3..4fc009da9 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -184,6 +184,10 @@ public:
 		_heightrate_p = heightrate_p;
 	}
 
+	void set_speedrate_p(float speedrate_p) {
+		_speedrate_p = speedrate_p;
+	}
+
 private:
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
@@ -208,6 +212,7 @@ private:
 	float _rollComp;
 	float _spdWeight;
 	float _heightrate_p;
+	float _speedrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
 	float _throttle_dem;