diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 13:37:49 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 13:37:49 +0100 |
commit | a48264d5d44018412b9443b245e6974d3f54b20d (patch) | |
tree | 4f3cc3d330ab87a14763edc7c331e02f8e1103dc /src/modules/navigator/navigator_main.cpp | |
parent | 429a11a21d25e34ca711b2c0debb2ac3e84c45ca (diff) | |
download | px4-firmware-a48264d5d44018412b9443b245e6974d3f54b20d.tar.gz px4-firmware-a48264d5d44018412b9443b245e6974d3f54b20d.tar.bz2 px4-firmware-a48264d5d44018412b9443b245e6974d3f54b20d.zip |
navigator: load geofence from textfile
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 31 |
1 files changed, 30 insertions, 1 deletions
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(); |