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,si;
 283         gint satforfix[12];
 284
 285         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 286
 287 #define DELIM ","
 288
 289         /* Skip Auto selection. */
 290         token = strsep(&sentence, DELIM);
 291
 292         /* 3D fix. */
 293         token = strsep(&sentence, DELIM);
 294         if (token && *token)
 295                 MACRO_PARSE_INT(_gps.fix, token);
 296
 297         _gps.satinuse = 0;
 298         for (i = 0; i < 12; i++) {
 299                 gint fprn;
 300                 token = strsep(&sentence, DELIM);
 301                 if (token && *token)
 302                         fprn=atoi(token);
 303                 else
 304                         fprn=-1;
 305                 satforfix[i]=fprn;
 306                 _gps.sat[i].fix=FALSE;
 307         }
 308
 309         for (i=0;i<12;i++)
 310                 for (si=0;si<12;si++) {
 311                         if (_gps.sat[i].prn==satforfix[si])
 312                                 _gps.sat[i].fix=TRUE;
 313         }
 314
 315         /* PDOP */
 316         token = strsep(&sentence, DELIM);
 317         if (token && *token)
 318                 MACRO_PARSE_FLOAT(_gps.pdop, token);
 319
 320         /* HDOP */
 321         token = strsep(&sentence, DELIM);
 322         if (token && *token)
 323                 MACRO_PARSE_FLOAT(_gps.hdop, token);
 324
 325         /* VDOP */
 326         token = strsep(&sentence, DELIM);
 327         if (token && *token)
 328                 MACRO_PARSE_FLOAT(_gps.vdop, token);
 329
 330         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 331 }
 332
 333 static void
 334 channel_parse_gsv(gchar * sentence)
 335 {
 336         /* Must be GSV - Satellites in view
 337          *  1) total number of messages
 338          *  2) message number
 339          *  3) satellites in view
 340          *  4) satellite number
 341          *  5) elevation in degrees (0-90)
 342          *  6) azimuth in degrees to true north (0-359)
 343          *  7) SNR in dB (0-99)
 344          *  more satellite infos like 4)-7)
 345          *  n) checksum
 346          */
 347         gchar *token;
 348         guint msgcnt = 0, nummsgs = 0;
 349         static guint running_total = 0;
 350         static guint num_sats_used = 0;
 351         static guint satcnt = 0;
 352         g_printf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 353
 354         /* Parse number of messages. */
 355         token = strsep(&sentence, DELIM);
 356         if (token && *token)
 357                 MACRO_PARSE_INT(nummsgs, token);
 358
 359         /* Parse message number. */
 360         token = strsep(&sentence, DELIM);
 361         if (token && *token)
 362                 MACRO_PARSE_INT(msgcnt, token);
 363
 364         /* Parse number of satellites in view. */
 365         token = strsep(&sentence, DELIM);
 366         if (token && *token) {
 367                 MACRO_PARSE_INT(_gps.satinview, token);
 368                 if (_gps.satinview > 12)        /* Handle buggy NMEA. */
 369                         _gps.satinview = 12;
 370         }
 371
 372         /* Loop until there are no more satellites to parse. */
 373         while (sentence && satcnt < 12) {
 374                 /* Get token for Satellite Number. */
 375                 token = strsep(&sentence, DELIM);
 376                 if (token && *token)
 377                         _gps.sat[satcnt].prn = atoi(token);
 378
 379                 /* Get token for elevation in degrees (0-90). */
 380                 token = strsep(&sentence, DELIM);
 381                 if (token && *token)
 382                         _gps.sat[satcnt].elevation = atoi(token);
 383
 384                 /* Get token for azimuth in degrees to true north (0-359). */
 385                 token = strsep(&sentence, DELIM);
 386                 if (token && *token)
 387                         _gps.sat[satcnt].azimuth = atoi(token);
 388
 389                 /* Get token for SNR. */
 390                 token = strsep(&sentence, DELIM);
 391                 if (token && *token && (_gps.sat[satcnt].snr = atoi(token))) {
 392                         /* SNR is non-zero - add to total and count as used. */
 393                         running_total += _gps.sat[satcnt].snr;
 394                         num_sats_used++;
 395                 }
 396                 satcnt++;
 397         }
 398
 399         if (msgcnt == nummsgs) {
 400                 /*  This is the last message. Calculate signal strength. */
 401                 if (num_sats_used) {
 402                         if (_conn_state == RCVR_UP) {
 403                                 gdouble fraction = running_total * sqrt(num_sats_used) / num_sats_used / 100.0;
 404                                 BOUND(fraction, 0.0, 1.0);
 405                                 set_fix_progress(fraction);
 406                         }
 407                         running_total = 0;
 408                         num_sats_used = 0;
 409                 }
 410                 satcnt = 0;
 411
 412                 /* Keep awake while they watch the progress bar. */
 413                 KEEP_DISPLAY_ON();
 414         }
 415
 416         g_printf("%s(): return\n", __PRETTY_FUNCTION__);
 417 }
 418
 419 gboolean
 420 channel_cb_input(GIOChannel * src, GIOCondition condition, gpointer data)
 421 {
 422         gsize bytes_read;
 423         vprintf("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 424
 425         if (G_IO_STATUS_NORMAL == g_io_channel_read_chars(_channel,
 426                                                           _gps_read_buf_curr, _gps_read_buf_last - _gps_read_buf_curr,
 427                                                           &bytes_read, NULL)) {
 428                 gchar *eol;
 429
 430                 _gps_read_buf_curr += bytes_read;
 431                 *_gps_read_buf_curr = '\0';     /* append a \0 so we can read as string */
 432                 while ((eol = strchr(_gps_read_buf, '\n'))) {
 433                         gchar *sptr = _gps_read_buf + 1;        /* Skip the $ */
 434                         guint csum = 0;
 435                         if (*_gps_read_buf == '$') {
 436                                 /* This is the beginning of a sentence; okay to parse. */
 437                                 *eol = '\0';    /* overwrite \n with \0 */
 438                                 while (*sptr && *sptr != '*')
 439                                         csum ^= *sptr++;
 440
 441                                 /* If we're at a \0 (meaning there is no checksum), or if the
 442                                  * checksum is good, then parse the sentence. */
 443                                 if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
 444                                         if (*sptr)
 445                                                 *sptr = '\0';   /* take checksum out of the buffer. */
 446                                         if (!strncmp(_gps_read_buf + 3, "GSV", 3)) {
 447                                                 if (_conn_state == RCVR_UP)
 448                                                         channel_parse_gsv(_gps_read_buf + 7);
 449                                         } else if (!strncmp(_gps_read_buf + 3, "RMC", 3))
 450                                                 channel_parse_rmc(_gps_read_buf + 7);
 451                                         else if (!strncmp(_gps_read_buf + 3, "GGA", 3))
 452                                                 channel_parse_gga(_gps_read_buf + 7);
 453                                         else if (!strncmp(_gps_read_buf + 3, "GSA", 3))
 454                                                 channel_parse_gsa(_gps_read_buf + 7);
 455                                         else g_print("Unknown NMEA: [%s]\n", _gps_read_buf);
 456
 457                                         if (_gps_info)
 458                                                 gps_display_data();
 459
 460                                         if (_satdetails_on)
 461                                                 gps_display_details();
 462                                 } else {
 463                                         /* There was a checksum, and it was bad. */
 464                                         g_printerr
 465                                             ("%s: Bad checksum in NMEA sentence:\n%s\n",
 466                                              __PRETTY_FUNCTION__,
 467                                              _gps_read_buf);
 468                                 }
 469                         }
 470
 471                         /* If eol is at or after (_gps_read_buf_curr - 1) */
 472                         if (eol >= (_gps_read_buf_curr - 1)) {
 473                                 /* Last read was a newline - reset read buffer */
 474                                 _gps_read_buf_curr = _gps_read_buf;
 475                                 *_gps_read_buf_curr = '\0';
 476                         } else {
 477                                 /* Move the next line to the front of the buffer. */
 478                                 memmove(_gps_read_buf, eol + 1, _gps_read_buf_curr - eol);      /* include terminating 0 */
 479                                 /* Subtract _curr so that it's pointing at the new \0. */
 480                                 _gps_read_buf_curr -= (eol - _gps_read_buf + 1);
 481                         }
 482                 }
 483         }
 484
 485         vprintf("%s(): return TRUE\n", __PRETTY_FUNCTION__);
 486         return TRUE;
 487 }