X-Git-Url: http://git.openwrt.org/?a=blobdiff_plain;f=nmea.c;h=195753e80773d5feb940d7080fd8c3851ecf184c;hb=HEAD;hp=5a691b92b669f48751da85d1e343babd76afd480;hpb=2c9ea76af28d3d5a506676380ff2890e8b6fd8d3;p=project%2Fugps.git diff --git a/nmea.c b/nmea.c index 5a691b9..195753e 100644 --- a/nmea.c +++ b/nmea.c @@ -16,8 +16,9 @@ * Copyright (C) 2014 John Crispin */ -#define _BSD_SOURCE +#define _DEFAULT_SOURCE #define _XOPEN_SOURCE +#define _BSD_SOURCE #include #include @@ -30,6 +31,8 @@ #include #include #include +#include +#include #include #include @@ -40,7 +43,7 @@ #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 { @@ -49,8 +52,9 @@ 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) @@ -63,11 +67,68 @@ 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; @@ -79,76 +140,81 @@ nmea_rmc_cb(void) 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 @@ -156,8 +222,13 @@ nmea_gga_cb(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 @@ -166,7 +237,8 @@ nmea_vtg_cb(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); } @@ -188,10 +260,18 @@ static struct nmea_msg { .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, }, }; @@ -237,9 +317,11 @@ static void 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++; @@ -259,7 +341,8 @@ nmea_process(char *a) } 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(); @@ -314,7 +397,7 @@ nmea_open(char *dev, struct ustream_fd *s, speed_t speed) 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; }