Merge pull request #1335 from PX4/landingtecsunderspeed

disable underspeed protection when landing also in tecs

3 files changed, 18 insertions, 0 deletions

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index a57a0481a..d27bf776f 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state)
 
 void TECS::_detect_underspeed(void)
 {
+	if (!_detect_underspeed_enabled) {
+		_underspeed = false;
+		return;
+	}
+
 	if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
 		_underspeed = true;
 
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index bcc2d90e5..36ae4ecaf 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -66,6 +66,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),
@@ -221,6 +224,10 @@ public:
 		_speedrate_p = speedrate_p;
 	}
 
+	void set_detect_underspeed_enabled(bool enabled) {
+		_detect_underspeed_enabled = enabled;
+	}
+
 private:
 
 	struct tecs_state _tecs_state;
@@ -323,6 +330,9 @@ private:
 	// Underspeed condition
 	bool _underspeed;
 
+	// Underspeed detection enabled
+	bool _detect_underspeed_enabled;
+
 	// Bad descent condition caused by unachievable airspeed demand
 	bool _badDescent;
 
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 350ce6dec..522f5caca 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
@@ -1380,6 +1380,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
 				limitOverride);
 	} else {
+		/* No underspeed protection in landing mode */
+		_tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM));
+
 		/* Using tecs library */
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,