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>
2026-10-22 21:33:34 +08:00 · 2022-11-21 18:03:21 +01:00
parent cce3b43b4f
commit 4cd7dfa162
7 changed files with 144 additions and 115 deletions
@@ -54,8 +54,7 @@

 bool
 MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
-		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
-		bool land_start_req)
+		float max_distance_to_1st_waypoint, float max_distance_between_waypoints)
 {
 	// 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 {
-		failed = failed || !checkFixedwing(mission, home_alt, land_start_req);
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		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) {
-		// check for a takeoff waypoint, after the above conditions have been met
-		// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
-		// while the vehicle is flying and it does not require a takeoff waypoint
-		if (!has_takeoff) {
-			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;
-
-		} 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;
-		}
+	if (_has_takeoff && !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) {
-		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))) {
+	if (_has_landing && (!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
-	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
+	// Go through all mission items and search for a landing waypoint, then run some checks on it

-	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) {
-		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)) {
+	if (_has_landing && (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)
 {