Navigator: MissionFeasibilityCheck: check if items fit to the current…

… vehicle type (#21602) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
PX4 · May 31, 2023 · 4b5e14a · 4b5e14a
1 parent d6413a6
commit 4b5e14a
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 2 deletions.
diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp
@@ -206,6 +206,10 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int
 	if (!_takeoff_failed) {
 		_takeoff_failed = !checkTakeoff(mission_item);
 	}
+
+	if (!_items_fit_to_vehicle_type_failed) {
+		_items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item);
+	}
 }
 
 void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index)
@@ -686,12 +690,26 @@ bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_
 
 	if (PX4_ISFINITE(_home_alt_msl) && _home_alt_msl > wp_alt && MissionBlock::item_contains_position(mission_item)) {
 
-
-
 		mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home\t", current_index + 1);
 		events::send<int16_t>(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info},
 				      "Waypoint {1} below home", current_index + 1);
 	}
 
 	return true;
 }
+
+bool FeasibilityChecker::checkItemsFitToVehicleType(const mission_item_s &mission_item)
+{
+	if (_vehicle_type != VehicleType::Vtol &&
+	    (mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || mission_item.nav_cmd == NAV_CMD_VTOL_LAND
+	     || mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION)) {
+
+		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Mission contains VTOL items but vehicle is not a VTOL\t");
+		events::send(events::ID("navigator_mis_vtol_items"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: Mission contains VTOL items but vehicle is not a VTOL");
+
+		return false;
+	}
+
+	return true;
+}
diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp
@@ -120,6 +120,7 @@ class FeasibilityChecker : public ModuleParams
 	bool _below_home_alt_failed{false};
 	bool _fixed_wing_land_approach_failed{false};
 	bool _takeoff_land_available_failed{false};
+	bool _items_fit_to_vehicle_type_failed{false};
 
 	// internal checkTakeoff related variables
 	bool _found_item_with_position{false};
@@ -163,6 +164,14 @@ class FeasibilityChecker : public ModuleParams
 	*/
 	bool checkTakeoff(mission_item_s &mission_item);
 
+	/**
+	 * @brief Check if the mission items fit to the vehicle type
+	 *
+	 * @param mission_item The current mission item
+	 * @return False if the check failed.
+	*/
+	bool checkItemsFitToVehicleType(const mission_item_s &mission_item);
+
 	/**
 	 * @brief Check validity of landing pattern (fixed wing & vtol)
 	 *