fix reading of speed

[project/ugps.git] / nmea.c

diff --git a/nmea.c b/nmea.c
index 2478f9fe9853ee8ff7ea6c4bcc64a654cca7d088..9c4bde8130c89f6781555543fbe218c6dc176a08 100644 (file)
--- a/nmea.c
+++ b/nmea.c
@@ -30,6 +30,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <unistd.h>
+#include <errno.h>
 
 #include <string.h>
 #include <termios.h>
@@ -49,7 +50,7 @@ struct nmea_param {
 } nmea_params[MAX_NMEA_PARAM];
 
 static int nmea_bad_time;
-char longitude[32] = { 0 }, lattitude[32] = { 0 }, course[16] = { 0 }, speed[16] = { 0 }, elivation[16] = { 0 };
+char longitude[32] = { 0 }, latitude[32] = { 0 }, course[16] = { 0 }, speed[16] = { 0 }, elivation[16] = { 0 };
 int gps_valid = 0;
 
 static void
@@ -60,7 +61,7 @@ nmea_txt_cb(void)
        if (nmea_params[3].num < 0 || nmea_params[3].num > 2)
                nmea_params[3].num = 0;
 
-       LOG("%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
+       DEBUG(3, "%s: %s\n", ids[nmea_params[3].num], nmea_params[4].str);
 }
 
 static void
@@ -71,7 +72,7 @@ nmea_rmc_cb(void)
 
        if (*nmea_params[2].str != 'A') {
                gps_valid = 0;
-               fprintf(stderr, "waiting for valid signal\n");
+               DEBUG(4, "waiting for valid signal\n");
                return;
        }
 
@@ -89,7 +90,7 @@ nmea_rmc_cb(void)
                struct timeval cur;
 
                strftime(tmp, 256, "%D %02H:%02M:%02S", &tm);
-               LOG("date: %s UTC\n", tmp);
+               DEBUG(3, "date: %s UTC\n", tmp);
 
                tv.tv_sec -= timezone;
                if (daylight)
@@ -107,13 +108,14 @@ nmea_rmc_cb(void)
                }
        }
 
-       if (strlen(nmea_params[3].str) != 9 || strlen(nmea_params[5].str) != 10) {
-               ERROR("lat/lng have invalid string length\n");
+       if (strlen(nmea_params[3].str) < 9 || strlen(nmea_params[5].str) < 10) {
+               ERROR("lat/lng have invalid string length %d<9, %d<10\n",
+                      strlen(nmea_params[3].str), strlen(nmea_params[5].str));
        } else {
                int latd, latm, lats;
                int lngd, lngm, lngs;
                float flats, flngs;
-               LOG("position: %s, %s\n",
+               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';
@@ -139,14 +141,14 @@ nmea_rmc_cb(void)
 
 #define ms_to_deg(x, y) (((x * 10000) + y) / 60)
 
-               LOG("position: %d°%d.%04d, %d°%d.%04d\n",
+               DEBUG(4, "position: %d°%d.%04d, %d°%d.%04d\n",
                        latd, latm, lats, lngd, lngm, lngs);
-               LOG("position: %d°%d'%.1f\" %d°%d'%.1f\"\n",
+               DEBUG(4, "position: %d°%d'%.1f\" %d°%d'%.1f\"\n",
                        latd, latm, flats, lngd, lngm, flngs);
 
-               snprintf(lattitude, sizeof(lattitude), "%d.%d", latd, ms_to_deg(latm, lats));
-               snprintf(longitude, sizeof(longitude), "%d.%d", lngd, ms_to_deg(lngm, lngs));
-               LOG("position: %s %s\n", lattitude, longitude);
+               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();
        }
 }
@@ -157,7 +159,7 @@ nmea_gga_cb(void)
        if (!gps_valid)
                return;
        strncpy(elivation, nmea_params[9].str, sizeof(elivation));
-       LOG("height: %s\n", elivation);
+       DEBUG(4, "height: %s\n", elivation);
 }
 
 static void
@@ -166,9 +168,9 @@ nmea_vtg_cb(void)
        if (!gps_valid)
                return;
        strncpy(course, nmea_params[1].str, sizeof(course));
-       strncpy(speed, nmea_params[6].str, sizeof(speed));
-       LOG("course: %s\n", course);
-       LOG("speed: %s\n", speed);
+       strncpy(speed, nmea_params[7].str, sizeof(speed));
+       DEBUG(4, "course: %s\n", course);
+       DEBUG(4, "speed: %s\n", speed);
 }
 
 static struct nmea_msg {
@@ -221,13 +223,13 @@ static int
 nmea_tokenize(char *msg)
 {
        int cnt = 0;
-       char *tok = strtok(msg, ",");
+       char *tok = strsep(&msg, ",");
 
        while (tok && cnt < MAX_NMEA_PARAM) {
                nmea_params[cnt].str = tok;
                nmea_params[cnt].num = atoi(tok);
                cnt++;
-               tok = strtok(NULL, ",");
+               tok = strsep(&msg, ",");
        }
 
        return cnt;
@@ -248,7 +250,7 @@ nmea_process(char *a)
                return;
 
        if (nmea_verify_checksum(a)) {
-               ERROR("nmea message has invlid checksum\n");
+               ERROR("nmea message has invalid checksum\n");
                return;
        }
 
@@ -314,7 +316,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;
        }