navigator: add pre mission geofence check

This commit is contained in:
Thomas Gubler
2014-01-04 14:46:17 +01:00
parent a65de1e0b9
commit ec60a254d2
3 changed files with 26 additions and 3 deletions
+7 -2
View File
@@ -70,15 +70,20 @@ Geofence::~Geofence()
bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
{
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
return inside(lat, lon);
}
bool Geofence::inside(double lat, double lon)
{
/* 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 / 1e7d;
double lon = vehicle->lon / 1e7d;
// skip vertex 0 (return point)
for (i = 0, j = vertices - 1; i < vertices; j = i++)