MissionFeasibility: add combined takeoff/landing requirement check

Replace the existing check for the availability of a takeoff mission item with a combined
check for takeoff and landing item (or landing pattern). New param MIS_TKO_LAND_REQ
can be set to require only a takeoff, only a landing, both takeoff and landing, and
both or none. The latter is meant to be set if is e.g. deemed unsafe to start a flight
through a Takeoff WP without though defining a Landing - as then in case of a RTL the
vehicle doesn't follow a pre-defined path but instead only can do default RTL that especially
for FW and VTOL isn't always safe.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -45,6 +45,7 @@ param set-default NAV_ACC_RAD 10
 param set-default MIS_DIST_WPS 5000
 param set-default MIS_LTRMIN_ALT 25
 param set-default MIS_TAKEOFF_ALT 25
+param set-default MIS_TKO_LAND_REQ 2
 
 #
 # FW takeoff acceleration can easily exceed ublox GPS 2G default.

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -19,6 +19,7 @@ param set-default EKF2_FUSE_BETA 1
 param set-default HTE_VXY_THR 2.0
 
 param set-default MIS_DIST_WPS 5000
+param set-default MIS_TKO_LAND_REQ 2
 
 param set-default MPC_ACC_HOR_MAX 2
 param set-default MPC_VEL_MANUAL 5

src/modules/navigator/mission.cpp

@@ -1743,8 +1743,7 @@ Mission::check_mission_valid(bool force)
 		_navigator->get_mission_result()->valid =
 			_missionFeasibilityChecker.checkMissionFeasible(_mission,
 					_param_mis_dist_1wp.get(),
-					_param_mis_dist_wps.get(),
+					_param_mis_dist_wps.get());
-					_navigator->mission_landing_required());
 
 		_navigator->get_mission_result()->seq_total = _mission.count;
 		_navigator->increment_mission_instance_count();

src/modules/navigator/mission_feasibility_checker.cpp

@@ -54,8 +54,7 @@
 
 bool
 MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
-		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
+		float max_distance_to_1st_waypoint, float max_distance_between_waypoints)
-		bool land_start_req)
 {
 	// Reset warning flag
 	_navigator->get_mission_result()->warning = false;
@@ -82,57 +81,30 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
 
 	const float home_alt = _navigator->get_home_position()->alt;
 
+	// reset for next check
+	_has_takeoff = false;
+	_has_landing = false;
+
 	// check if all mission item commands are supported
 	failed = failed || !checkMissionItemValidity(mission);
 	failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
 	failed = failed || !checkGeofence(mission, home_alt, home_valid);
 	failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid);
+	failed = failed || !checkTakeoff(mission, home_alt);
 
 	if (_navigator->get_vstatus()->is_vtol) {
-		failed = failed || !checkVTOL(mission, home_alt, false);
+		failed |= (!checkTakeoff(mission, home_alt) || !checkVTOLLanding(mission) || !checkTakeoffLandAvailable());
 
 	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-		failed = failed || !checkRotarywing(mission, home_alt);
+		failed |= (!checkTakeoff(mission, home_alt) || !checkRotaryWingLanding(mission) || !checkTakeoffLandAvailable());
 
-	} else {
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		failed = failed || !checkFixedwing(mission, home_alt, land_start_req);
+		failed |= (!checkTakeoff(mission, home_alt) || !checkFixedWingLanding(mission) || !checkTakeoffLandAvailable());
 	}
 
 	return !failed;
 }
 
-bool
-MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt)
-{
-	/*
-	 * Perform check and issue feedback to the user
-	 * Mission is only marked as feasible if takeoff check passes
-	 */
-	return checkTakeoff(mission, home_alt);
-}
-
-bool
-MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req)
-{
-	/* Perform checks and issue feedback to the user for all checks */
-	bool resTakeoff = checkTakeoff(mission, home_alt);
-	bool resLanding = checkFixedWingLanding(mission, land_start_req);
-
-	/* Mission is only marked as feasible if all checks return true */
-	return (resTakeoff && resLanding);
-}
-
-bool
-MissionFeasibilityChecker::checkVTOL(const mission_s &mission, float home_alt, bool land_start_req)
-{
-	/* Perform checks and issue feedback to the user for all checks */
-	bool resTakeoff = checkTakeoff(mission, home_alt);
-	bool resLanding = checkVTOLLanding(mission, land_start_req);
-
-	/* Mission is only marked as feasible if all checks return true */
-	return (resTakeoff && resLanding);
-}
-
 bool
 MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_alt, bool home_valid)
 {
@@ -327,7 +299,6 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 bool
 MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt)
 {
-	bool has_takeoff = false;
 	bool takeoff_first = false;
 	int takeoff_index = -1;
 
@@ -341,7 +312,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 		}
 
 		// look for a takeoff waypoint
-		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
+		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF || missionitem.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
 			// make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius
 			// this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
 
@@ -367,7 +338,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 			}
 
 			// tell that mission has a takeoff waypoint
-			has_takeoff = true;
+			_has_takeoff = true;
 
 			// tell that a takeoff waypoint is the first "waypoint"
 			// mission item
@@ -427,25 +398,14 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 		}
 	}
 
-	if (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
+	if (_has_takeoff && !takeoff_first) {
-		// check for a takeoff waypoint, after the above conditions have been met
+		// check if the takeoff waypoint is the first waypoint item on the mission
-		// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
+		// i.e, an item with position/attitude change modification
-		// while the vehicle is flying and it does not require a takeoff waypoint
+		// if it is not, the mission should be rejected
-		if (!has_takeoff) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t");
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff waypoint required.\t");
+		events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info},
-			events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: takeoff is not the first waypoint item");
-				     "Mission rejected: takeoff waypoint required");
+		return false;
-			return false;
-
-		} else if (!takeoff_first) {
-			// check if the takeoff waypoint is the first waypoint item on the mission
-			// i.e, an item with position/attitude change modification
-			// if it is not, the mission should be rejected
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t");
-			events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: takeoff is not the first waypoint item");
-			return false;
-		}
 	}
 
 	// all checks have passed
@@ -453,14 +413,36 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 }
 
 bool
-MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool land_start_req)
+MissionFeasibilityChecker::checkRotaryWingLanding(const mission_s &mission)
+{
+	// Go through all mission items and search for a landing waypoint.
+	// For MC we currently do not run any checks on the validity of the planned landing.
+
+	for (size_t i = 0; i < mission.count; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(missionitem);
+
+		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND) {
+			_has_landing = true;
+		}
+	}
+
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission)
 {
 	/* Go through all mission items and search for a landing waypoint
 	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
 
 	bool landing_valid = false;
 
-	bool land_start_found = false;
 	size_t do_land_start_index = 0;
 	size_t landing_approach_index = 0;
 
@@ -475,14 +457,14 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 
 		// if DO_LAND_START found then require valid landing AFTER
 		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
-			if (land_start_found) {
+			if (_has_landing) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t");
 				events::send(events::ID("navigator_mis_multiple_land"), {events::Log::Error, events::LogInternal::Info},
 					     "Mission rejected: more than one land start commands");
 				return false;
 
 			} else {
-				land_start_found = true;
+				_has_landing = true;
 				do_land_start_index = i;
 			}
 		}
@@ -615,7 +597,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 			}
 
 		} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-			if (land_start_found && do_land_start_index < i) {
+			if (_has_landing && do_land_start_index < i) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
 						     "Mission rejected: land start item before RTL item not possible.\t");
 				events::send(events::ID("navigator_mis_land_before_rtl"), {events::Log::Error, events::LogInternal::Info},
@@ -625,14 +607,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 		}
 	}
 
-	if (land_start_req && !land_start_found) {
+	if (_has_landing && (!landing_valid || (do_land_start_index > landing_approach_index))) {
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t");
-		events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info},
-			     "Mission rejected: landing pattern required");
-		return false;
-	}
-
-	if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) {
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t");
 		events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info},
 			     "Mission rejected: invalid land start");
@@ -644,12 +619,10 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 }
 
 bool
-MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_start_req)
+MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission)
 {
-	/* Go through all mission items and search for a landing waypoint
+	// Go through all mission items and search for a landing waypoint, then run some checks on it
-	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
 
-	bool land_start_found = false;
 	size_t do_land_start_index = 0;
 	size_t landing_approach_index = 0;
 
@@ -664,14 +637,14 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
 
 		// if DO_LAND_START found then require valid landing AFTER
 		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
-			if (land_start_found) {
+			if (_has_landing) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t");
 				events::send(events::ID("navigator_mis_multi_land"), {events::Log::Error, events::LogInternal::Info},
 					     "Mission rejected: more than one land start commands");
 				return false;
 
 			} else {
-				land_start_found = true;
+				_has_landing = true;
 				do_land_start_index = i;
 			}
 		}
@@ -696,7 +669,7 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
 			}
 
 		} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-			if (land_start_found && do_land_start_index < i) {
+			if (_has_landing && do_land_start_index < i) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
 						     "Mission rejected: land start item before RTL item not possible.\t");
 				events::send(events::ID("navigator_mis_land_before_rtl2"), {events::Log::Error, events::LogInternal::Info},
@@ -706,14 +679,7 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
 		}
 	}
 
-	if (land_start_req && !land_start_found) {
+	if (_has_landing && (do_land_start_index > landing_approach_index)) {
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t");
-		events::send(events::ID("navigator_mis_land_missing2"), {events::Log::Error, events::LogInternal::Info},
-			     "Mission rejected: landing pattern required");
-		return false;
-	}
-
-	if (land_start_found && (do_land_start_index > landing_approach_index)) {
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t");
 		events::send(events::ID("navigator_mis_invalid_land2"), {events::Log::Error, events::LogInternal::Info},
 			     "Mission rejected: invalid land start");
@@ -724,6 +690,74 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
 	return true;
 }
 
+bool
+MissionFeasibilityChecker::checkTakeoffLandAvailable()
+{
+	bool result = true;
+
+	switch (_navigator->get_takeoff_land_required()) {
+	case 0:
+		result = true;
+		break;
+
+	case 1:
+		result = _has_takeoff;
+
+		if (!result) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff waypoint required.\t");
+			events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Takeoff waypoint required");
+			return false;
+		}
+
+		break;
+
+	case 2:
+		result = _has_landing;
+
+		if (!result) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Landing waypoint/pattern required.\t");
+			events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Landing waypoint/pattern required");
+		}
+
+		break;
+
+	case 3:
+		result = _has_takeoff && _has_landing;
+
+		if (!result) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff or Landing item missing.\t");
+			events::send(events::ID("navigator_mis_takeoff_or_land_missing"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Takeoff or Landing item missing");
+		}
+
+		break;
+
+	case 4:
+		result = _has_takeoff == _has_landing;
+
+		if (!result && (_has_takeoff)) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Landing item or remove Takeoff.\t");
+			events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Add Landing item or remove Takeoff");
+
+		} else if (!result && (_has_landing)) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Takeoff item or remove Landing.\t");
+			events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Add Takeoff item or remove Landing");
+		}
+
+		break;
+
+	default:
+		result = true;
+		break;
+	}
+
+	return result;
+}
+
 bool
 MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance)
 {

src/modules/navigator/mission_feasibility_checker.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -56,26 +56,18 @@ private:
 
 	/* Checks for all airframes */
 	bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid);
-
 	bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid);
-
 	bool checkMissionItemValidity(const mission_s &mission);
-
 	bool checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance);
 	bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance);
-
 	bool checkTakeoff(const mission_s &mission, float home_alt);
+	bool checkTakeoffLandAvailable();
+	bool checkRotaryWingLanding(const mission_s &mission);
+	bool checkFixedWingLanding(const mission_s &mission);
+	bool checkVTOLLanding(const mission_s &mission);
 
-	/* Checks specific to fixedwing airframes */
+	bool _has_takeoff{false};
-	bool checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req);
+	bool _has_landing{false};
-	bool checkFixedWingLanding(const mission_s &mission, bool land_start_req);
-
-	/* Checks specific to rotarywing airframes */
-	bool checkRotarywing(const mission_s &mission, float home_alt);
-
-	/* Checks specific to VTOL airframes */
-	bool checkVTOL(const mission_s &mission, float home_alt, bool land_start_req);
-	bool checkVTOLLanding(const mission_s &mission, bool land_start_req);
 
 public:
 	MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {}
@@ -88,6 +80,5 @@ public:
 	 * Returns true if mission is feasible and false otherwise
 	 */
 	bool checkMissionFeasible(const mission_s &mission,
-				  float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
+				  float max_distance_to_1st_waypoint, float max_distance_between_waypoints);
-				  bool land_start_req);
 };

src/modules/navigator/mission_params.c

@@ -58,14 +58,19 @@
 PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
 
 /**
- * Take-off waypoint required
+ * Mission takeoff/landing required
  *
- * If set, the mission feasibility checker will check for a takeoff waypoint on the mission.
+ * Specifies if a mission has to contain a takeoff and/or mission landing.
+ * Validity of configured takeoffs/landings is checked independently of the setting here.
  *
- * @boolean
+ * @value 0 No requirements
+ * @value 1 Require a takeoff
+ * @value 2 Require a landing
+ * @value 3 Require a takeoff and a landing
+ * @value 4 Require a takeoff and a landing, or neither of both
  * @group Mission
  */
-PARAM_DEFINE_INT32(MIS_TAKEOFF_REQ, 0);
+PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
 
 /**
  * Minimum Loiter altitude

src/modules/navigator/navigator.h

@@ -297,8 +297,6 @@ public:
 	float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
 
 	// RTL
-	bool mission_landing_required() { return _rtl.get_rtl_type() == RTL::RTL_TYPE_MISSION_LANDING; }
-
 	bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
 
 	bool abort_landing();
@@ -308,7 +306,7 @@ public:
 	// Param access
 	float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
 	float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
-	bool  get_takeoff_required() const { return _param_mis_takeoff_req.get(); }
+	int  get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); }
 	float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
 	float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
 	float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); }
@@ -451,7 +449,7 @@ private:
 		// non-navigator parameters: Mission (MIS_*)
 		(ParamFloat<px4::params::MIS_LTRMIN_ALT>)  _param_mis_ltrmin_alt,
 		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
-		(ParamBool<px4::params::MIS_TAKEOFF_REQ>)  _param_mis_takeoff_req,
+		(ParamInt<px4::params::MIS_TKO_LAND_REQ>)  _para_mis_takeoff_land_req,
 		(ParamFloat<px4::params::MIS_YAW_TMT>)     _param_mis_yaw_tmt,
 		(ParamFloat<px4::params::MIS_YAW_ERR>)     _param_mis_yaw_err,
 		(ParamFloat<px4::params::MIS_PD_TO>)       _param_mis_payload_delivery_timeout,