From 660ad5e85aa3698c672a2938b72c6652c94f923f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Dec 2019 13:47:25 +0100 Subject: [PATCH 1/4] navigator: check fixedwing mission for VTOL A VTOL mission can also contain a fixedwing landing. Therefore, I think, it makes sense to run the fixedwing checks as well, however, don't check for a landing start necessarily. --- src/modules/navigator/mission_feasibility_checker.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 86ddef9baab7..3d48b53a55d1 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -87,6 +87,10 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, || _navigator->get_vstatus()->is_vtol) { failed = failed || !checkRotarywing(mission, home_alt); + if (_navigator->get_vstatus()->is_vtol) { + failed = failed || !checkFixedwing(mission, home_alt, false); + } + } else { failed = failed || !checkFixedwing(mission, home_alt, land_start_req); } From 29fed0274524ae1e4333fa550c7b3b670b8ce9a5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Dec 2019 14:07:26 +0100 Subject: [PATCH 2/4] navigator: don't allow Land together with RTL item This is a check to protect a user from upload a mission which includes a land_start marker as well as a RTL mission item. This works around the problem that we experienced where the navigator would switch to mission mode on RTL and start with land_start, and then continue to the RTL item. At this point, navigator would send the vehicle_command for RTL to commander which would deny it because it is already in RTL mode. Thus navigator won't (re)set any items to actually do RTL and it means the vehicle will just head to whatever setpoint it received last. --- src/modules/navigator/mission_feasibility_checker.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 3d48b53a55d1..7f997584f89c 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -505,6 +505,13 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool 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; + } } } From 6b00764ed2b5121b2e5963a2a5db6d0b12b1297a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Dec 2019 13:29:35 +0100 Subject: [PATCH 3/4] navigator: improve if structure for vtol/mc/fw --- src/modules/navigator/mission_feasibility_checker.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 7f997584f89c..39aeb58133ac 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -82,14 +82,12 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, failed = failed || !checkGeofence(mission, home_alt, home_valid); failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned); - // VTOL always respects rotary wing feasibility - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - || _navigator->get_vstatus()->is_vtol) { + if (_navigator->get_vstatus()->is_vtol) { failed = failed || !checkRotarywing(mission, home_alt); + failed = failed || !checkFixedwing(mission, home_alt, false); - if (_navigator->get_vstatus()->is_vtol) { - failed = failed || !checkFixedwing(mission, home_alt, false); - } + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + failed = failed || !checkRotarywing(mission, home_alt); } else { failed = failed || !checkFixedwing(mission, home_alt, land_start_req); From 656d5a6b9fe204f379ca4aa95e680d70b561cf1f Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 17 Dec 2019 10:16:52 +0100 Subject: [PATCH 4/4] mission feasibility checker: move checks for VTOL landing into separate function Signed-off-by: Silvan Fuhrer --- .../navigator/mission_feasibility_checker.cpp | 85 ++++++++++++++++++- .../navigator/mission_feasibility_checker.h | 7 +- 2 files changed, 89 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 39aeb58133ac..203e48b4512a 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 5c2f82538932..97c6e165f77c 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/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;