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         gboolean newly_fixed = FALSE;
  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                         newly_fixed = TRUE;
  86                         gps_conn_set_state(gps, RCVR_FIXED);
  87                 }
  88         } else {
  89                 /* Data is invalid - not enough satellites?. */
  90                 if (gps->io.conn != RCVR_UP) {
  91                         gps_conn_set_state(gps, RCVR_UP);
  92                         /* track_add(NULL, FALSE); */
  93                 }
  94         }
  95
  96         /* Parse the latitude. */
  97         token = strsep(&sentence, DELIM);
  98         if (token && *token) {
  99                 dpoint = strchr(token, '.');
 100                 if (!dpoint)    /* handle buggy NMEA */
 101                         dpoint = token + strlen(token);
 102                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 103                 dpoint[-2] = '\0';
 104                 MACRO_PARSE_INT(tmpi, token);
 105                 gps->data.lat = tmpi + (tmpd * (1.0 / 60.0));
 106         }
 107
 108         /* Parse N or S. */
 109         token = strsep(&sentence, DELIM);
 110         if (token && *token == 'S')
 111                 gps->data.lat = -gps->data.lat;
 112
 113         /* Parse the longitude. */
 114         token = strsep(&sentence, DELIM);
 115         if (token && *token) {
 116                 dpoint = strchr(token, '.');
 117                 if (!dpoint)    /* handle buggy NMEA */
 118                         dpoint = token + strlen(token);
 119                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 120                 dpoint[-2] = '\0';
 121                 MACRO_PARSE_INT(tmpi, token);
 122                 gps->data.lon = tmpi + (tmpd * (1.0 / 60.0));
 123         }
 124
 125         /* Parse E or W. */
 126         token = strsep(&sentence, DELIM);
 127         if (token && *token == 'W')
 128                 gps->data.lon = -gps->data.lon;
 129
 130         /* Parse speed over ground, knots. */
 131         token = strsep(&sentence, DELIM);
 132         if (token && *token) {
 133                 MACRO_PARSE_FLOAT(gps->data.speed, token);
 134                 if (gps->data.fix > FIX_NOFIX) {
 135                         gps->data.maxspeed=MAX(gps->data.maxspeed, gps->data.speed);
 136                 }
 137         }
 138
 139         /* Parse heading, degrees from true north. */
 140         token = strsep(&sentence, DELIM);
 141         if (token && *token) {
 142                 MACRO_PARSE_FLOAT(gps->data.heading, token);
 143         }
 144
 145         /* Parse date. */
 146         token = strsep(&sentence, DELIM);
 147         if (token && *token && gpsdate) {
 148                 struct tm time;
 149                 gpsdate[6] = '\0';      /* Make sure time is 6 chars long. */
 150                 strcat(gpsdate, token);
 151                 strptime(gpsdate, "%H%M%S%d%m%y", &time);
 152                 gps->data.time = mktime(&time) + _gmtoffset;
 153         } else {
 154                 gps->data.time = time(NULL);
 155         }
 156
 157         gps_data_integerize(&gps->data);
 158
 159         if ((gps->io.conn==RCVR_FIXED) && (gps->update_location!=NULL)) {
 160                 gps->update_location(gps, newly_fixed);
 161         }
 162         gps->data.lheading=gps->data.heading;
 163 }
 164
 165 static void
 166 gps_nmea_parse_gga(Gps *gps, gchar * sentence)
 167 {
 168         /* GGA          Global Positioning System Fix Data
 169            1. Fix Time
 170            2. Latitude
 171            3. N or S
 172            4. Longitude
 173            5. E or W
 174            6. Fix quality
 175            0 = invalid
 176            1 = GPS fix (SPS)
 177            2 = DGPS fix
 178            3 = PPS fix
 179            4 = Real Time Kinematic
 180            5 = Float RTK
 181            6 = estimated (dead reckoning) (2.3 feature)
 182            7 = Manual input mode
 183            8 = Simulation mode
 184            7. Number of satellites being tracked
 185            8. Horizontal dilution of position
 186            9. Altitude, Meters, above mean sea level
 187            10. Alt unit (meters)
 188            11. Height of geoid (mean sea level) above WGS84 ellipsoid
 189            12. unit
 190            13. (empty field) time in seconds since last DGPS update
 191            14. (empty field) DGPS station ID number
 192            15. the checksum data
 193          */
 194         gchar *token;
 195         g_debug("%s(): %s", __PRETTY_FUNCTION__, sentence);
 196
 197         /* Skip Fix time */
 198         token = strsep(&sentence, DELIM);
 199         /* Skip latitude */
 200         token = strsep(&sentence, DELIM);
 201         /* Skip N or S */
 202         token = strsep(&sentence, DELIM);
 203         /* Skip longitude */
 204         token = strsep(&sentence, DELIM);
 205         /* Skip S or W */
 206         token = strsep(&sentence, DELIM);
 207
 208         /* Parse Fix quality */
 209         token = strsep(&sentence, DELIM);
 210         if (token && *token)
 211                 MACRO_PARSE_INT(gps->data.fixquality, token);
 212
 213         /* Skip number of satellites */
 214         token = strsep(&sentence, DELIM);
 215
 216         /* Parse Horizontal dilution of position */
 217         token = strsep(&sentence, DELIM);
 218         if (token && *token)
 219                 MACRO_PARSE_INT(gps->data.hdop, token);
 220
 221         /* Altitude */
 222         token = strsep(&sentence, DELIM);
 223         if (token && *token) {
 224                 MACRO_PARSE_FLOAT(gps->data.altitude, token);
 225         } else
 226                 gps->data.altitude = NAN;
 227 }
 228
 229 static void
 230 gps_nmea_parse_gsa(Gps *gps, gchar * sentence)
 231 {
 232         /* GPS DOP and active satellites
 233          *  1) Auto selection of 2D or 3D fix (M = manual)
 234          *  2) 3D fix - values include: 1 = no fix, 2 = 2D, 3 = 2D
 235          *  3) PRNs of satellites used for fix
 236          *     (space for 12)
 237          *  4) PDOP (dilution of precision)
 238          *  5) Horizontal dilution of precision (HDOP)
 239          *  6) Vertical dilution of precision (VDOP)
 240          *  7) Checksum
 241          */
 242         gchar *token;
 243         guint i,si;
 244         gint satforfix[12];
 245
 246         g_debug("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 247
 248         /* Skip Auto selection. */
 249         token = strsep(&sentence, DELIM);
 250
 251         /* 3D fix. */
 252         token = strsep(&sentence, DELIM);
 253         if (token && *token)
 254                 MACRO_PARSE_INT(gps->data.fix, token);
 255
 256         gps->data.satinuse = 0;
 257         for (i = 0; i < 12; i++) {
 258                 gint fprn;
 259                 token = strsep(&sentence, DELIM);
 260                 if (token && *token)
 261                         fprn=atoi(token);
 262                 else
 263                         fprn=-1;
 264                 satforfix[i]=fprn;
 265                 gps->data.sat[i].fix=FALSE;
 266         }
 267
 268         for (i=0;i<12;i++)
 269                 for (si=0;si<12;si++) {
 270                         if (gps->data.sat[i].prn==satforfix[si])
 271                                 gps->data.sat[i].fix=TRUE;
 272         }
 273
 274         /* PDOP */
 275         token = strsep(&sentence, DELIM);
 276         if (token && *token)
 277                 MACRO_PARSE_FLOAT(gps->data.pdop, token);
 278
 279         /* HDOP */
 280         token = strsep(&sentence, DELIM);
 281         if (token && *token)
 282                 MACRO_PARSE_FLOAT(gps->data.hdop, token);
 283
 284         /* VDOP */
 285         token = strsep(&sentence, DELIM);
 286         if (token && *token)
 287                 MACRO_PARSE_FLOAT(gps->data.vdop, token);
 288 }
 289
 290 static void
 291 gps_nmea_parse_gsv(Gps *gps, gchar * sentence)
 292 {
 293         /* Must be GSV - Satellites in view
 294          *  1) total number of messages
 295          *  2) message number
 296          *  3) satellites in view
 297          *  4) satellite number
 298          *  5) elevation in degrees (0-90)
 299          *  6) azimuth in degrees to true north (0-359)
 300          *  7) SNR in dB (0-99)
 301          *  more satellite infos like 4)-7)
 302          *  n) checksum
 303          */
 304         gchar *token;
 305         guint msgcnt = 0, nummsgs = 0;
 306         static guint running_total = 0;
 307         static guint num_sats_used = 0;
 308         static guint satcnt = 0;
 309         /* g_printf("%s(): %s\n", __PRETTY_FUNCTION__, sentence); */
 310
 311         /* Parse number of messages. */
 312         token = strsep(&sentence, DELIM);
 313         if (token && *token)
 314                 MACRO_PARSE_INT(nummsgs, token);
 315
 316         /* Parse message number. */
 317         token = strsep(&sentence, DELIM);
 318         if (token && *token)
 319                 MACRO_PARSE_INT(msgcnt, token);
 320
 321         /* Parse number of satellites in view. */
 322         token = strsep(&sentence, DELIM);
 323         if (token && *token) {
 324                 MACRO_PARSE_INT(gps->data.satinview, token);
 325                 if (gps->data.satinview > 12)   /* Handle buggy NMEA. */
 326                         gps->data.satinview = 12;
 327         }
 328
 329         /* Loop until there are no more satellites to parse. */
 330         while (sentence && satcnt < 12) {
 331                 /* Get token for Satellite Number. */
 332                 token = strsep(&sentence, DELIM);
 333                 if (token && *token)
 334                         gps->data.sat[satcnt].prn = atoi(token);
 335
 336                 /* Get token for elevation in degrees (0-90). */
 337                 token = strsep(&sentence, DELIM);
 338                 if (token && *token)
 339                         gps->data.sat[satcnt].elevation = atoi(token);
 340
 341                 /* Get token for azimuth in degrees to true north (0-359). */
 342                 token = strsep(&sentence, DELIM);
 343                 if (token && *token)
 344                         gps->data.sat[satcnt].azimuth = atoi(token);
 345
 346                 /* Get token for SNR. */
 347                 token = strsep(&sentence, DELIM);
 348                 if (token && *token && (gps->data.sat[satcnt].snr = atoi(token))) {
 349                         /* SNR is non-zero - add to total and count as used. */
 350                         running_total += gps->data.sat[satcnt].snr;
 351                         num_sats_used++;
 352                 }
 353                 satcnt++;
 354         }
 355
 356         if (msgcnt == nummsgs) {
 357                 /*  This is the last message. Calculate signal strength. */
 358                 if (num_sats_used) {
 359                         if (gps->io.conn==RCVR_UP) {
 360                                 gdouble fraction = running_total * sqrt(num_sats_used) / num_sats_used / 100.0;
 361                                 BOUND(fraction, 0.0, 1.0);
 362                                 if (gps->connection_progress!=NULL)
 363                                         gps->connection_progress(gps, fraction);
 364                         }
 365                         running_total=0;
 366                         num_sats_used=0;
 367                 }
 368                 satcnt = 0;
 369         }
 370 }
 371
 372 gboolean
 373 gps_nmea_parse(Gps *gps)
 374 {
 375 g_assert(gps);
 376
 377 g_debug("NMEA: %s", gps->io.nmea);
 378
 379 if (!gps->io.nmea)
 380         return FALSE;
 381
 382 if (strlen(gps->io.nmea)<NMEA_PRLEN+1)
 383         return FALSE;
 384
 385 if (!strncmp(gps->io.nmea, NMEA_GSV, NMEA_PRLEN)) {
 386         gps_nmea_parse_gsv(gps, gps->io.nmea + NMEA_PRLEN+1);
 387 } else if (!strncmp(gps->io.nmea, NMEA_RMC, NMEA_PRLEN))
 388         gps_nmea_parse_rmc(gps, gps->io.nmea + NMEA_PRLEN+1);
 389 else if (!strncmp(gps->io.nmea, NMEA_GGA, NMEA_PRLEN))
 390         gps_nmea_parse_gga(gps, gps->io.nmea + NMEA_PRLEN+1);
 391 else if (!strncmp(gps->io.nmea, NMEA_GSA, NMEA_PRLEN))
 392         gps_nmea_parse_gsa(gps, gps->io.nmea + NMEA_PRLEN+1);
 393 else g_printerr("Unknown NMEA: [%s]\n", gps->io.nmea);
 394 g_free(gps->io.nmea);
 395 gps->io.nmea=NULL;
 396
 397 return FALSE;
 398 }