mtecs: landing mode which limits pitch and as well throttle at the end of the landing

3 files changed, 52 insertions, 3 deletions

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index a4cf0bdf8..b717429d3 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -63,6 +63,8 @@ mTecs::mTecs() :
 	_BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
 	_BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"),
 	_BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true),
+	_BlockOutputLimiterLandThrottle(this, "LND_THR"),
+	_BlockOutputLimiterLandPitch(this, "LND_PIT", true),
 	timestampLastIteration(hrt_absolute_time()),
 	_firstIterationAfterReset(true),
 	_dtCalculated(false),
@@ -148,6 +150,12 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh
 	if (mode == TECS_MODE_TAKEOFF) {
 		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
+	} else if (mode == TECS_MODE_LAND) {
+		// only limit pitch but do not limit throttle
+		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
+	} else if (mode == TECS_MODE_LAND_THROTTLELIM) {
+		outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
+		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
 	} else if (mode == TECS_MODE_UNDERSPEED) {
 		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index fab4b161a..9ed233ba0 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -61,7 +61,9 @@ public:
 	typedef enum {
 		TECS_MODE_NORMAL,
 		TECS_MODE_UNDERSPEED,
-		TECS_MODE_TAKEOFF
+		TECS_MODE_TAKEOFF,
+		TECS_MODE_LAND,
+		TECS_MODE_LAND_THROTTLELIM
 	} tecs_mode;
 
 	/*
@@ -111,8 +113,10 @@ protected:
 	/* Output Limits in special modes */
 	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;		/**< Throttle Limits during takeoff */
 	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;      	/**< Pitch Limit during takeoff */
-	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;	/**< Throttle Limits during takeoff */
-	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;   	/**< Pitch Limit during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;	/**< Throttle Limits when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;   	/**< Pitch Limit when underspeed is detected */
+	BlockOutputLimiter _BlockOutputLimiterLandThrottle;		/**< Throttle Limits during landing (only in last phase)*/
+	BlockOutputLimiter _BlockOutputLimiterLandPitch;		/**< Pitch Limit during landing */
 
 	/* Time measurements */
 	hrt_abstime timestampLastIteration;		/**< Saves the result of hrt_absolute_time() of the last iteration */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index fd501c17a..3a05f2c88 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -323,3 +323,40 @@ PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f);
  */
 PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f);
 
+/**
+ * Minimal throttle in landing mode (only last phase of landing)
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f);
+
+/**
+ * Maximal throttle in landing mode (only last phase of landing)
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f);
+
+/**
+ * Minimal pitch in landing mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f);
+
+/**
+ * Maximal pitch in landing mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f);