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 #define GPS_FILTER_MAX_SKIP (10)
  30 static gint track_drop_cnt=0;
  31
  32 gboolean
  33 channel_cb_error(GIOChannel * src, GIOCondition condition, gpointer data)
  34 {
  35         printf("%s(%d)\n", __PRETTY_FUNCTION__, condition);
  36
  37         /* An error has occurred - re-connect(). */
  38         rcvr_disconnect();
  39         track_add(0, FALSE);
  40         _speed_excess = FALSE;
  41
  42         if (_conn_state > RCVR_OFF) {
  43                 set_conn_state(RCVR_DOWN);
  44                 gps_hide_text();
  45                 rcvr_connect_later();
  46         }
  47
  48         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
  49         return FALSE;
  50 }
  51
  52 static void
  53 channel_parse_rmc(gchar * sentence)
  54 {
  55         /* Recommended Minimum Navigation Information C
  56          *  1) UTC Time
  57          *  2) Status, V=Navigation receiver warning A=Valid
  58          *  3) Latitude
  59          *  4) N or S
  60          *  5) Longitude
  61          *  6) E or W
  62          *  7) Speed over ground, knots
  63          *  8) Track made good, degrees true
  64          *  9) Date, ddmmyy
  65          * 10) Magnetic Variation, degrees
  66          * 11) E or W
  67          * 12) FAA mode indicator (NMEA 2.3 and later)
  68          * 13) Checksum
  69          */
  70         gchar *token, *dpoint, *gpsdate = NULL;
  71         gdouble tmpd = 0.f;
  72         guint tmpi = 0;
  73         gboolean newly_fixed = FALSE;
  74         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
  75
  76 #define DELIM ","
  77
  78         /* Parse time. */
  79         token = strsep(&sentence, DELIM);
  80         if (token && *token)
  81                 gpsdate = token;
  82
  83         token = strsep(&sentence, DELIM);
  84         /* Token is now Status. */
  85         if (token && *token == 'A') {
  86                 /* Data is valid. */
  87                 if (_conn_state != RCVR_FIXED) {
  88                         newly_fixed = TRUE;
  89                         set_conn_state(RCVR_FIXED);
  90                 }
  91         } else {
  92                 /* Data is invalid - not enough satellites?. */
  93                 if (_conn_state != RCVR_UP) {
  94                         set_conn_state(RCVR_UP);
  95                         track_add(0, FALSE);
  96                 }
  97         }
  98
  99         /* Parse the latitude. */
 100         token = strsep(&sentence, DELIM);
 101         if (token && *token) {
 102                 dpoint = strchr(token, '.');
 103                 if (!dpoint)    /* handle buggy NMEA */
 104                         dpoint = token + strlen(token);
 105                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 106                 dpoint[-2] = '\0';
 107                 MACRO_PARSE_INT(tmpi, token);
 108                 _gps.lat = tmpi + (tmpd * (1.0 / 60.0));
 109         }
 110
 111         /* Parse N or S. */
 112         token = strsep(&sentence, DELIM);
 113         if (token && *token == 'S')
 114                 _gps.lat = -_gps.lat;
 115
 116         /* Parse the longitude. */
 117         token = strsep(&sentence, DELIM);
 118         if (token && *token) {
 119                 dpoint = strchr(token, '.');
 120                 if (!dpoint)    /* handle buggy NMEA */
 121                         dpoint = token + strlen(token);
 122                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 123                 dpoint[-2] = '\0';
 124                 MACRO_PARSE_INT(tmpi, token);
 125                 _gps.lon = tmpi + (tmpd * (1.0 / 60.0));
 126         }
 127
 128         /* Parse E or W. */
 129         token = strsep(&sentence, DELIM);
 130         if (token && *token == 'W')
 131                 _gps.lon = -_gps.lon;
 132
 133         /* Parse speed over ground, knots. */
 134         token = strsep(&sentence, DELIM);
 135         if (token && *token) {
 136                 MACRO_PARSE_FLOAT(_gps.speed, token);
 137                 if (_gps.fix > 1) {
 138                         _gps.maxspeed = MAX(_gps.maxspeed, _gps.speed);
 139                         gps_display_data_speed(info_banner.speed, _gps.speed);
 140                 }
 141         }
 142
 143         /* Parse heading, degrees from true north. */
 144         token = strsep(&sentence, DELIM);
 145         if (token && *token) {
 146                 MACRO_PARSE_FLOAT(_gps.heading, token);
 147         }
 148
 149         /* Parse date. */
 150         token = strsep(&sentence, DELIM);
 151         if (token && *token && gpsdate) {
 152                 struct tm time;
 153                 gpsdate[6] = '\0';      /* Make sure time is 6 chars long. */
 154                 strcat(gpsdate, token);
 155                 strptime(gpsdate, "%H%M%S%d%m%y", &time);
 156                 _pos.time = mktime(&time) + _gmtoffset;
 157         } else
 158                 _pos.time = time(NULL);
 159
 160         /* Add new data to track only if we have a fix */
 161         _track_store=TRUE;
 162
 163         /* XXX: Set filter logic somewhere else */
 164
 165         if ((_conn_state == RCVR_FIXED) && (_track_store==TRUE)) {
 166                 if ((_gps_filter==TRUE) && (track_drop_cnt<GPS_FILTER_MAX_SKIP)) {
 167                         integerize_data(_vel_offsetx, _vel_offsety, _pos, _gps);
 168                         if ( (_gps.hdop<_filter_hdop || _filter_hdop==0.0) &&
 169                                 (_gps.vdop<_filter_vdop || _filter_vdop==0.0) &&
 170                                 (fabs(_gps.heading-_gps.lheading)>_filter_angle || _filter_angle==0.0 ) &&
 171                                 (_map_location_known==TRUE && (_map_location_dist<_filter_osm || _map_location_dist==0.0)) ) {
 172                                 track_add(_pos.time, newly_fixed);
 173                                 _gps.lheading=_gps.heading;
 174                                 track_drop_cnt=0;
 175                         } else {
 176                                 track_drop_cnt++;
 177                                 g_printf("*** Filtering by: [%s %s %s %s] A: %f (%d)\n",
 178                                         _gps.hdop>_filter_hdop ? "HDOP" : "-",
 179                                         _gps.vdop>_filter_vdop ? "VDOP" : "-",
 180                                         (fabs(_gps.heading-_gps.lheading)<_filter_angle) ? "Angle" : "-",
 181                                         (_map_location_known==TRUE && (_map_location_dist>_filter_osm)) ? "OSM" : "-",
 182                                         (fabs(_gps.heading-_gps.lheading)<_filter_angle),
 183                                         track_drop_cnt);
 184                         }
 185                         map_refresh_mark();
 186                 } else {
 187                         track_drop_cnt=0;
 188                         integerize_data(_vel_offsetx, _vel_offsety, _pos, _gps);
 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;
 281         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 282
 283 #define DELIM ","
 284
 285         /* Skip Auto selection. */
 286         token = strsep(&sentence, DELIM);
 287
 288         /* 3D fix. */
 289         token = strsep(&sentence, DELIM);
 290         if (token && *token)
 291                 MACRO_PARSE_INT(_gps.fix, token);
 292
 293         _gps.satinuse = 0;
 294         for (i = 0; i < 12; i++) {
 295                 token = strsep(&sentence, DELIM);
 296                 if (token && *token)
 297                         _gps.satforfix[_gps.satinuse++] = atoi(token);
 298         }
 299
 300         /* PDOP */
 301         token = strsep(&sentence, DELIM);
 302         if (token && *token)
 303                 MACRO_PARSE_FLOAT(_gps.pdop, token);
 304
 305         /* HDOP */
 306         token = strsep(&sentence, DELIM);
 307         if (token && *token)
 308                 MACRO_PARSE_FLOAT(_gps.hdop, token);
 309
 310         /* VDOP */
 311         token = strsep(&sentence, DELIM);
 312         if (token && *token)
 313                 MACRO_PARSE_FLOAT(_gps.vdop, token);
 314
 315         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 316 }
 317
 318 static void
 319 channel_parse_gsv(gchar * sentence)
 320 {
 321         /* Must be GSV - Satellites in view
 322          *  1) total number of messages
 323          *  2) message number
 324          *  3) satellites in view
 325          *  4) satellite number
 326          *  5) elevation in degrees (0-90)
 327          *  6) azimuth in degrees to true north (0-359)
 328          *  7) SNR in dB (0-99)
 329          *  more satellite infos like 4)-7)
 330          *  n) checksum
 331          */
 332         gchar *token;
 333         guint msgcnt = 0, nummsgs = 0;
 334         static guint running_total = 0;
 335         static guint num_sats_used = 0;
 336         static guint satcnt = 0;
 337         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 338
 339         /* Parse number of messages. */
 340         token = strsep(&sentence, DELIM);
 341         if (token && *token)
 342                 MACRO_PARSE_INT(nummsgs, token);
 343
 344         /* Parse message number. */
 345         token = strsep(&sentence, DELIM);
 346         if (token && *token)
 347                 MACRO_PARSE_INT(msgcnt, token);
 348
 349         /* Parse number of satellites in view. */
 350         token = strsep(&sentence, DELIM);
 351         if (token && *token) {
 352                 MACRO_PARSE_INT(_gps.satinview, token);
 353                 if (_gps.satinview > 12)        /* Handle buggy NMEA. */
 354                         _gps.satinview = 12;
 355         }
 356
 357         /* Loop until there are no more satellites to parse. */
 358         while (sentence && satcnt < 12) {
 359                 /* Get token for Satellite Number. */
 360                 token = strsep(&sentence, DELIM);
 361                 if (token && *token)
 362                         _gps_sat[satcnt].prn = atoi(token);
 363
 364                 /* Get token for elevation in degrees (0-90). */
 365                 token = strsep(&sentence, DELIM);
 366                 if (token && *token)
 367                         _gps_sat[satcnt].elevation = atoi(token);
 368
 369                 /* Get token for azimuth in degrees to true north (0-359). */
 370                 token = strsep(&sentence, DELIM);
 371                 if (token && *token)
 372                         _gps_sat[satcnt].azimuth = atoi(token);
 373
 374                 /* Get token for SNR. */
 375                 token = strsep(&sentence, DELIM);
 376                 if (token && *token && (_gps_sat[satcnt].snr = atoi(token))) {
 377                         /* SNR is non-zero - add to total and count as used. */
 378                         running_total += _gps_sat[satcnt].snr;
 379                         num_sats_used++;
 380                 }
 381                 satcnt++;
 382         }
 383
 384         if (msgcnt == nummsgs) {
 385                 /*  This is the last message. Calculate signal strength. */
 386                 if (num_sats_used) {
 387                         if (_conn_state == RCVR_UP) {
 388                                 gdouble fraction = running_total * sqrt(num_sats_used) / num_sats_used / 100.0;
 389                                 BOUND(fraction, 0.0, 1.0);
 390                                 set_fix_progress(fraction);
 391                         }
 392                         running_total = 0;
 393                         num_sats_used = 0;
 394                 }
 395                 satcnt = 0;
 396
 397                 /* Keep awake while they watch the progress bar. */
 398                 KEEP_DISPLAY_ON();
 399         }
 400
 401         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 402 }
 403
 404 gboolean
 405 channel_cb_input(GIOChannel * src, GIOCondition condition, gpointer data)
 406 {
 407         gsize bytes_read;
 408         vprintf("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 409
 410         if (G_IO_STATUS_NORMAL == g_io_channel_read_chars(_channel,
 411                                                           _gps_read_buf_curr,
 412                                                           _gps_read_buf_last -
 413                                                           _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 }