X-Git-Url: http://git.openwrt.org/?a=blobdiff_plain;f=nmea.c;h=195753e80773d5feb940d7080fd8c3851ecf184c;hb=HEAD;hp=61c57ab883da6e47897306885c923e2b90a23878;hpb=db40e3a22599641be68c88ece287d3fbbacd8a45;p=project%2Fugps.git diff --git a/nmea.c b/nmea.c index 61c57ab..195753e 100644 --- a/nmea.c +++ b/nmea.c @@ -52,8 +52,9 @@ struct nmea_param { } nmea_params[MAX_NMEA_PARAM]; static int nmea_bad_time; -char longitude[33] = { 0 }, latitude[33] = { 0 }, course[17] = { 0 }, speed[17] = { 0 }, elevation[17] = { 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) @@ -66,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; @@ -90,61 +148,73 @@ nmea_rmc_cb(void) &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 { tm.tm_year += 100; /* year starts with 1900 */ tm.tm_mon -= 1; /* month starts with 0 */ - 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) || (abs(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; - } - } + do_adjust_clock(&tm); } 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 { - float minutes; - float degrees; - float lat = strtof(nmea_params[3].str, NULL); - float lon = strtof(nmea_params[5].str, NULL); + parse_gps_coords(nmea_params[3].str, nmea_params[4].str, nmea_params[5].str, nmea_params[6].str); + } +} - degrees = floor(lat / 100.0); - minutes = lat - (degrees * 100.0); - lat = degrees + minutes / 60.0; +static void +nmea_zda_cb(void) +{ + struct tm tm; + + 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 ((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; + } - degrees = floor(lon / 100.0); - minutes = lon - (degrees * 100.0); - lon = degrees + minutes / 60.0; + if (tm.tm_year == 0) { + DEBUG(4, "waiting for valid date\n"); + return; + } - if (*nmea_params[4].str == 'S') - lat *= -1.0; - if (*nmea_params[6].str == 'W') - lon *= -1.0; + tm.tm_mon -= 1; /* month starts with 0 */ + tm.tm_year -= 1900; /* full 4-digit year, tm expects years till 1900 */ - snprintf(latitude, sizeof(latitude), "%f", lat); - snprintf(longitude, sizeof(longitude), "%f", lon); + do_adjust_clock(&tm); +} - DEBUG(3, "position: %s %s\n", latitude, longitude); - gps_timestamp(); +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 @@ -152,7 +222,12 @@ nmea_gga_cb(void) { if (!gps_valid) return; + 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); } @@ -163,6 +238,7 @@ nmea_vtg_cb(void) return; strncpy(course, nmea_params[1].str, sizeof(course)); 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); } @@ -184,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, }, }; @@ -236,7 +320,8 @@ nmea_process(char *a) int cnt; unsigned int i; - if (strncmp(a, "$GP", 3)) + if (strncmp(a, "$GP", 3) && + strncmp(a, "$GN", 3)) return; a++; @@ -256,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();