From 408d147a18f28867c6d0b070b8070bae3b01d913 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 20 Mar 2016 23:35:29 -0400 Subject: [PATCH] MissionFeasibilityChecker geofence ignore NAV_CMD_DO_* -closes #4040 --- src/modules/navigator/mission_block.cpp | 12 +++++++----- src/modules/navigator/mission_block.h | 4 ++-- .../navigator/mission_feasibility_checker.cpp | 5 ++++- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index fbdd684ec6..aa38478e2a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -318,11 +318,13 @@ bool MissionBlock::item_contains_position(const struct mission_item_s *item) { // XXX: maybe extend that check onto item properties - if (item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || - item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST || - item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || - item->nav_cmd == NAV_CMD_DO_SET_SERVO || - item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED) { + if (item->nav_cmd == NAV_CMD_DO_JUMP || + item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED || + item->nav_cmd == NAV_CMD_DO_SET_SERVO || + item->nav_cmd == NAV_CMD_DO_REPEAT_SERVO || + item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || + item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST || + item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) { return false; } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index e151994525..9a2be82422 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -67,6 +67,8 @@ public: */ virtual ~MissionBlock(); + static bool item_contains_position(const struct mission_item_s *item); + protected: /** * Check if mission item has been reached @@ -78,8 +80,6 @@ protected: */ void reset_mission_item_reached(); - bool item_contains_position(const struct mission_item_s *item); - /** * Convert a mission item to a position setpoint * diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index f4d06a7084..83882083a5 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -41,6 +41,7 @@ #include "mission_feasibility_checker.h" +#include "mission_block.h" #include #include #include @@ -161,7 +162,9 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { + if (MissionBlock::item_contains_position(&missionitem) && + !geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { + mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i); return false; }