dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MissionFeasibilityChecker geofence ignore NAV_CMD_DO_* 

-closes #4040
This commit is contained in:
Daniel Agar 2016-03-20 23:35:29 -04:00 committed by Andreas Antener
parent c37cf7abd4
commit 408d147a18
3 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/navigator/mission_block.cpp
View File

@ -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;
}

4
src/modules/navigator/mission_block.h
View File

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

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

@ -41,6 +41,7 @@
#include "mission_feasibility_checker.h"
#include "mission_block.h"
#include <geo/geo.h>
#include <math.h>
#include <mathlib/mathlib.h>
@ -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;
}