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

4 files changed, 60 insertions, 17 deletions

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 44afa43d8..5cdab10a1 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
@@ -362,12 +362,14 @@ private:
 
 	/*
 	 * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
+	 * XXX need to clean up/remove this function once mtecs fully replaces TECS
 	 */
 	void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
 			float pitch_min_rad, float pitch_max_rad,
 			float throttle_min, float throttle_max, float throttle_cruise,
 			bool climbout_mode, float climbout_pitch_min_rad,
-			const math::Vector<3> &ground_speed);
+			const math::Vector<3> &ground_speed,
+			fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL);
 
 };
 
@@ -958,7 +960,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas,
 						flare_pitch_angle_rad, math::radians(15.0f),
 						0.0f, throttle_max, throttle_land,
-						false, flare_pitch_angle_rad, ground_speed);
+						false, flare_pitch_angle_rad, ground_speed, land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -1031,7 +1033,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas,
 							math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 							_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed);
+							true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed, fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
 
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
@@ -1355,24 +1357,16 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
 		bool climbout_mode, float climbout_pitch_min_rad,
-		const math::Vector<3> &ground_speed)
+		const math::Vector<3> &ground_speed,
+		fwPosctrl::mTecs::tecs_mode mode)
 {
 	if (_mTecs.getEnabled()) {
-
 		float flightPathAngle = 0.0f;
 		float ground_speed_length = ground_speed.length();
 		if (ground_speed_length > FLT_EPSILON) {
 			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
 		}
-
-		if (!climbout_mode) {
-			/* Normal operation */
-			_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_NORMAL);
-		} else {
-			/* Climbout/Takeoff mode requested */
-			_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
-		}
-
+		_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
 	} else {
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
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);