err.no Git - mapper/blob - src/gps-nmea-parse.c

   1 /*
   2  * This file is part of mapper
   3  *
   4  * Copyright (C) 2007 Kaj-Michael Lang
   5  * Copyright (C) 2006-2007 John Costigan.
   6  *
   7  * POI and GPS-Info code originally written by Cezary Jackiewicz.
   8  *
   9  * Default map data provided by http://www.openstreetmap.org/
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 2 of the License, or
  14  * (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful,
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  * GNU General Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License along
  22  * with this program; if not, write to the Free Software Foundation, Inc.,
  23  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  24  */
  25
  26 #include <config.h>
  27 #include <unistd.h>
  28 #include <stdlib.h>
  29 #include <string.h>
  30 #include <strings.h>
  31 #include <stddef.h>
  32 #include <libintl.h>
  33 #include <locale.h>
  34 #include <math.h>
  35 #include <errno.h>
  36 #include <glib/gstdio.h>
  37 #include <fcntl.h>
  38
  39 #include "utils.h"
  40 #include "gps.h"
  41 #include "gps-nmea-parse.h"
  42
  43 #define DELIM ","
  44
  45 #define NMEA_RMC "$GPRMC"
  46 #define NMEA_GGA "$GPGGA"
  47 #define NMEA_GSA "$GPGSA"
  48 #define NMEA_GSV "$GPGSV"
  49 #define NMEA_PRLEN (6)
  50
  51 static void
  52 gps_nmea_parse_rmc(Gps *gps, gchar *sentence)
  53 {
  54         /* Recommended Minimum Navigation Information C
  55          *  1) UTC Time
  56          *  2) Status, V=Navigation receiver warning A=Valid
  57          *  3) Latitude
  58          *  4) N or S
  59          *  5) Longitude
  60          *  6) E or W
  61          *  7) Speed over ground, knots
  62          *  8) Track made good, degrees true
  63          *  9) Date, ddmmyy
  64          * 10) Magnetic Variation, degrees
  65          * 11) E or W
  66          * 12) FAA mode indicator (NMEA 2.3 and later)
  67          * 13) Checksum
  68          */
  69         gchar *token, *dpoint, *gpsdate = NULL;
  70         gdouble tmpd = 0.f;
  71         guint tmpi = 0;
  72
  73         g_debug("%s(): %s", __PRETTY_FUNCTION__, sentence);
  74
  75         /* Parse time. */
  76         token = strsep(&sentence, DELIM);
  77         if (token && *token)
  78                 gpsdate = token;
  79
  80         token = strsep(&sentence, DELIM);
  81         /* Token is now Status. */
  82         if (token && *token == 'A') {
  83                 /* Data is valid. */
  84                 if (gps->io.conn != RCVR_FIXED) {
  85                         gps->data.newly_fixed = TRUE;
  86                         gps_conn_set_state(gps, RCVR_FIXED);
  87                 } else {
  88                         gps->data.newly_fixed = FALSE;
  89                 }
  90         } else {
  91                 /* Data is invalid - not enough satellites?. */
  92                 if (gps->io.conn != RCVR_UP) {
  93                         gps_conn_set_state(gps, RCVR_UP);
  94                         /* track_add(NULL, FALSE); */
  95                 }
  96         }
  97
  98         /* Parse the latitude. */
  99         token = strsep(&sentence, DELIM);
 100         if (token && *token) {
 101                 dpoint = strchr(token, '.');
 102                 if (!dpoint)    /* handle buggy NMEA */
 103                         dpoint = token + strlen(token);
 104                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 105                 dpoint[-2] = '\0';
 106                 MACRO_PARSE_INT(tmpi, token);
 107                 gps->data.lat = tmpi + (tmpd * (1.0 / 60.0));
 108         }
 109
 110         /* Parse N or S. */
 111         token = strsep(&sentence, DELIM);
 112         if (token && *token == 'S')
 113                 gps->data.lat = -gps->data.lat;
 114
 115         /* Parse the longitude. */
 116         token = strsep(&sentence, DELIM);
 117         if (token && *token) {
 118                 dpoint = strchr(token, '.');
 119                 if (!dpoint)    /* handle buggy NMEA */
 120                         dpoint = token + strlen(token);
 121                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 122                 dpoint[-2] = '\0';
 123                 MACRO_PARSE_INT(tmpi, token);
 124                 gps->data.lon = tmpi + (tmpd * (1.0 / 60.0));
 125         }
 126
 127         /* Parse E or W. */
 128         token = strsep(&sentence, DELIM);
 129         if (token && *token == 'W')
 130                 gps->data.lon = -gps->data.lon;
 131
 132         /* Parse speed over ground, knots. */
 133         token = strsep(&sentence, DELIM);
 134         if (token && *token) {
 135                 MACRO_PARSE_FLOAT(gps->data.speed, token);
 136                 if (gps->data.fix > FIX_NOFIX) {
 137                         gps->data.maxspeed=MAX(gps->data.maxspeed, gps->data.speed);
 138                 }
 139         }
 140
 141         /* Parse heading, degrees from true north. */
 142         token = strsep(&sentence, DELIM);
 143         if (token && *token) {
 144                 MACRO_PARSE_FLOAT(gps->data.heading, token);
 145         }
 146
 147         /* Parse date. */
 148         token = strsep(&sentence, DELIM);
 149         if (token && *token && gpsdate) {
 150                 struct tm time;
 151                 gpsdate[6] = '\0';      /* Make sure time is 6 chars long. */
 152                 strcat(gpsdate, token);
 153                 strptime(gpsdate, "%H%M%S%d%m%y", &time);
 154                 gps->data.time = mktime(&time) + _gmtoffset;
 155         } else {
 156                 gps->data.time = time(NULL);
 157         }
 158
 159         gps_data_integerize(&gps->data);
 160         gps->data.lheading=gps->data.heading;
 161 }
 162
 163 static void
 164 gps_nmea_parse_gga(Gps *gps, gchar * sentence)
 165 {
 166         /* GGA          Global Positioning System Fix Data
 167            1. Fix Time
 168            2. Latitude
 169            3. N or S
 170            4. Longitude
 171            5. E or W
 172            6. Fix quality
 173            0 = invalid
 174            1 = GPS fix (SPS)
 175            2 = DGPS fix
 176            3 = PPS fix
 177            4 = Real Time Kinematic
 178            5 = Float RTK
 179            6 = estimated (dead reckoning) (2.3 feature)
 180            7 = Manual input mode
 181            8 = Simulation mode
 182            7. Number of satellites being tracked
 183            8. Horizontal dilution of position
 184            9. Altitude, Meters, above mean sea level
 185            10. Alt unit (meters)
 186            11. Height of geoid (mean sea level) above WGS84 ellipsoid
 187            12. unit
 188            13. (empty field) time in seconds since last DGPS update
 189            14. (empty field) DGPS station ID number
 190            15. the checksum data
 191          */
 192         gchar *token;
 193         g_debug("%s(): %s", __PRETTY_FUNCTION__, sentence);
 194
 195         /* Skip Fix time */
 196         token = strsep(&sentence, DELIM);
 197         /* Skip latitude */
 198         token = strsep(&sentence, DELIM);
 199         /* Skip N or S */
 200         token = strsep(&sentence, DELIM);
 201         /* Skip longitude */
 202         token = strsep(&sentence, DELIM);
 203         /* Skip S or W */
 204         token = strsep(&sentence, DELIM);
 205
 206         /* Parse Fix quality */
 207         token = strsep(&sentence, DELIM);
 208         if (token && *token)
 209                 MACRO_PARSE_INT(gps->data.fixquality, token);
 210
 211         /* Skip number of satellites */
 212         token = strsep(&sentence, DELIM);
 213
 214         /* Parse Horizontal dilution of position */
 215         token = strsep(&sentence, DELIM);
 216         if (token && *token)
 217                 MACRO_PARSE_INT(gps->data.hdop, token);
 218
 219         /* Altitude */
 220         token = strsep(&sentence, DELIM);
 221         if (token && *token) {
 222                 MACRO_PARSE_FLOAT(gps->data.altitude, token);
 223         } else
 224                 gps->data.altitude = NAN;
 225 }
 226
 227 static void
 228 gps_nmea_parse_gsa(Gps *gps, gchar * sentence)
 229 {
 230         /* GPS DOP and active satellites
 231          *  1) Auto selection of 2D or 3D fix (M = manual)
 232          *  2) 3D fix - values include: 1 = no fix, 2 = 2D, 3 = 2D
 233          *  3) PRNs of satellites used for fix
 234          *     (space for 12)
 235          *  4) PDOP (dilution of precision)
 236          *  5) Horizontal dilution of precision (HDOP)
 237          *  6) Vertical dilution of precision (VDOP)
 238          *  7) Checksum
 239          */
 240         gchar *token;
 241         guint i,si;
 242         gint satforfix[12];
 243
 244         g_debug("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 245
 246         /* Skip Auto selection. */
 247         token = strsep(&sentence, DELIM);
 248
 249         /* 3D fix. */
 250         token = strsep(&sentence, DELIM);
 251         if (token && *token)
 252                 MACRO_PARSE_INT(gps->data.fix, token);
 253
 254         gps->data.satinuse = 0;
 255         for (i = 0; i < 12; i++) {
 256                 gint fprn;
 257                 token = strsep(&sentence, DELIM);
 258                 if (token && *token)
 259                         fprn=atoi(token);
 260                 else
 261                         fprn=-1;
 262                 satforfix[i]=fprn;
 263                 gps->data.sat[i].fix=FALSE;
 264         }
 265
 266         for (i=0;i<12;i++)
 267                 for (si=0;si<12;si++) {
 268                         if (gps->data.sat[i].prn==satforfix[si])
 269                                 gps->data.sat[i].fix=TRUE;
 270         }
 271
 272         /* PDOP */
 273         token = strsep(&sentence, DELIM);
 274         if (token && *token)
 275                 MACRO_PARSE_FLOAT(gps->data.pdop, token);
 276
 277         /* HDOP */
 278         token = strsep(&sentence, DELIM);
 279         if (token && *token)
 280                 MACRO_PARSE_FLOAT(gps->data.hdop, token);
 281
 282         /* VDOP */
 283         token = strsep(&sentence, DELIM);
 284         if (token && *token)
 285                 MACRO_PARSE_FLOAT(gps->data.vdop, token);
 286 }
 287
 288 static void
 289 gps_nmea_parse_gsv(Gps *gps, gchar * sentence)
 290 {
 291         /* Must be GSV - Satellites in view
 292          *  1) total number of messages
 293          *  2) message number
 294          *  3) satellites in view
 295          *  4) satellite number
 296          *  5) elevation in degrees (0-90)
 297          *  6) azimuth in degrees to true north (0-359)
 298          *  7) SNR in dB (0-99)
 299          *  more satellite infos like 4)-7)
 300          *  n) checksum
 301          */
 302         gchar *token;
 303         guint msgcnt = 0, nummsgs = 0;
 304         static guint running_total = 0;
 305         static guint num_sats_used = 0;
 306         static guint satcnt = 0;
 307         /* g_printf("%s(): %s\n", __PRETTY_FUNCTION__, sentence); */
 308
 309         /* Parse number of messages. */
 310         token = strsep(&sentence, DELIM);
 311         if (token && *token)
 312                 MACRO_PARSE_INT(nummsgs, token);
 313
 314         /* Parse message number. */
 315         token = strsep(&sentence, DELIM);
 316         if (token && *token)
 317                 MACRO_PARSE_INT(msgcnt, token);
 318
 319         /* Parse number of satellites in view. */
 320         token = strsep(&sentence, DELIM);
 321         if (token && *token) {
 322                 MACRO_PARSE_INT(gps->data.satinview, token);
 323                 if (gps->data.satinview > 12)   /* Handle buggy NMEA. */
 324                         gps->data.satinview = 12;
 325         }
 326
 327         /* Loop until there are no more satellites to parse. */
 328         while (sentence && satcnt < 12) {
 329                 /* Get token for Satellite Number. */
 330                 token = strsep(&sentence, DELIM);
 331                 if (token && *token)
 332                         gps->data.sat[satcnt].prn = atoi(token);
 333
 334                 /* Get token for elevation in degrees (0-90). */
 335                 token = strsep(&sentence, DELIM);
 336                 if (token && *token)
 337                         gps->data.sat[satcnt].elevation = atoi(token);
 338
 339                 /* Get token for azimuth in degrees to true north (0-359). */
 340                 token = strsep(&sentence, DELIM);
 341                 if (token && *token)
 342                         gps->data.sat[satcnt].azimuth = atoi(token);
 343
 344                 /* Get token for SNR. */
 345                 token = strsep(&sentence, DELIM);
 346                 if (token && *token && (gps->data.sat[satcnt].snr = atoi(token))) {
 347                         /* SNR is non-zero - add to total and count as used. */
 348                         running_total += gps->data.sat[satcnt].snr;
 349                         num_sats_used++;
 350                 }
 351                 satcnt++;
 352         }
 353
 354         if (msgcnt == nummsgs) {
 355                 /*  This is the last message. Calculate signal strength. */
 356                 if (num_sats_used) {
 357                         if (gps->io.conn==RCVR_UP) {
 358                                 gdouble fraction = running_total * sqrt(num_sats_used) / num_sats_used / 100.0;
 359                                 BOUND(fraction, 0.0, 1.0);
 360                                 if (gps->connection_progress!=NULL)
 361                                         gps->connection_progress(gps, fraction);
 362                         }
 363                         running_total=0;
 364                         num_sats_used=0;
 365                 }
 366                 satcnt = 0;
 367         }
 368 }
 369
 370 gboolean
 371 gps_nmea_parse(Gps *gps)
 372 {
 373 g_assert(gps);
 374
 375 g_debug("NMEA2 %d %s", gps->io.nmea_cnt, gps->io.nmea);
 376
 377 if (!gps->io.nmea)
 378         return FALSE;
 379
 380 if (strlen(gps->io.nmea)<NMEA_PRLEN+1)
 381         return FALSE;
 382
 383 if (!strncmp(gps->io.nmea, NMEA_GSV, NMEA_PRLEN)) {
 384         gps_nmea_parse_gsv(gps, gps->io.nmea + NMEA_PRLEN+1);
 385         if (gps->update_satellite)
 386                 g_idle_add_full(G_PRIORITY_DEFAULT, (GSourceFunc)gps->update_satellite, gps, NULL);
 387
 388 } else if (!strncmp(gps->io.nmea, NMEA_RMC, NMEA_PRLEN)) {
 389         gps_nmea_parse_rmc(gps, gps->io.nmea + NMEA_PRLEN+1);
 390         if ((gps->io.conn==RCVR_FIXED) && (gps->update_location!=NULL))
 391                 gps->update_location(gps);
 392
 393 } else if (!strncmp(gps->io.nmea, NMEA_GGA, NMEA_PRLEN)) {
 394         gps_nmea_parse_gga(gps, gps->io.nmea + NMEA_PRLEN+1);
 395         if (gps->update_info)
 396                 g_idle_add_full(G_PRIORITY_DEFAULT, (GSourceFunc)gps->update_info, gps, NULL);
 397
 398 } else if (!strncmp(gps->io.nmea, NMEA_GSA, NMEA_PRLEN)) {
 399         gps_nmea_parse_gsa(gps, gps->io.nmea + NMEA_PRLEN+1);
 400         if (gps->update_satellite)
 401                 g_idle_add_full(G_PRIORITY_DEFAULT, (GSourceFunc)gps->update_satellite, gps, NULL);
 402         if (gps->update_info)
 403                 g_idle_add_full(G_PRIORITY_DEFAULT, (GSourceFunc)gps->update_info, gps, NULL);
 404
 405 } else g_printerr("Unknown NMEA: [%s]\n", gps->io.nmea);
 406
 407 return FALSE;
 408 }