Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Merge pull request #1523 from bansiesta/geofence_parse_fix

Browse Source 
Geofence parse fix
This commit is contained in:
Thomas Gubler
2014-12-23 09:20:00 +01:00
parent 6a8ebc8afd 82383533c1
commit ba4b8c8e00
1 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+13 -4
View File
@@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename)
while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
/* if the line starts with #, skip */ /* if the line starts with #, skip */
if (line[textStart] == commentChar) if (line[textStart] == commentChar) {
continue; continue;
}
/* if there is only a linefeed, skip it */
if (line[0] == '\n') {
continue;
}
if (gotVertical) { if (gotVertical) {
/* Parse the line as a geofence point */ /* Parse the line as a geofence point */
@@ -291,8 +297,10 @@ Geofence::loadFromFile(const char *filename)
/* Handle degree minute second format */ /* Handle degree minute second format */
float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; 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) if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
warnx("Scanf to parse DMS geofence vertex failed.");
return ERROR; 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); // 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);
@@ -301,9 +309,10 @@ Geofence::loadFromFile(const char *filename)
} else { } else {
/* Handle decimal degree format */ /* Handle decimal degree format */
if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) warnx("Scanf to parse geofence vertex failed.");
return ERROR; return ERROR;
}
} }
if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))