From ed478f40fdf69b1f779f4154f50b835dd1102cbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=83=C2=BCng?= Date: Wed, 17 May 2017 10:35:51 +0200 Subject: [PATCH] geofence: implement circular areas --- src/modules/mavlink/mavlink_mission.cpp | 49 ++++++++++--- src/modules/navigator/geofence.cpp | 96 +++++++++++++++++++++---- src/modules/navigator/geofence.h | 17 ++++- src/modules/navigator/navigation.h | 6 +- 4 files changed, 143 insertions(+), 25 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 2b316a8583..2257e24d30 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -336,7 +336,14 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t mission_item.lat = mission_fence_point.lat; mission_item.lon = mission_fence_point.lon; mission_item.altitude = mission_fence_point.alt; - mission_item.vertex_count = mission_fence_point.vertex_count; + + if (mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || + mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + mission_item.vertex_count = mission_fence_point.vertex_count; + + } else { + mission_item.circle_radius = mission_fence_point.circle_radius; + } } break; @@ -1050,17 +1057,28 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_fence_point.lat = mission_item.lat; mission_fence_point.lon = mission_item.lon; mission_fence_point.alt = mission_item.altitude; - mission_fence_point.vertex_count = mission_item.vertex_count; + + if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + mission_fence_point.vertex_count = mission_item.vertex_count; + + if (mission_item.vertex_count < 3) { // feasibility check + PX4_ERR("Fence: too few vertices"); + check_failed = true; + update_geofence_count(0); + } + + } else { + mission_fence_point.circle_radius = mission_item.circle_radius; + } + mission_fence_point.frame = mission_item.frame; - write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_fence_point, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); - - if (mission_item.vertex_count < 3) { // feasibility check - PX4_ERR("Fence: too few vertices"); - check_failed = true; - update_geofence_count(0); + if (!check_failed) { + write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_fence_point, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); } + } break; @@ -1242,7 +1260,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->altitude_is_relative = true; } - /* this field is shared with pitch_min in memory and + /* this field is shared with pitch_min (and circle_radius for geofence) in memory and * exclusive in the MAVLink spec. Set it to 0 first * and then set minimum pitch later only for the * corresponding item @@ -1305,6 +1323,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->vertex_count = (uint16_t)(mavlink_mission_item->param1 + 0.5f); break; + case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION: + case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION: + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + mission_item->circle_radius = mavlink_mission_item->param1; + break; + case MAV_CMD_NAV_RALLY_POINT: mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; break; @@ -1526,6 +1550,11 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->param1 = (float)mission_item->vertex_count; break; + case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION: + case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION: + mavlink_mission_item->param1 = mission_item->circle_radius; + break; + case MAV_CMD_NAV_RALLY_POINT: break; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 6671ff7d82..7b0f554dc6 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -91,6 +91,7 @@ void Geofence::updateFence() while (current_seq <= num_fence_items) { mission_fence_point_s mission_fence_point; + bool is_circle_area = false; if (dm_read(DM_KEY_FENCE_POINTS, current_seq, &mission_fence_point, sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { @@ -104,9 +105,14 @@ void Geofence::updateFence() ++current_seq; break; + case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION: + case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION: + is_circle_area = true; + + /* FALLTHROUGH */ case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION: - if (mission_fence_point.vertex_count == 0) { + if (!is_circle_area && mission_fence_point.vertex_count == 0) { ++current_seq; // avoid endless loop PX4_ERR("Polygon with 0 vertices. Skipping"); @@ -135,8 +141,16 @@ void Geofence::updateFence() PolygonInfo &polygon = _polygons[_num_polygons]; polygon.dataman_index = current_seq; polygon.fence_type = mission_fence_point.nav_cmd; - polygon.vertex_count = mission_fence_point.vertex_count; - current_seq += mission_fence_point.vertex_count; + + if (is_circle_area) { + polygon.circle_radius = mission_fence_point.circle_radius; + current_seq += 1; + + } else { + polygon.vertex_count = mission_fence_point.vertex_count; + current_seq += mission_fence_point.vertex_count; + } + ++_num_polygons; } @@ -266,29 +280,47 @@ bool Geofence::checkPolygons(double lat, double lon, float altitude) } } - /* Horizontal check: iterate all polygons */ + /* Horizontal check: iterate all polygons & circles */ bool outside_exclusion = true; bool inside_inclusion = false; - bool had_inclusion_polygons = false; + bool had_inclusion_areas = false; for (int polygon_idx = 0; polygon_idx < _num_polygons; ++polygon_idx) { - bool inside = insidePolygon(_polygons[polygon_idx], lat, lon, altitude); + if (_polygons[polygon_idx].fence_type == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION) { + bool inside = insideCircle(_polygons[polygon_idx], lat, lon, altitude); - if (_polygons[polygon_idx].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) { if (inside) { inside_inclusion = true; } - had_inclusion_polygons = true; + had_inclusion_areas = true; + + } else if (_polygons[polygon_idx].fence_type == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) { + bool inside = insideCircle(_polygons[polygon_idx], lat, lon, altitude); - } else { // exclusion if (inside) { outside_exclusion = false; } + + } 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 (inside) { + inside_inclusion = true; + } + + had_inclusion_areas = true; + + } else { // exclusion + if (inside) { + outside_exclusion = false; + } + } } } - return (!had_inclusion_polygons || inside_inclusion) && outside_exclusion; + return (!had_inclusion_areas || inside_inclusion) && outside_exclusion; } bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude) @@ -333,6 +365,36 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, return c; } +bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude) +{ + + mission_fence_point_s circle_point; + + if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index, &circle_point, + sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + PX4_ERR("dm_read failed"); + 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) { + // TODO: handle different frames + PX4_ERR("Frame type %i not supported", (int)circle_point.frame); + return false; + } + + if (!map_projection_initialized(&_projection_reference)) { + map_projection_init(&_projection_reference, lat, lon); + } + + float x1, y1, x2, y2; + map_projection_project(&_projection_reference, lat, lon, &x1, &y1); + map_projection_project(&_projection_reference, circle_point.lat, circle_point.lon, &x2, &y2); + float dx = x1 - x2, dy = y1 - y2; + return dx * dx + dy * dy < circle_point.circle_radius * circle_point.circle_radius; +} + bool Geofence::valid() { @@ -485,6 +547,7 @@ bool Geofence::isHomeRequired() void Geofence::printStatus() { int num_inclusion_polygons = 0, num_exclusion_polygons = 0, total_num_vertices = 0; + int num_inclusion_circles = 0, num_exclusion_circles = 0; for (int i = 0; i < _num_polygons; ++i) { total_num_vertices += _polygons[i].vertex_count; @@ -496,8 +559,17 @@ void Geofence::printStatus() if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { ++num_exclusion_polygons; } + + if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION) { + ++num_inclusion_circles; + } + + if (_polygons[i].fence_type == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) { + ++num_exclusion_circles; + } } - PX4_INFO("Geofence: %i inclusion, %i exclusion polygons, %i total vertices", - num_inclusion_polygons, num_exclusion_polygons, total_num_vertices); + PX4_INFO("Geofence: %i inclusion, %i exclusion polygons, %i inclusion, %i exclusion circles, %i total vertices", + num_inclusion_polygons, num_exclusion_polygons, num_inclusion_circles, num_exclusion_circles, + total_num_vertices); } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index bd093bda7b..99b299c6a3 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -145,13 +146,18 @@ private: float _altitude_max{0.0f}; struct PolygonInfo { - uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* + uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region) uint16_t dataman_index; - uint16_t vertex_count; + union { + uint16_t vertex_count; + float circle_radius; + }; }; PolygonInfo *_polygons{nullptr}; int _num_polygons{0}; + map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] + /* Params */ control::BlockParamInt _param_action; control::BlockParamInt _param_altitude_mode; @@ -190,6 +196,13 @@ private: * @return true if within polygon */ bool insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude); + + /** + * Check if a single point is within a circle + * @param polygon must be a circle! + * @return true if within polygon the circle + */ + bool insideCircle(const PolygonInfo &polygon, double lat, double lon, float altitude); }; #endif /* GEOFENCE_H_ */ diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 8786f06c21..a1f2264231 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -117,6 +117,7 @@ struct mission_item_s { union { float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ + float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */ }; float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */ @@ -163,7 +164,10 @@ struct mission_fence_point_s { double lon; float alt; uint16_t nav_cmd; /**< navigation command (one of MAV_CMD_NAV_FENCE_*) */ - uint16_t vertex_count; /**< number of vertices in this polygon */ + union { + uint16_t vertex_count; /**< number of vertices in this polygon */ + float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */ + }; uint8_t frame; /**< MAV_FRAME */ };