Merge remote-tracking branch 'jean-m-cyr/master' into navigator_wip

2 files changed, 36 insertions, 0 deletions

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 43105fdba..8aeca5be7 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -465,4 +465,26 @@ __EXPORT float _wrap_360(float bearing)
 	return bearing;
 }
 
+__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence)
+{
+
+	/* Adaptation of algorithm originally presented as
+	 * PNPOLY - Point Inclusion in Polygon Test
+	 * W. Randolph Franklin (WRF) */
+
+	unsigned int i, j, vertices = fence->count;
+	bool c = false;
+	double lat = vehicle->lat / 1e7;
+	double lon = vehicle->lon / 1e7;
+
+	// skip vertex 0 (return point)
+	for (i = 0, j = vertices - 1; i < vertices; j = i++)
+		if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) &&
+		    (lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) /
+		     (fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat))
+			c = !c;
+	return c;
+}
+
+
 
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 123ff80f1..6bce9309b 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -47,6 +47,9 @@
 
 #pragma once
 
+#include "uORB/topics/fence.h"
+#include "uORB/topics/vehicle_global_position.h"
+
 __BEGIN_DECLS
 
 #include <stdbool.h>
@@ -126,4 +129,15 @@ __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
 
+/**
+ * Return whether craft is inside geofence.
+ *
+ * Calculate whether point is inside arbitrary polygon
+ * @param craft pointer craft coordinates
+ * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
+ * @return true: craft is inside fence, false:craft is outside fence
+ */
+__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence);
+
+
 __END_DECLS