diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 39aeb58133..203e48b451 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/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) { diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 5c2f825389..97c6e165f7 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -63,14 +63,19 @@ private: 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;