diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 0f4086926a1..f53b79edff7 100644 --- 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,8 +690,6 @@ 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(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info}, "Waypoint {1} below home", current_index + 1); @@ -695,3 +697,19 @@ bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_ 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 index 48124daef0a..493d618cce0 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -120,6 +120,7 @@ private: 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 @@ private: */ 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) *