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

navigator: allow mission items with same position

Browse Source 
This reverts the addition of a check against mission items that have the
same position. This breaks existing MAVSDK implementations where a
LOITER_TIME item is set right after a WAYPOINT with the same
coordinates. It is an interim hack to allow the vehicle to hold still
during a photo is captured.

This leaves the check in place for gates where we need to be able to
calculate the direction between gate and waypoint.
This commit is contained in:
Julian Oes
2020-05-19 08:51:21 +02:00
parent a605444462
commit 793eb82153
1 changed files with 9 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+9 -10
View File
@@ -676,7 +676,6 @@ 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 */
@@ -715,15 +714,16 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi
/* 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. */
/* and do not allow condition gates that are at the same position as a navigation waypoint */
} else if (dist_between_waypoints < 0.05f &&
(mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || last_cmd == NAV_CMD_CONDITION_GATE)) {
/* Waypoints and gate 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",
"Distance between waypoint and gate too close: %d meters",
(int)dist_between_waypoints);
_navigator->get_mission_result()->warning = true;
@@ -733,7 +733,6 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &missi
last_lat = mission_item.lat;
last_lon = mission_item.lon;
last_alt = mission_item.altitude;
last_cmd = mission_item.nav_cmd;
}