diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 18:49:31 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 21:50:12 +0100 |
commit | 70d4ef480ac5461ef54ac72a54bd335007e233cc (patch) | |
tree | f451a1f1c941e121cad6608a5c74a0742ce38bd5 /src/modules | |
parent | 099c2f5a00f9789533888409f478f4157a5f88b6 (diff) | |
download | px4-firmware-70d4ef480ac5461ef54ac72a54bd335007e233cc.tar.gz px4-firmware-70d4ef480ac5461ef54ac72a54bd335007e233cc.tar.bz2 px4-firmware-70d4ef480ac5461ef54ac72a54bd335007e233cc.zip |
geofence: do not keep fence in memory
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/geofence.cpp | 88 | ||||
-rw-r--r-- | src/modules/navigator/geofence.h | 9 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 2 |
3 files changed, 42 insertions, 57 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 373429c08..1063fd2e2 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -57,9 +57,10 @@ static const int ERROR = -1; Geofence::Geofence() : _fence_pub(-1), _altitude_min(0), - _altitude_max(0) + _altitude_max(0), + _verticesCount(0) { - memset(&_fence, 0, sizeof(_fence)); + } Geofence::~Geofence() @@ -79,59 +80,56 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) bool Geofence::inside(double lat, double lon, float altitude) { - /* Vertical check */ - if (altitude > _altitude_max || altitude < _altitude_min) - return false; + 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) */ - - unsigned int i, j, vertices = _fence.count; - bool c = false; - - // 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; -} + /*Horizontal check */ + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ -bool -Geofence::loadFromDm(unsigned vertices) -{ - struct fence_s temp_fence; + bool c = false; - 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; - } - } + struct fence_vertex_s temp_vertex_i; + struct fence_vertex_s temp_vertex_j; + + /* 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; + } - 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 c; - return _fence.count != 0; + } else { + return true; + } } bool Geofence::valid() { // NULL fence is valid - if (_fence.count == 0) { + if (_verticesCount == 0) { return true; } // 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); return false; } @@ -266,17 +264,11 @@ Geofence::loadFromFile(const char *filename) fclose(fp); - /* Re-Load imported geofence from DM */ + /* Check if import was successful */ if(gotVertical && pointCounter > 0) { - bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES); - if (fence_valid) { - warnx("Geofence: imported and loaded successfully"); - return OK; - } else { - warnx("Geofence: datamanager read error"); - return ERROR; - } + _verticesCount = pointCounter; + warnx("Geofence: imported successfully"); } else { warnx("Geofence: import error"); } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index d834280cc..9c753a11d 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -46,11 +46,12 @@ class Geofence { private: - struct fence_s _fence; orb_advert_t _fence_pub; /**< publish fence topic */ float _altitude_min; float _altitude_max; + + int _verticesCount; public: Geofence(); ~Geofence(); @@ -66,12 +67,6 @@ public: bool inside(const struct vehicle_global_position_s *craft); bool inside(double lat, double lon, float altitude); - - /** - * Load fence parameters. - */ - bool loadFromDm(unsigned vertices); - int clearDm(); bool valid(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 80e006d14..8b5cc4576 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -547,8 +547,6 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES); - /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager |