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