Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Remove MavLink dependency in navigator

Browse Source
This commit is contained in:
acfloria
2018-03-09 13:49:49 +01:00
committed by Lorenz Meier
parent dbdb2c9c22
commit fd0be3c412
3 changed files with 41 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/CMakeLists.txt
-2
View File
@@ -31,8 +31,6 @@
#
############################################################################
include_directories(${PX4_SOURCE_DIR}/mavlink/include/mavlink)
px4_add_module(
MODULE modules__navigator
MAIN navigator
src/modules/navigator/geofence.cpp
+20 -21
View File
@@ -47,7 +47,6 @@
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <systemlib/mavlink_log.h>
#include <v2.0/common/mavlink.h>
#include "navigator.h"
@@ -117,18 +116,18 @@ void Geofence::_updateFence()
}
switch (mission_fence_point.nav_cmd) {
case MAV_CMD_NAV_FENCE_RETURN_POINT:
case NAV_CMD_FENCE_RETURN_POINT:
// TODO: do we need to store this?
++current_seq;
break;
case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION:
case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION:
case NAV_CMD_FENCE_CIRCLE_INCLUSION:
case NAV_CMD_FENCE_CIRCLE_EXCLUSION:
is_circle_area = true;
/* FALLTHROUGH */
case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION:
case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION:
if (!is_circle_area && mission_fence_point.vertex_count == 0) {
++current_seq; // avoid endless loop
PX4_ERR("Polygon with 0 vertices. Skipping");
@@ -320,7 +319,7 @@ bool Geofence::checkPolygons(double lat, double lon, float altitude)
bool had_inclusion_areas = false;
for (int polygon_idx = 0; polygon_idx < _num_polygons; ++polygon_idx) {
if (_polygons[polygon_idx].fence_type == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION) {
if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) {
bool inside = insideCircle(_polygons[polygon_idx], lat, lon, altitude);
if (inside) {
@@ -329,7 +328,7 @@ bool Geofence::checkPolygons(double lat, double lon, float altitude)
had_inclusion_areas = true;
} else if (_polygons[polygon_idx].fence_type == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) {
} else if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
bool inside = insideCircle(_polygons[polygon_idx], lat, lon, altitude);
if (inside) {
@@ -339,7 +338,7 @@ bool Geofence::checkPolygons(double lat, double lon, float altitude)
} else { // it's a polygon
bool inside = insidePolygon(_polygons[polygon_idx], lat, lon, altitude);
if (_polygons[polygon_idx].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) {
if (_polygons[polygon_idx].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) {
if (inside) {
inside_inclusion = true;
}
@@ -383,9 +382,9 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon,
break;
}
if (temp_vertex_i.frame != MAV_FRAME_GLOBAL && temp_vertex_i.frame != MAV_FRAME_GLOBAL_INT
&& temp_vertex_i.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT
&& temp_vertex_i.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
if (temp_vertex_i.frame != NAV_FRAME_GLOBAL && temp_vertex_i.frame != NAV_FRAME_GLOBAL_INT
&& temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT
&& temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
// TODO: handle different frames
PX4_ERR("Frame type %i not supported", (int)temp_vertex_i.frame);
break;
@@ -412,9 +411,9 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
return false;
}
if (circle_point.frame != MAV_FRAME_GLOBAL && circle_point.frame != MAV_FRAME_GLOBAL_INT
&& circle_point.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT
&& circle_point.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
if (circle_point.frame != NAV_FRAME_GLOBAL && circle_point.frame != NAV_FRAME_GLOBAL_INT
&& circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT
&& circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
// TODO: handle different frames
PX4_ERR("Frame type %i not supported", (int)circle_point.frame);
return false;
@@ -482,8 +481,8 @@ Geofence::loadFromFile(const char *filename)
if (gotVertical) {
/* Parse the line as a geofence point */
mission_fence_point_s vertex;
vertex.frame = MAV_FRAME_GLOBAL;
vertex.nav_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION;
vertex.frame = NAV_FRAME_GLOBAL;
vertex.nav_cmd = NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION;
vertex.vertex_count = 0; // this will be filled in a second pass
vertex.alt = 0; // alt is not used
@@ -588,19 +587,19 @@ void Geofence::printStatus()
for (int i = 0; i < _num_polygons; ++i) {
total_num_vertices += _polygons[i].vertex_count;
if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) {
if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) {
++num_inclusion_polygons;
}
if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) {
++num_exclusion_polygons;
}
if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION) {
if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) {
++num_inclusion_circles;
}
if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) {
if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) {
++num_exclusion_circles;
}
}
src/modules/navigator/navigation.h
+21
View File
@@ -90,6 +90,11 @@ enum NAV_CMD {
NAV_CMD_VIDEO_START_CAPTURE = 2500,
NAV_CMD_VIDEO_STOP_CAPTURE = 2501,
NAV_CMD_DO_VTOL_TRANSITION = 3000,
NAV_CMD_FENCE_RETURN_POINT = 5000,
NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION = 5001,
NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION = 5002,
NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003,
NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004,
NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
};
@@ -98,6 +103,22 @@ enum ORIGIN {
ORIGIN_ONBOARD
};
/* compatible to mavlink MAV_FRAME */
enum NAV_FRAME {
NAV_FRAME_GLOBAL = 0,
NAV_FRAME_LOCAL_NED = 1,
NAV_FRAME_MISSION = 2,
NAV_FRAME_GLOBAL_RELATIVE_ALT = 3,
NAV_FRAME_LOCAL_ENU = 4,
NAV_FRAME_GLOBAL_INT = 5,
NAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6,
NAV_FRAME_LOCAL_OFFSET_NED = 7,
NAV_FRAME_BODY_NED = 8,
NAV_FRAME_BODY_OFFSET_NED = 9,
NAV_FRAME_GLOBAL_TERRAIN_ALT = 10,
NAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11
};
/**
* @addtogroup topics
* @{