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

create geofence class and start moving fence functionality to this class

This commit is contained in:
Thomas Gubler 2014-01-02 14:18:02 +01:00
parent 4191ae33c2
commit dca6d97a52
6 changed files with 141 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/lib/geo/geo.c
View File

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

11
src/lib/geo/geo.h
View File

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

73
src/modules/navigator/geofence.cpp Normal file
View File

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

64
src/modules/navigator/geofence.h Normal file
View File

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

3
src/modules/navigator/module.mk
View File

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

2
src/modules/navigator/navigator_main.cpp
View File

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