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

   1 #define _GNU_SOURCE
   2
   3 #include <config.h>
   4 #include <unistd.h>
   5 #include <stdlib.h>
   6 #include <string.h>
   7 #include <strings.h>
   8 #include <stddef.h>
   9 #include <libintl.h>
  10 #include <locale.h>
  11 #include <math.h>
  12 #include <errno.h>
  13 #include <glib/gstdio.h>
  14 #include <gtk/gtk.h>
  15 #include <fcntl.h>
  16 #include <libxml/parser.h>
  17
  18 #include "utils.h"
  19 #include "bt.h"
  20 #include "gps.h"
  21 #include "mapper-types.h"
  22 #include "ui-common.h"
  23 #include "map.h"
  24 #include "track.h"
  25 #include "route.h"
  26 #include "gps-panels.h"
  27 #include "settings.h"
  28 #include "gps-conn.h"
  29 #include "gtkgps.h"
  30 #include "gtkcompass.h"
  31
  32
  33 #define GPS_FILTER_MAX_SKIP (10)
  34 static gint track_drop_cnt=0;
  35
  36 gboolean
  37 channel_cb_error(GIOChannel * src, GIOCondition condition, gpointer data)
  38 {
  39 /* An error has occurred - re-connect(). */
  40 rcvr_disconnect();
  41 track_add(0, FALSE);
  42 _speed_excess = FALSE;
  43
  44 if (_conn_state > RCVR_OFF) {
  45         gps_conn_set_state(RCVR_DOWN);
  46         gps_hide_text();
  47         rcvr_connect_later();
  48 }
  49
  50 return FALSE;
  51 }
  52
  53 static void
  54 channel_parse_rmc(gchar * sentence)
  55 {
  56         /* Recommended Minimum Navigation Information C
  57          *  1) UTC Time
  58          *  2) Status, V=Navigation receiver warning A=Valid
  59          *  3) Latitude
  60          *  4) N or S
  61          *  5) Longitude
  62          *  6) E or W
  63          *  7) Speed over ground, knots
  64          *  8) Track made good, degrees true
  65          *  9) Date, ddmmyy
  66          * 10) Magnetic Variation, degrees
  67          * 11) E or W
  68          * 12) FAA mode indicator (NMEA 2.3 and later)
  69          * 13) Checksum
  70          */
  71         gchar *token, *dpoint, *gpsdate = NULL;
  72         gdouble tmpd = 0.f;
  73         guint tmpi = 0;
  74         gboolean newly_fixed = FALSE;
  75         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
  76
  77 #define DELIM ","
  78
  79         /* Parse time. */
  80         token = strsep(&sentence, DELIM);
  81         if (token && *token)
  82                 gpsdate = token;
  83
  84         token = strsep(&sentence, DELIM);
  85         /* Token is now Status. */
  86         if (token && *token == 'A') {
  87                 /* Data is valid. */
  88                 if (_conn_state != RCVR_FIXED) {
  89                         newly_fixed = TRUE;
  90                         gps_conn_set_state(RCVR_FIXED);
  91                 }
  92         } else {
  93                 /* Data is invalid - not enough satellites?. */
  94                 if (_conn_state != RCVR_UP) {
  95                         gps_conn_set_state(RCVR_UP);
  96                         track_add(0, FALSE);
  97                 }
  98         }
  99
 100         /* Parse the latitude. */
 101         token = strsep(&sentence, DELIM);
 102         if (token && *token) {
 103                 dpoint = strchr(token, '.');
 104                 if (!dpoint)    /* handle buggy NMEA */
 105                         dpoint = token + strlen(token);
 106                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 107                 dpoint[-2] = '\0';
 108                 MACRO_PARSE_INT(tmpi, token);
 109                 _gps.lat = tmpi + (tmpd * (1.0 / 60.0));
 110         }
 111
 112         /* Parse N or S. */
 113         token = strsep(&sentence, DELIM);
 114         if (token && *token == 'S')
 115                 _gps.lat = -_gps.lat;
 116
 117         /* Parse the longitude. */
 118         token = strsep(&sentence, DELIM);
 119         if (token && *token) {
 120                 dpoint = strchr(token, '.');
 121                 if (!dpoint)    /* handle buggy NMEA */
 122                         dpoint = token + strlen(token);
 123                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 124                 dpoint[-2] = '\0';
 125                 MACRO_PARSE_INT(tmpi, token);
 126                 _gps.lon = tmpi + (tmpd * (1.0 / 60.0));
 127         }
 128
 129         /* Parse E or W. */
 130         token = strsep(&sentence, DELIM);
 131         if (token && *token == 'W')
 132                 _gps.lon = -_gps.lon;
 133
 134         /* Parse speed over ground, knots. */
 135         token = strsep(&sentence, DELIM);
 136         if (token && *token) {
 137                 MACRO_PARSE_FLOAT(_gps.speed, token);
 138                 if (_gps.fix > 1) {
 139                         _gps.maxspeed = MAX(_gps.maxspeed, _gps.speed);
 140                         gps_display_data_speed(info_banner.speed, _gps.speed);
 141                 }
 142         }
 143
 144         /* Parse heading, degrees from true north. */
 145         token = strsep(&sentence, DELIM);
 146         if (token && *token) {
 147                 MACRO_PARSE_FLOAT(_gps.heading, token);
 148         }
 149
 150         /* Parse date. */
 151         token = strsep(&sentence, DELIM);
 152         if (token && *token && gpsdate) {
 153                 struct tm time;
 154                 gpsdate[6] = '\0';      /* Make sure time is 6 chars long. */
 155                 strcat(gpsdate, token);
 156                 strptime(gpsdate, "%H%M%S%d%m%y", &time);
 157                 _pos.time = mktime(&time) + _gmtoffset;
 158         } else
 159                 _pos.time = time(NULL);
 160
 161         /* Add new data to track only if we have a fix */
 162         _track_store=TRUE;
 163
 164         /* XXX: Set filter logic somewhere else */
 165
 166         if ((_conn_state == RCVR_FIXED) && (_track_store==TRUE)) {
 167                 if ((_gps_filter==TRUE) && (track_drop_cnt<GPS_FILTER_MAX_SKIP)) {
 168                         integerize_data(&_gps, &_pos);
 169                         if ( (_gps.hdop<_filter_hdop || _filter_hdop==0.0) &&
 170                                 (_gps.vdop<_filter_vdop || _filter_vdop==0.0) &&
 171                                 (fabs(_gps.heading-_gps.lheading)>_filter_angle || _filter_angle==0.0 ) &&
 172                                 (_map_location_known==TRUE && (_map_location_dist<_filter_osm || _map_location_dist==0.0)) ) {
 173                                 track_add(_pos.time, newly_fixed);
 174                                 _gps.lheading=_gps.heading;
 175                                 track_drop_cnt=0;
 176                         } else {
 177                                 track_drop_cnt++;
 178                                 g_printf("*** Filtering by: [%s %s %s %s] A: %f (%d)\n",
 179                                         _gps.hdop>_filter_hdop ? "HDOP" : "-",
 180                                         _gps.vdop>_filter_vdop ? "VDOP" : "-",
 181                                         (fabs(_gps.heading-_gps.lheading)<_filter_angle) ? "Angle" : "-",
 182                                         (_map_location_known==TRUE && (_map_location_dist>_filter_osm)) ? "OSM" : "-",
 183                                         fabs(_gps.heading-_gps.lheading), track_drop_cnt);
 184                         }
 185                         map_refresh_mark();
 186                 } else {
 187                         track_drop_cnt=0;
 188                         integerize_data(&_gps, &_pos);
 189                         track_add(_pos.time, newly_fixed);
 190                         _gps.lheading=_gps.heading;
 191                         map_refresh_mark();
 192                 }
 193         }
 194
 195         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 196 }
 197
 198 static void
 199 channel_parse_gga(gchar * sentence)
 200 {
 201         /* GGA          Global Positioning System Fix Data
 202            1. Fix Time
 203            2. Latitude
 204            3. N or S
 205            4. Longitude
 206            5. E or W
 207            6. Fix quality
 208            0 = invalid
 209            1 = GPS fix (SPS)
 210            2 = DGPS fix
 211            3 = PPS fix
 212            4 = Real Time Kinematic
 213            5 = Float RTK
 214            6 = estimated (dead reckoning) (2.3 feature)
 215            7 = Manual input mode
 216            8 = Simulation mode
 217            7. Number of satellites being tracked
 218            8. Horizontal dilution of position
 219            9. Altitude, Meters, above mean sea level
 220            10. Alt unit (meters)
 221            11. Height of geoid (mean sea level) above WGS84 ellipsoid
 222            12. unit
 223            13. (empty field) time in seconds since last DGPS update
 224            14. (empty field) DGPS station ID number
 225            15. the checksum data
 226          */
 227         gchar *token;
 228         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 229
 230 #define DELIM ","
 231
 232         /* Skip Fix time */
 233         token = strsep(&sentence, DELIM);
 234         /* Skip latitude */
 235         token = strsep(&sentence, DELIM);
 236         /* Skip N or S */
 237         token = strsep(&sentence, DELIM);
 238         /* Skip longitude */
 239         token = strsep(&sentence, DELIM);
 240         /* Skip S or W */
 241         token = strsep(&sentence, DELIM);
 242
 243         /* Parse Fix quality */
 244         token = strsep(&sentence, DELIM);
 245         if (token && *token)
 246                 MACRO_PARSE_INT(_gps.fixquality, token);
 247
 248         /* Skip number of satellites */
 249         token = strsep(&sentence, DELIM);
 250
 251         /* Parse Horizontal dilution of position */
 252         token = strsep(&sentence, DELIM);
 253         if (token && *token)
 254                 MACRO_PARSE_INT(_gps.hdop, token);
 255
 256         /* Altitude */
 257         token = strsep(&sentence, DELIM);
 258         if (token && *token) {
 259                 MACRO_PARSE_FLOAT(_pos.altitude, token);
 260         } else
 261                 _pos.altitude = NAN;
 262
 263         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 264 }
 265
 266 static void
 267 channel_parse_gsa(gchar * sentence)
 268 {
 269         /* GPS DOP and active satellites
 270          *  1) Auto selection of 2D or 3D fix (M = manual)
 271          *  2) 3D fix - values include: 1 = no fix, 2 = 2D, 3 = 2D
 272          *  3) PRNs of satellites used for fix
 273          *     (space for 12)
 274          *  4) PDOP (dilution of precision)
 275          *  5) Horizontal dilution of precision (HDOP)
 276          *  6) Vertical dilution of precision (VDOP)
 277          *  7) Checksum
 278          */
 279         gchar *token;
 280         guint i,si;
 281         gint satforfix[12];
 282
 283         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 284
 285 #define DELIM ","
 286
 287         /* Skip Auto selection. */
 288         token = strsep(&sentence, DELIM);
 289
 290         /* 3D fix. */
 291         token = strsep(&sentence, DELIM);
 292         if (token && *token)
 293                 MACRO_PARSE_INT(_gps.fix, token);
 294
 295         _gps.satinuse = 0;
 296         for (i = 0; i < 12; i++) {
 297                 gint fprn;
 298                 token = strsep(&sentence, DELIM);
 299                 if (token && *token)
 300                         fprn=atoi(token);
 301                 else
 302                         fprn=-1;
 303                 satforfix[i]=fprn;
 304                 _gps.sat[i].fix=FALSE;
 305         }
 306
 307         for (i=0;i<12;i++)
 308                 for (si=0;si<12;si++) {
 309                         if (_gps.sat[i].prn==satforfix[si])
 310                                 _gps.sat[i].fix=TRUE;
 311         }
 312
 313         /* PDOP */
 314         token = strsep(&sentence, DELIM);
 315         if (token && *token)
 316                 MACRO_PARSE_FLOAT(_gps.pdop, token);
 317
 318         /* HDOP */
 319         token = strsep(&sentence, DELIM);
 320         if (token && *token)
 321                 MACRO_PARSE_FLOAT(_gps.hdop, token);
 322
 323         /* VDOP */
 324         token = strsep(&sentence, DELIM);
 325         if (token && *token)
 326                 MACRO_PARSE_FLOAT(_gps.vdop, token);
 327
 328         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 329 }
 330
 331 static void
 332 channel_parse_gsv(gchar * sentence)
 333 {
 334         /* Must be GSV - Satellites in view
 335          *  1) total number of messages
 336          *  2) message number
 337          *  3) satellites in view
 338          *  4) satellite number
 339          *  5) elevation in degrees (0-90)
 340          *  6) azimuth in degrees to true north (0-359)
 341          *  7) SNR in dB (0-99)
 342          *  more satellite infos like 4)-7)
 343          *  n) checksum
 344          */
 345         gchar *token;
 346         guint msgcnt = 0, nummsgs = 0;
 347         static guint running_total = 0;
 348         static guint num_sats_used = 0;
 349         static guint satcnt = 0;
 350         g_printf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 351
 352         /* Parse number of messages. */
 353         token = strsep(&sentence, DELIM);
 354         if (token && *token)
 355                 MACRO_PARSE_INT(nummsgs, token);
 356
 357         /* Parse message number. */
 358         token = strsep(&sentence, DELIM);
 359         if (token && *token)
 360                 MACRO_PARSE_INT(msgcnt, token);
 361
 362         /* Parse number of satellites in view. */
 363         token = strsep(&sentence, DELIM);
 364         if (token && *token) {
 365                 MACRO_PARSE_INT(_gps.satinview, token);
 366                 if (_gps.satinview > 12)        /* Handle buggy NMEA. */
 367                         _gps.satinview = 12;
 368         }
 369
 370         /* Loop until there are no more satellites to parse. */
 371         while (sentence && satcnt < 12) {
 372                 /* Get token for Satellite Number. */
 373                 token = strsep(&sentence, DELIM);
 374                 if (token && *token)
 375                         _gps.sat[satcnt].prn = atoi(token);
 376
 377                 /* Get token for elevation in degrees (0-90). */
 378                 token = strsep(&sentence, DELIM);
 379                 if (token && *token)
 380                         _gps.sat[satcnt].elevation = atoi(token);
 381
 382                 /* Get token for azimuth in degrees to true north (0-359). */
 383                 token = strsep(&sentence, DELIM);
 384                 if (token && *token)
 385                         _gps.sat[satcnt].azimuth = atoi(token);
 386
 387                 /* Get token for SNR. */
 388                 token = strsep(&sentence, DELIM);
 389                 if (token && *token && (_gps.sat[satcnt].snr = atoi(token))) {
 390                         /* SNR is non-zero - add to total and count as used. */
 391                         running_total += _gps.sat[satcnt].snr;
 392                         num_sats_used++;
 393                 }
 394                 satcnt++;
 395         }
 396
 397         if (msgcnt == nummsgs) {
 398                 /*  This is the last message. Calculate signal strength. */
 399                 if (num_sats_used) {
 400                         if (_conn_state == RCVR_UP) {
 401                                 gdouble fraction = running_total * sqrt(num_sats_used) / num_sats_used / 100.0;
 402                                 BOUND(fraction, 0.0, 1.0);
 403                                 gps_conn_set_progress(fraction);
 404                         }
 405                         running_total = 0;
 406                         num_sats_used = 0;
 407                 }
 408                 satcnt = 0;
 409
 410                 /* Keep awake while they watch the progress bar. */
 411                 KEEP_DISPLAY_ON();
 412         }
 413
 414         g_printf("%s(): return\n", __PRETTY_FUNCTION__);
 415 }
 416
 417 gboolean
 418 channel_cb_input(GIOChannel * src, GIOCondition condition, gpointer data)
 419 {
 420         gsize bytes_read;
 421         vprintf("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 422
 423         if (G_IO_STATUS_NORMAL == g_io_channel_read_chars(_channel,
 424                                                           _gps_read_buf_curr, _gps_read_buf_last - _gps_read_buf_curr,
 425                                                           &bytes_read, NULL)) {
 426                 gchar *eol;
 427
 428                 _gps_read_buf_curr += bytes_read;
 429                 *_gps_read_buf_curr = '\0';     /* append a \0 so we can read as string */
 430                 while ((eol = strchr(_gps_read_buf, '\n'))) {
 431                         gchar *sptr = _gps_read_buf + 1;        /* Skip the $ */
 432                         guint csum = 0;
 433                         if (*_gps_read_buf == '$') {
 434                                 /* This is the beginning of a sentence; okay to parse. */
 435                                 *eol = '\0';    /* overwrite \n with \0 */
 436                                 while (*sptr && *sptr != '*')
 437                                         csum ^= *sptr++;
 438
 439                                 /* If we're at a \0 (meaning there is no checksum), or if the
 440                                  * checksum is good, then parse the sentence. */
 441                                 if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
 442                                         if (*sptr)
 443                                                 *sptr = '\0';   /* take checksum out of the buffer. */
 444                                         if (!strncmp(_gps_read_buf + 3, "GSV", 3)) {
 445                                                 if (_conn_state == RCVR_UP)
 446                                                         channel_parse_gsv(_gps_read_buf + 7);
 447                                         } else if (!strncmp(_gps_read_buf + 3, "RMC", 3))
 448                                                 channel_parse_rmc(_gps_read_buf + 7);
 449                                         else if (!strncmp(_gps_read_buf + 3, "GGA", 3))
 450                                                 channel_parse_gga(_gps_read_buf + 7);
 451                                         else if (!strncmp(_gps_read_buf + 3, "GSA", 3))
 452                                                 channel_parse_gsa(_gps_read_buf + 7);
 453                                         else g_print("Unknown NMEA: [%s]\n", _gps_read_buf);
 454
 455                                         if (_gps_info)
 456                                                 gps_display_data();
 457
 458                                         if (_satdetails_on)
 459                                                 gps_display_details();
 460                                 } else {
 461                                         /* There was a checksum, and it was bad. */
 462                                         g_printerr
 463                                             ("%s: Bad checksum in NMEA sentence:\n%s\n",
 464                                              __PRETTY_FUNCTION__,
 465                                              _gps_read_buf);
 466                                 }
 467                         }
 468
 469                         /* If eol is at or after (_gps_read_buf_curr - 1) */
 470                         if (eol >= (_gps_read_buf_curr - 1)) {
 471                                 /* Last read was a newline - reset read buffer */
 472                                 _gps_read_buf_curr = _gps_read_buf;
 473                                 *_gps_read_buf_curr = '\0';
 474                         } else {
 475                                 /* Move the next line to the front of the buffer. */
 476                                 memmove(_gps_read_buf, eol + 1, _gps_read_buf_curr - eol);      /* include terminating 0 */
 477                                 /* Subtract _curr so that it's pointing at the new \0. */
 478                                 _gps_read_buf_curr -= (eol - _gps_read_buf + 1);
 479                         }
 480                 }
 481         }
 482
 483         vprintf("%s(): return TRUE\n", __PRETTY_FUNCTION__);
 484         return TRUE;
 485 }