geofence: implement circular areas

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,
+				if (!check_failed) {
-							sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s);
+					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);
 				}
+
 			}
 			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;
 

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",
+	PX4_INFO("Geofence: %i inclusion, %i exclusion polygons, %i inclusion, %i exclusion circles, %i total vertices",
-		 num_inclusion_polygons, num_exclusion_polygons, total_num_vertices);
+		 num_inclusion_polygons, num_exclusion_polygons, num_inclusion_circles, num_exclusion_circles,
+		 total_num_vertices);
 }

src/modules/navigator/geofence.h

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

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