fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mission feasibility checker: Prevent zero-length trajectories

Browse Source 
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.
This commit is contained in:
Lorenz Meier 2020-03-13 10:40:54 +01:00
parent 76d20be29d
commit 35ed5607b4
1 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -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. */