mission feasibility checker: move checks for VTOL landing into separa…

…te function

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

src/modules/navigator/mission_feasibility_checker.cpp

@@ -83,8 +83,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
 	failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned);
 
 	if (_navigator->get_vstatus()->is_vtol) {
-		failed = failed || !checkRotarywing(mission, home_alt);
-		failed = failed || !checkFixedwing(mission, home_alt, false);
+		failed = failed || !checkVTOL(mission, home_alt, false);
 
 	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 		failed = failed || !checkRotarywing(mission, home_alt);
@@ -117,6 +116,17 @@ MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_a
 	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)
 {
@@ -527,6 +537,77 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 	return true;
 }
 
+bool
+MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_start_req)
+{
+	/* 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 land_start_found = false;
+	size_t do_land_start_index = 0;
+	size_t landing_approach_index = 0;
+
+	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 DO_LAND_START found then require valid landing AFTER
+		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
+			if (land_start_found) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.");
+				return false;
+
+			} else {
+				land_start_found = true;
+				do_land_start_index = i;
+			}
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND) {
+			mission_item_s missionitem_previous {};
+
+			if (i > 0) {
+				landing_approach_index = i - 1;
+
+				if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+					return false;
+				}
+
+
+			} else {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.");
+				return false;
+			}
+
+		} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+			if (land_start_found && do_land_start_index < i) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Mission rejected: land start item before RTL item not possible.");
+				return false;
+			}
+		}
+	}
+
+	if (land_start_req && !land_start_found) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "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.");
+		return false;
+	}
+
+	/* No landing waypoints or no waypoints */
+	return true;
+}
+
 bool
 MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance)
 {

src/modules/navigator/mission_feasibility_checker.h

@@ -63,14 +63,19 @@ class MissionFeasibilityChecker
 	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);
+
 	/* Checks specific to fixedwing airframes */
 	bool checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req);
-	bool checkTakeoff(const mission_s &mission, float home_alt);
 	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) : _navigator(navigator) {}
 	~MissionFeasibilityChecker() = default;