diff options
| -rw-r--r-- | src/lib/geo/geo.c | 24 | ||||
| -rw-r--r-- | src/lib/geo/geo.h | 11 | ||||
| -rw-r--r-- | src/modules/navigator/geofence.cpp | 73 | ||||
| -rw-r--r-- | src/modules/navigator/geofence.h | 64 | ||||
| -rw-r--r-- | src/modules/navigator/module.mk | 3 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 2 |
6 files changed, 141 insertions, 36 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index f64bfb41a..08fe2b696 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -503,27 +503,3 @@ __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 / 1e7d; - double lon = vehicle->lon / 1e7d; - - // 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 5f92e14cf..5f4bce698 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -143,15 +143,4 @@ __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 diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp new file mode 100644 index 000000000..bc5ef44f2 --- /dev/null +++ b/src/modules/navigator/geofence.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Jean Cyr <jean.m.cyr@gmail.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file geofence.cpp + * Provides functions for handling the geofence + */ +#include "geofence.h" + +#include <uORB/topics/vehicle_global_position.h> + +Geofence::Geofence() +{ + +} + +Geofence::~Geofence() +{ + +} + + +bool Geofence::inside(const struct vehicle_global_position_s *vehicle) +{ + + /* 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++) + 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/modules/navigator/geofence.h b/src/modules/navigator/geofence.h new file mode 100644 index 000000000..6d6e6e87c --- /dev/null +++ b/src/modules/navigator/geofence.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Jean Cyr <jean.m.cyr@gmail.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file geofence.h + * Provides functions for handling the geofence + */ + +#ifndef GEOFENCE_H_ +#define GEOFENCE_H_ + +#include <uORB/topics/fence.h> + +class Geofence { +private: + struct fence_s _fence; +public: + Geofence(); + ~Geofence(); + + /** + * 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 + */ + bool inside(const struct vehicle_global_position_s *craft); +}; + + +#endif /* GEOFENCE_H_ */ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 6be4e87a0..b7900e84e 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ navigator_mission.cpp \ - mission_feasibility_checker.cpp + mission_feasibility_checker.cpp \ + geofence.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c88f237ad..a9547355f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -78,6 +78,7 @@ #include "navigator_mission.h" #include "mission_feasibility_checker.h" +#include "geofence.h" /* oddly, ERROR is not defined for c++ */ @@ -157,6 +158,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ + Geofence geofence; struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ |