From fd0be3c412a96455ae4d4197298b79f1717befe5 Mon Sep 17 00:00:00 2001 From: acfloria Date: Fri, 9 Mar 2018 13:49:49 +0100 Subject: [PATCH] Remove MavLink dependency in navigator --- src/modules/navigator/CMakeLists.txt | 2 -- src/modules/navigator/geofence.cpp | 41 ++++++++++++++-------------- src/modules/navigator/navigation.h | 21 ++++++++++++++ 3 files changed, 41 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 378137aa3e..6955a2542e 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -31,8 +31,6 @@ # ############################################################################ -include_directories(${PX4_SOURCE_DIR}/mavlink/include/mavlink) - px4_add_module( MODULE modules__navigator MAIN navigator diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 6bb39088d9..bdff40a1e2 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -47,7 +47,6 @@ #include #include #include -#include #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; } } diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 9f43d995b2..71b5dc0629 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -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 * @{