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