geofence: do not keep fence in memory

This commit is contained in:
Thomas Gubler 2014-01-04 18:49:31 +01:00
parent 099c2f5a00
commit 70d4ef480a
3 changed files with 41 additions and 56 deletions

View File

@ -57,9 +57,10 @@ static const int ERROR = -1;
Geofence::Geofence() : _fence_pub(-1), Geofence::Geofence() : _fence_pub(-1),
_altitude_min(0), _altitude_min(0),
_altitude_max(0) _altitude_max(0),
_verticesCount(0)
{ {
memset(&_fence, 0, sizeof(_fence));
} }
Geofence::~Geofence() Geofence::~Geofence()
@ -79,6 +80,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside(double lat, double lon, float altitude)
{ {
if (valid()) {
/* Vertical check */ /* Vertical check */
if (altitude > _altitude_max || altitude < _altitude_min) if (altitude > _altitude_max || altitude < _altitude_min)
return false; return false;
@ -88,50 +90,46 @@ bool Geofence::inside(double lat, double lon, float altitude)
* PNPOLY - Point Inclusion in Polygon Test * PNPOLY - Point Inclusion in Polygon Test
* W. Randolph Franklin (WRF) */ * W. Randolph Franklin (WRF) */
unsigned int i, j, vertices = _fence.count;
bool c = false; bool c = false;
// skip vertex 0 (return point) struct fence_vertex_s temp_vertex_i;
for (i = 0, j = vertices - 1; i < vertices; j = i++) struct fence_vertex_s temp_vertex_j;
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;
}
bool /* Red until fence is finished */
Geofence::loadFromDm(unsigned vertices) 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)) {
struct fence_s temp_fence;
unsigned i;
for (i = 0; i < vertices; i++) {
if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break; break;
} }
if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
break;
} }
temp_fence.count = i; // 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;
}
if (valid()) }
memcpy(&_fence, &temp_fence, sizeof(_fence));
else
warnx("Invalid fence file, ignored!");
return _fence.count != 0; return c;
} else {
return true;
}
} }
bool bool
Geofence::valid() Geofence::valid()
{ {
// NULL fence is valid // NULL fence is valid
if (_fence.count == 0) { if (_verticesCount == 0) {
return true; return true;
} }
// Otherwise // Otherwise
if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) { if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
return false; return false;
} }
@ -266,17 +264,11 @@ Geofence::loadFromFile(const char *filename)
fclose(fp); fclose(fp);
/* Re-Load imported geofence from DM */ /* Check if import was successful */
if(gotVertical && pointCounter > 0) if(gotVertical && pointCounter > 0)
{ {
bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES); _verticesCount = pointCounter;
if (fence_valid) { warnx("Geofence: imported successfully");
warnx("Geofence: imported and loaded successfully");
return OK;
} else {
warnx("Geofence: datamanager read error");
return ERROR;
}
} else { } else {
warnx("Geofence: import error"); warnx("Geofence: import error");
} }

View File

@ -46,11 +46,12 @@
class Geofence { class Geofence {
private: private:
struct fence_s _fence;
orb_advert_t _fence_pub; /**< publish fence topic */ orb_advert_t _fence_pub; /**< publish fence topic */
float _altitude_min; float _altitude_min;
float _altitude_max; float _altitude_max;
int _verticesCount;
public: public:
Geofence(); Geofence();
~Geofence(); ~Geofence();
@ -66,12 +67,6 @@ public:
bool inside(const struct vehicle_global_position_s *craft); bool inside(const struct vehicle_global_position_s *craft);
bool inside(double lat, double lon, float altitude); bool inside(double lat, double lon, float altitude);
/**
* Load fence parameters.
*/
bool loadFromDm(unsigned vertices);
int clearDm(); int clearDm();
bool valid(); bool valid();

View File

@ -547,8 +547,6 @@ Navigator::task_main()
mavlink_log_info(_mavlink_fd, "[navigator] started"); mavlink_log_info(_mavlink_fd, "[navigator] started");
_fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES);
/* Try to load the geofence: /* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file * if /fs/microsd/etc/geofence.txt load from this file
* else clear geofence data in datamanager * else clear geofence data in datamanager