diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 15:50:49 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 15:50:49 +0100 |
commit | 099c2f5a00f9789533888409f478f4157a5f88b6 (patch) | |
tree | 7df9a3bd8657242e387ab1bf29fc4df325f8b3ca /src/modules/navigator/geofence.cpp | |
parent | 31d1f436adad9dd963797872fa3575319099e940 (diff) | |
download | px4-firmware-099c2f5a00f9789533888409f478f4157a5f88b6.tar.gz px4-firmware-099c2f5a00f9789533888409f478f4157a5f88b6.tar.bz2 px4-firmware-099c2f5a00f9789533888409f478f4157a5f88b6.zip |
geofence: add DMS coordinate format support for textfile
Diffstat (limited to 'src/modules/navigator/geofence.cpp')
-rw-r--r-- | src/modules/navigator/geofence.cpp | 22 |
1 files changed, 19 insertions, 3 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f76597e60..373429c08 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); |