fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 16:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator/geofence: add isEmpty() function and checks

Browse Source
This commit is contained in:
Thomas Gubler
2014-01-04 22:01:31 +01:00
parent 70d4ef480a
commit 819822e172
2 changed files with 37 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+34 -28
View File
@@ -81,41 +81,48 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
bool Geofence::inside(double lat, double lon, float altitude)
{
if (valid()) {
/* Vertical check */
if (altitude > _altitude_max || altitude < _altitude_min)
return false;
/*Horizontal check */
/* Adaptation of algorithm originally presented as
* PNPOLY - Point Inclusion in Polygon Test
* W. Randolph Franklin (WRF) */
if (!isEmpty()) {
/* Vertical check */
if (altitude > _altitude_max || altitude < _altitude_min)
return false;
bool c = false;
/*Horizontal check */
/* Adaptation of algorithm originally presented as
* PNPOLY - Point Inclusion in Polygon Test
* W. Randolph Franklin (WRF) */
struct fence_vertex_s temp_vertex_i;
struct fence_vertex_s temp_vertex_j;
bool c = false;
struct fence_vertex_s temp_vertex_i;
struct fence_vertex_s temp_vertex_j;
/* Red until fence is finished */
for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
// skip vertex 0 (return point)
if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
(temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
c = !c;
}
/* Red until fence is finished */
for (int i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
}
// skip vertex 0 (return point)
if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
(temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
c = !c;
}
return c;
} else {
/* Empty fence --> accept all points */
return true;
}
return c;
} else {
/* Invalid fence --> accept all points */
return true;
}
}
@@ -124,9 +131,8 @@ bool
Geofence::valid()
{
// NULL fence is valid
if (_verticesCount == 0) {
if (isEmpty())
return true;
}
// Otherwise
if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
src/modules/navigator/geofence.h
+3 -1
View File
@@ -51,7 +51,7 @@ private:
float _altitude_min;
float _altitude_max;
int _verticesCount;
unsigned _verticesCount;
public:
Geofence();
~Geofence();
@@ -79,6 +79,8 @@ public:
void publishFence(unsigned vertices);
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
};