From 35ed5607b4e5d737b2d3cda5b5cf695be834f5ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Mar 2020 10:40:54 +0100 Subject: [PATCH] Mission feasibility checker: Prevent zero-length trajectories The mission logic depends in a number of locations on being able to calculate the direction from one waypoint to another. Missions that have waypoints that are in the same physical location do not make sense and need to be rejected (the GCS / SDK generating them needs to be fixed). By enforcing this we can work with a reasonable and simpler state machine while executing the mission. --- .../navigator/mission_feasibility_checker.cpp | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index af7b6736ab..0b07c32fc5 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -672,6 +672,8 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi double last_lat = (double)NAN; double last_lon = (double)NAN; + float last_alt = NAN; + int last_cmd = 0; /* Go through all waypoints */ for (size_t i = 0; i < mission.count; i++) { @@ -697,19 +699,38 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi mission_item.lat, mission_item.lon, last_lat, last_lon); + if (dist_between_waypoints > max_distance) { - /* item is too far from home */ + /* distance between waypoints is too high */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Distance between waypoints too far: %d meters, %d max.", (int)dist_between_waypoints, (int)max_distance); _navigator->get_mission_result()->warning = true; return false; + + /* do not allow waypoints that are literally on top of each other */ + + } else if ((dist_between_waypoints < 0.05f && fabsf(last_alt - mission_item.altitude) < 0.05f) || + /* and do not allow condition gates that are at the same position as a navigation waypoint */ + (dist_between_waypoints < 0.05f && (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE + || last_cmd == NAV_CMD_CONDITION_GATE))) { + /* waypoints are at the exact same position, + * which indicates an invalid mission and makes calculating + * the direction from one waypoint to another impossible. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Distance between waypoints too close: %d meters", + (int)dist_between_waypoints); + + _navigator->get_mission_result()->warning = true; + return false; } } last_lat = mission_item.lat; last_lon = mission_item.lon; + last_alt = mission_item.altitude; + last_cmd = mission_item.nav_cmd; } /* We ran through all waypoints and have not found any distances between waypoints that are too far. */