ugps: add quality measurement parameters

[project/ugps.git] / nmea.c

diff --git a/nmea.c b/nmea.c
index 5bf6d9af7124a8d82528c44ee57aaacea0316c73..195753e80773d5feb940d7080fd8c3851ecf184c 100644 (file)
--- 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)
@@ -94,6 +95,36 @@ do_adjust_clock(struct tm *tm)
        }
 }
 
+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)
 {
@@ -132,29 +163,7 @@ nmea_rmc_cb(void)
                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);
-
-               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 (*nmea_params[4].str == 'S')
-                       lat *= -1.0;
-               if (*nmea_params[6].str == '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_timestamp();
+               parse_gps_coords(nmea_params[3].str, nmea_params[4].str, nmea_params[5].str, nmea_params[6].str);
        }
 }
 
@@ -163,7 +172,8 @@ nmea_zda_cb(void)
 {
        struct tm tm;
 
-       gps_valid = 1;
+       if (!gps_valid)
+               return;
 
        memset(&tm, 0, sizeof(tm));
        tm.tm_isdst = 1;
@@ -193,12 +203,31 @@ nmea_zda_cb(void)
        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
 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);
 }
 
@@ -209,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);
 }
@@ -230,6 +260,10 @@ static struct nmea_msg {
                .msg = "GGA",
                .cnt = 14,
                .handler = nmea_gga_cb,
+       }, {
+               .msg = "GLL",
+               .cnt = 7,
+               .handler = nmea_gll_cb,
        }, {
                .msg = "VTG",
                .cnt = 9,
@@ -286,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++;
@@ -306,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();