From 099c2f5a00f9789533888409f478f4157a5f88b6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 15:50:49 +0100 Subject: [PATCH] geofence: add DMS coordinate format support for textfile --- src/modules/navigator/geofence.cpp | 22 +++++++++++++++++++--- src/modules/navigator/navigator_main.cpp | 2 +- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f76597e607..373429c084 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -225,12 +225,28 @@ Geofence::loadFromFile(const char *filename) /* 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 the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */ + if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') { + /* Handle degree minute second format */ + float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; + if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) + return ERROR; + +// warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); + + vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f; + vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f; + + } else { + /* Handle decimal degree format */ + + 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; + return ERROR; warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e52ef16fa5..80e006d147 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1515,7 +1515,7 @@ void Navigator::load_fence_from_file(const char *filename) static void usage() { - errx(1, "usage: navigator {start|stop|status|fence}"); + errx(1, "usage: navigator {start|stop|status|fence|fencefile}"); } int navigator_main(int argc, char *argv[])