diff options
| -rw-r--r-- | src/modules/navigator/geofence.cpp | 101 | ||||
| -rw-r--r-- | src/modules/navigator/geofence.h | 13 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 31 |
3 files changed, 140 insertions, 5 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4a0528b16..199ccb41b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -43,8 +43,21 @@ #include <dataman/dataman.h> #include <systemlib/err.h> #include <stdlib.h> +#include <stdio.h> +#include <ctype.h> +#include <nuttx/config.h> +#include <unistd.h> -Geofence::Geofence() : _fence_pub(-1) + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +Geofence::Geofence() : _fence_pub(-1), + _altitude_min(0), + _altitude_max(0) { memset(&_fence, 0, sizeof(_fence)); } @@ -77,7 +90,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) } bool -Geofence::load(unsigned vertices) +Geofence::loadFromDm(unsigned vertices) { struct fence_s temp_fence; @@ -164,3 +177,87 @@ Geofence::publishFence(unsigned vertices) orb_publish(ORB_ID(fence), _fence_pub, &vertices); } +int +Geofence::loadFromFile(const char *filename) +{ + FILE *fp; + char line[120]; + int pointCounter = 0; + bool gotVertical = false; + const char commentChar = '#'; + + /* Make sure no data is left in the datamanager */ + clearDm(); + + /* open the mixer definition file */ + fp = fopen(GEOFENCE_FILENAME, "r"); + if (fp == NULL) { + return ERROR; + } + + /* create geofence points from valid lines and store in DM */ + for (;;) { + + /* get a line, bail on error/EOF */ + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* Trim leading whitespace */ + size_t textStart = 0; + while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; + + /* if the line starts with #, skip */ + if (line[textStart] == commentChar) + continue; + + if (gotVertical) { + /* Parse the line as a geofence point */ + struct fence_vertex_s vertex; + + if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) + return ERROR; + + + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) + return ERROR; + + warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + + pointCounter++; + } else { + /* Parse the line as the vertical limits */ + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) + return ERROR; + + + warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); + gotVertical = true; + } + + + } + + fclose(fp); + + /* Re-Load imported geofence from DM */ + 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; + } + } else { + warnx("Geofence: import error"); + } + + return ERROR; +} + +int Geofence::clearDm() +{ + dm_clear(DM_KEY_FENCE_POINTS); +} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 8f3a07b02..8a1d06e71 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,10 +42,15 @@ #include <uORB/topics/fence.h> +#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" + class Geofence { private: - struct fence_s _fence; + struct fence_s _fence; orb_advert_t _fence_pub; /**< publish fence topic */ + + float _altitude_min; + float _altitude_max; public: Geofence(); ~Geofence(); @@ -64,7 +69,9 @@ public: /** * Load fence parameters. */ - bool load(unsigned vertices); + bool loadFromDm(unsigned vertices); + + int clearDm(); bool valid(); @@ -74,6 +81,8 @@ public: void addPoint(int argc, char *argv[]); void publishFence(unsigned vertices); + + int loadFromFile(const char *filename); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 354fa733b..a226aac7c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -75,6 +75,8 @@ #include <mathlib/mathlib.h> #include <dataman/dataman.h> #include <mavlink/mavlink_log.h> +#include <sys/types.h> +#include <sys/stat.h> #include "navigator_mission.h" #include "mission_feasibility_checker.h" @@ -124,6 +126,11 @@ public: */ void add_fence_point(int argc, char *argv[]); + /** + * Load fence from file + */ + void load_fence_from_file(const char *filename); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -497,7 +504,22 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = _geofence.load(GEOFENCE_MAX_VERTICES); + _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 + */ + struct stat buffer; + if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + warnx("Try to load geofence.txt"); + _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { + if (_geofence.clearDm() > 0 ) + warnx("Geofence cleared"); + else + warnx("Could not clear geofence"); + } /* * do subscriptions @@ -1252,6 +1274,11 @@ void Navigator::add_fence_point(int argc, char *argv[]) _geofence.addPoint(argc, argv); } +void Navigator::load_fence_from_file(const char *filename) +{ + _geofence.loadFromFile(filename); +} + static void usage() { @@ -1297,6 +1324,8 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); |