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

geofence: implement circular areas

This commit is contained in:
Beat Küng 2017-05-17 10:35:51 +02:00 committed by Lorenz Meier
parent 6667b6434b
commit ed478f40fd
4 changed files with 143 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/modules/mavlink/mavlink_mission.cpp
View File

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

96
src/modules/navigator/geofence.cpp
View File

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

17
src/modules/navigator/geofence.h
View File

@ -47,6 +47,7 @@
#include <controllib/block/BlockParam.hpp>
#include <controllib/blocks.hpp>
#include <drivers/drv_hrt.h>
#include <geo/geo.h>
#include <px4_defines.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
@ -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_ */

6
src/modules/navigator/navigation.h
View File

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