* Copyright (C) 2014 John Crispin <blogic@openwrt.org>
*/
-#define _BSD_SOURCE
+#define _DEFAULT_SOURCE
#define _XOPEN_SOURCE
+#define _BSD_SOURCE
#include <time.h>
#include <sys/types.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
+#include <errno.h>
+#include <math.h>
#include <string.h>
#include <termios.h>
#include "nmea.h"
#define MAX_NMEA_PARAM 20
-#define MAX_TIME_OFFSET 2
+#define MAX_TIME_OFFSET 5
#define MAX_BAD_TIME 3
struct nmea_param {
} nmea_params[MAX_NMEA_PARAM];
static int nmea_bad_time;
-char longitude[32] = { 0 }, latitude[32] = { 0 }, course[16] = { 0 }, speed[16] = { 0 }, elivation[16] = { 0 };
+char longitude[33] = { 0 }, latitude[33] = { 0 }, course[17] = { 0 }, speed[17] = { 0 }, elevation[17] = { 0 }, satellites[3] = { 0 }, hdop[5] = { 0 };
int gps_valid = 0;
+char gps_fields = 0;
static void
nmea_txt_cb(void)
DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
}
+static void
+do_adjust_clock(struct tm *tm)
+{
+ char tmp[256];
+
+ strftime(tmp, 256, "%Y-%m-%dT%H:%M:%S", tm);
+ DEBUG(3, "date: %s UTC\n", tmp);
+
+ if (adjust_clock) {
+ time_t sec = timegm(tm);
+ struct timeval cur;
+
+ gettimeofday(&cur, NULL);
+
+ if ((sec < 0) || (llabs(cur.tv_sec - sec) > MAX_TIME_OFFSET)) {
+ struct timeval tv = { 0 };
+ tv.tv_sec = sec;
+ if (++nmea_bad_time > MAX_BAD_TIME) {
+ LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp);
+ /* only set datetime if specified by command line argument! */
+ settimeofday(&tv, NULL);
+ }
+ } else {
+ nmea_bad_time = 0;
+ }
+ }
+}
+
+static void
+parse_gps_coords(char *latstr, char *vhem, char *lonstr, char *hhem)
+{
+ float minutes;
+ float degrees;
+ float lat = strtof(latstr, NULL);
+ float lon = strtof(lonstr, NULL);
+
+ degrees = floor(lat / 100.0);
+ minutes = lat - (degrees * 100.0);
+ lat = degrees + minutes / 60.0;
+
+ degrees = floor(lon / 100.0);
+ minutes = lon - (degrees * 100.0);
+ lon = degrees + minutes / 60.0;
+
+ if (*vhem == 'S')
+ lat *= -1.0;
+ if (*hhem == 'W')
+ lon *= -1.0;
+
+ snprintf(latitude, sizeof(latitude), "%f", lat);
+ snprintf(longitude, sizeof(longitude), "%f", lon);
+
+ DEBUG(3, "position: %s %s\n", latitude, longitude);
+ gps_fields |= GPS_FIELD_LAT | GPS_FIELD_LON;
+
+ gps_timestamp();
+}
+
static void
nmea_rmc_cb(void)
{
struct tm tm;
- char tmp[256];
if (*nmea_params[2].str != 'A') {
gps_valid = 0;
memset(&tm, 0, sizeof(tm));
tm.tm_isdst = 1;
- if (!strptime(nmea_params[1].str, "%H%M%S", &tm))
- ERROR("failed to parse time\n");
- else if (!strptime(nmea_params[9].str, "%d%m%y", &tm))
- ERROR("failed to parse date\n");
+ if (sscanf(nmea_params[1].str, "%02d%02d%02d",
+ &tm.tm_hour, &tm.tm_min, &tm.tm_sec) != 3) {
+ ERROR("failed to parse time '%s'\n", nmea_params[1].str);
+ }
+ else if (sscanf(nmea_params[9].str, "%02d%02d%02d",
+ &tm.tm_mday, &tm.tm_mon, &tm.tm_year) != 3) {
+ ERROR("failed to parse date '%s'\n", nmea_params[9].str);
+ }
+ else if (tm.tm_year == 0) {
+ DEBUG(4, "waiting for valid date\n");
+ return;
+ }
else {
- /* is there a libc api for the tz adjustment ? */
- struct timeval tv = { mktime(&tm), 0 };
- struct timeval cur;
+ tm.tm_year += 100; /* year starts with 1900 */
+ tm.tm_mon -= 1; /* month starts with 0 */
- strftime(tmp, 256, "%D %02H:%02M:%02S", &tm);
- DEBUG(3, "date: %s UTC\n", tmp);
+ do_adjust_clock(&tm);
+ }
- tv.tv_sec -= timezone;
- if (daylight)
- tv.tv_sec += 3600;
+ if (strlen(nmea_params[3].str) < 9 || strlen(nmea_params[5].str) < 10) {
+ ERROR("lat/lng have invalid string length %zu<9, %zu<10\n",
+ strlen(nmea_params[3].str), strlen(nmea_params[5].str));
+ } else {
+ parse_gps_coords(nmea_params[3].str, nmea_params[4].str, nmea_params[5].str, nmea_params[6].str);
+ }
+}
- gettimeofday(&cur, NULL);
+static void
+nmea_zda_cb(void)
+{
+ struct tm tm;
- if (abs(cur.tv_sec - tv.tv_sec) > MAX_TIME_OFFSET) {
- if (++nmea_bad_time > MAX_BAD_TIME) {
- LOG("system time differs from GPS time by more than %d seconds. Using %s UTC as the new time\n", MAX_TIME_OFFSET, tmp);
- settimeofday(&tv, NULL);
- }
- } else {
- nmea_bad_time = 0;
- }
+ if (!gps_valid)
+ return;
+
+ memset(&tm, 0, sizeof(tm));
+ tm.tm_isdst = 1;
+
+ if (sscanf(nmea_params[1].str, "%02d%02d%02d",
+ &tm.tm_hour, &tm.tm_min, &tm.tm_sec) != 3) {
+ ERROR("failed to parse time '%s'\n", nmea_params[1].str);
+ return;
}
- if (strlen(nmea_params[3].str) != 9 || strlen(nmea_params[5].str) != 10) {
- ERROR("lat/lng have invalid string length\n");
- } else {
- int latd, latm, lats;
- int lngd, lngm, lngs;
- float flats, flngs;
- DEBUG(4, "position: %s, %s\n",
- nmea_params[3].str, nmea_params[5].str);
- latm = atoi(&nmea_params[3].str[2]);
- nmea_params[3].str[2] = '\0';
- latd = atoi(nmea_params[3].str);
- lats = atoi(&nmea_params[3].str[5]);
- if (*nmea_params[4].str != 'N')
- latm *= -1;
-
- lngm = atoi(&nmea_params[5].str[3]);
- nmea_params[5].str[3] = '\0';
- lngd = atoi(nmea_params[5].str);
- lngs = atoi(&nmea_params[5].str[6]);
- if (*nmea_params[6].str != 'E')
- lngm *= -1;
-
- flats = lats;
- flats *= 60;
- flats /= 10000;
-
- flngs = lngs;
- flngs *= 60;
- flngs /= 10000;
-
-#define ms_to_deg(x, y) (((x * 10000) + y) / 60)
-
- DEBUG(4, "position: %d°%d.%04d, %d°%d.%04d\n",
- latd, latm, lats, lngd, lngm, lngs);
- DEBUG(4, "position: %d°%d'%.1f\" %d°%d'%.1f\"\n",
- latd, latm, flats, lngd, lngm, flngs);
-
- snprintf(latitude, sizeof(latitude), "%d.%04d", latd, ms_to_deg(latm, lats));
- snprintf(longitude, sizeof(longitude), "%d.%04d", lngd, ms_to_deg(lngm, lngs));
- DEBUG(3, "position: %s %s\n", latitude, longitude);
- gps_timestamp();
+ if ((sscanf(nmea_params[2].str, "%02d", &tm.tm_mday) != 1) ||
+ (sscanf(nmea_params[3].str, "%02d", &tm.tm_mon) != 1) ||
+ (sscanf(nmea_params[4].str, "%04d", &tm.tm_year) != 1)) {
+ ERROR("failed to parse time '%s,%s,%s'\n",
+ nmea_params[2].str, nmea_params[3].str, nmea_params[4].str);
+ return;
}
+
+ if (tm.tm_year == 0) {
+ DEBUG(4, "waiting for valid date\n");
+ return;
+ }
+
+ tm.tm_mon -= 1; /* month starts with 0 */
+ tm.tm_year -= 1900; /* full 4-digit year, tm expects years till 1900 */
+
+ do_adjust_clock(&tm);
+}
+
+static void
+nmea_gll_cb(void)
+{
+ if (*nmea_params[6].str != 'A') {
+ gps_valid = 0;
+ DEBUG(4, "waiting for valid signal\n");
+ return;
+ }
+
+ gps_valid = 1;
+
+ parse_gps_coords(nmea_params[1].str, nmea_params[2].str, nmea_params[3].str, nmea_params[4].str);
}
static void
{
if (!gps_valid)
return;
- strncpy(elivation, nmea_params[9].str, sizeof(elivation));
- DEBUG(4, "height: %s\n", elivation);
+ strncpy(satellites, nmea_params[7].str, sizeof(satellites));
+ strncpy(hdop, nmea_params[8].str, sizeof(hdop));
+ strncpy(elevation, nmea_params[9].str, sizeof(elevation));
+ gps_fields |= GPS_FIELD_SAT | GPS_FIELD_HDP | GPS_FIELD_ALT;
+ DEBUG(4, "satellites: %s\n", satellites);
+ DEBUG(4, "HDOP: %s\n", hdop);
+ DEBUG(4, "height: %s\n", elevation);
}
static void
if (!gps_valid)
return;
strncpy(course, nmea_params[1].str, sizeof(course));
- strncpy(speed, nmea_params[6].str, sizeof(speed));
+ strncpy(speed, nmea_params[7].str, sizeof(speed));
+ gps_fields |= GPS_FIELD_COG | GPS_FIELD_SPD;
DEBUG(4, "course: %s\n", course);
DEBUG(4, "speed: %s\n", speed);
}
.msg = "GGA",
.cnt = 14,
.handler = nmea_gga_cb,
+ }, {
+ .msg = "GLL",
+ .cnt = 7,
+ .handler = nmea_gll_cb,
}, {
.msg = "VTG",
.cnt = 9,
.handler = nmea_vtg_cb,
+ }, {
+ .msg = "ZDA",
+ .cnt = 5,
+ .handler = nmea_zda_cb,
},
};
nmea_process(char *a)
{
char *csum;
- int cnt, i;
+ int cnt;
+ unsigned int i;
- if (strncmp(a, "$GP", 3))
+ if (strncmp(a, "$GP", 3) &&
+ strncmp(a, "$GN", 3))
return;
a++;
return;
if (nmea_verify_checksum(a)) {
- ERROR("nmea message has invlid checksum\n");
+ ERROR("nmea message has invalid checksum\n");
return;
}
}
for (i = 0; i < ARRAY_SIZE(nmea_msgs); i++) {
- if (strcmp(nmea_params[0].str, nmea_msgs[i].msg))
+ if (strcmp(nmea_params[0].str, nmea_msgs[i].msg) &&
+ strcmp(nmea_params[3].str, nmea_msgs[i].msg))
continue;
if (nmea_msgs[i].cnt <= cnt)
nmea_msgs[i].handler();
tty = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (tty < 0) {
- ERROR("%s: device open failed\n", dev);
+ ERROR("%s: device open failed: %s\n", dev, strerror(errno));
return -1;
}
projects / project / ugps.git / blobdiff