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                         _gps.lheading=_gps.heading;
 187                         refresh_mark();
 188                 }
 189         }
 190
 191         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 192 }
 193
 194 void channel_parse_gga(gchar * sentence)
 195 {
 196         /* GGA          Global Positioning System Fix Data
 197            1. Fix Time
 198            2. Latitude
 199            3. N or S
 200            4. Longitude
 201            5. E or W
 202            6. Fix quality
 203            0 = invalid
 204            1 = GPS fix (SPS)
 205            2 = DGPS fix
 206            3 = PPS fix
 207            4 = Real Time Kinematic
 208            5 = Float RTK
 209            6 = estimated (dead reckoning) (2.3 feature)
 210            7 = Manual input mode
 211            8 = Simulation mode
 212            7. Number of satellites being tracked
 213            8. Horizontal dilution of position
 214            9. Altitude, Meters, above mean sea level
 215            10. Alt unit (meters)
 216            11. Height of geoid (mean sea level) above WGS84 ellipsoid
 217            12. unit
 218            13. (empty field) time in seconds since last DGPS update
 219            14. (empty field) DGPS station ID number
 220            15. the checksum data
 221          */
 222         gchar *token;
 223         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 224
 225 #define DELIM ","
 226
 227         /* Skip Fix time */
 228         token = strsep(&sentence, DELIM);
 229         /* Skip latitude */
 230         token = strsep(&sentence, DELIM);
 231         /* Skip N or S */
 232         token = strsep(&sentence, DELIM);
 233         /* Skip longitude */
 234         token = strsep(&sentence, DELIM);
 235         /* Skip S or W */
 236         token = strsep(&sentence, DELIM);
 237
 238         /* Parse Fix quality */
 239         token = strsep(&sentence, DELIM);
 240         if (token && *token)
 241                 MACRO_PARSE_INT(_gps.fixquality, token);
 242
 243         /* Skip number of satellites */
 244         token = strsep(&sentence, DELIM);
 245
 246         /* Parse Horizontal dilution of position */
 247         token = strsep(&sentence, DELIM);
 248         if (token && *token)
 249                 MACRO_PARSE_INT(_gps.hdop, token);
 250
 251         /* Altitude */
 252         token = strsep(&sentence, DELIM);
 253         if (token && *token) {
 254                 MACRO_PARSE_FLOAT(_pos.altitude, token);
 255         } else
 256                 _pos.altitude = NAN;
 257
 258         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 259 }
 260
 261 void channel_parse_gsa(gchar * sentence)
 262 {
 263         /* GPS DOP and active satellites
 264          *  1) Auto selection of 2D or 3D fix (M = manual)
 265          *  2) 3D fix - values include: 1 = no fix, 2 = 2D, 3 = 2D
 266          *  3) PRNs of satellites used for fix
 267          *     (space for 12)
 268          *  4) PDOP (dilution of precision)
 269          *  5) Horizontal dilution of precision (HDOP)
 270          *  6) Vertical dilution of precision (VDOP)
 271          *  7) Checksum
 272          */
 273         gchar *token;
 274         guint i;
 275         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 276
 277 #define DELIM ","
 278
 279         /* Skip Auto selection. */
 280         token = strsep(&sentence, DELIM);
 281
 282         /* 3D fix. */
 283         token = strsep(&sentence, DELIM);
 284         if (token && *token)
 285                 MACRO_PARSE_INT(_gps.fix, token);
 286
 287         _gps.satinuse = 0;
 288         for (i = 0; i < 12; i++) {
 289                 token = strsep(&sentence, DELIM);
 290                 if (token && *token)
 291                         _gps.satforfix[_gps.satinuse++] = atoi(token);
 292         }
 293
 294         /* PDOP */
 295         token = strsep(&sentence, DELIM);
 296         if (token && *token)
 297                 MACRO_PARSE_FLOAT(_gps.pdop, token);
 298
 299         /* HDOP */
 300         token = strsep(&sentence, DELIM);
 301         if (token && *token)
 302                 MACRO_PARSE_FLOAT(_gps.hdop, token);
 303
 304         /* VDOP */
 305         token = strsep(&sentence, DELIM);
 306         if (token && *token)
 307                 MACRO_PARSE_FLOAT(_gps.vdop, token);
 308
 309         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 310 }
 311
 312 void channel_parse_gsv(gchar * sentence)
 313 {
 314         /* Must be GSV - Satellites in view
 315          *  1) total number of messages
 316          *  2) message number
 317          *  3) satellites in view
 318          *  4) satellite number
 319          *  5) elevation in degrees (0-90)
 320          *  6) azimuth in degrees to true north (0-359)
 321          *  7) SNR in dB (0-99)
 322          *  more satellite infos like 4)-7)
 323          *  n) checksum
 324          */
 325         gchar *token;
 326         guint msgcnt = 0, nummsgs = 0;
 327         static guint running_total = 0;
 328         static guint num_sats_used = 0;
 329         static guint satcnt = 0;
 330         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 331
 332         /* Parse number of messages. */
 333         token = strsep(&sentence, DELIM);
 334         if (token && *token)
 335                 MACRO_PARSE_INT(nummsgs, token);
 336
 337         /* Parse message number. */
 338         token = strsep(&sentence, DELIM);
 339         if (token && *token)
 340                 MACRO_PARSE_INT(msgcnt, token);
 341
 342         /* Parse number of satellites in view. */
 343         token = strsep(&sentence, DELIM);
 344         if (token && *token) {
 345                 MACRO_PARSE_INT(_gps.satinview, token);
 346                 if (_gps.satinview > 12)        /* Handle buggy NMEA. */
 347                         _gps.satinview = 12;
 348         }
 349
 350         /* Loop until there are no more satellites to parse. */
 351         while (sentence && satcnt < 12) {
 352                 /* Get token for Satellite Number. */
 353                 token = strsep(&sentence, DELIM);
 354                 if (token && *token)
 355                         _gps_sat[satcnt].prn = atoi(token);
 356
 357                 /* Get token for elevation in degrees (0-90). */
 358                 token = strsep(&sentence, DELIM);
 359                 if (token && *token)
 360                         _gps_sat[satcnt].elevation = atoi(token);
 361
 362                 /* Get token for azimuth in degrees to true north (0-359). */
 363                 token = strsep(&sentence, DELIM);
 364                 if (token && *token)
 365                         _gps_sat[satcnt].azimuth = atoi(token);
 366
 367                 /* Get token for SNR. */
 368                 token = strsep(&sentence, DELIM);
 369                 if (token && *token && (_gps_sat[satcnt].snr = atoi(token))) {
 370                         /* SNR is non-zero - add to total and count as used. */
 371                         running_total += _gps_sat[satcnt].snr;
 372                         num_sats_used++;
 373                 }
 374                 satcnt++;
 375         }
 376
 377         if (msgcnt == nummsgs) {
 378                 /*  This is the last message. Calculate signal strength. */
 379                 if (num_sats_used) {
 380                         if (_conn_state == RCVR_UP) {
 381                                 gdouble fraction = running_total * sqrt(num_sats_used) / num_sats_used / 100.0;
 382                                 BOUND(fraction, 0.0, 1.0);
 383                                 set_fix_progress(fraction);
 384                         }
 385                         running_total = 0;
 386                         num_sats_used = 0;
 387                 }
 388                 satcnt = 0;
 389
 390                 /* Keep awake while they watch the progress bar. */
 391                 KEEP_DISPLAY_ON();
 392         }
 393
 394         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 395 }
 396
 397 gboolean
 398 channel_cb_input(GIOChannel * src, GIOCondition condition, gpointer data)
 399 {
 400         gsize bytes_read;
 401         vprintf("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 402
 403         if (G_IO_STATUS_NORMAL == g_io_channel_read_chars(_channel,
 404                                                           _gps_read_buf_curr,
 405                                                           _gps_read_buf_last -
 406                                                           _gps_read_buf_curr,
 407                                                           &bytes_read, NULL)) {
 408                 gchar *eol;
 409
 410                 _gps_read_buf_curr += bytes_read;
 411                 *_gps_read_buf_curr = '\0';     /* append a \0 so we can read as string */
 412                 while ((eol = strchr(_gps_read_buf, '\n'))) {
 413                         gchar *sptr = _gps_read_buf + 1;        /* Skip the $ */
 414                         guint csum = 0;
 415                         if (*_gps_read_buf == '$') {
 416                                 /* This is the beginning of a sentence; okay to parse. */
 417                                 *eol = '\0';    /* overwrite \n with \0 */
 418                                 while (*sptr && *sptr != '*')
 419                                         csum ^= *sptr++;
 420
 421                                 /* If we're at a \0 (meaning there is no checksum), or if the
 422                                  * checksum is good, then parse the sentence. */
 423                                 if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
 424                                         if (*sptr)
 425                                                 *sptr = '\0';   /* take checksum out of the buffer. */
 426                                         if (!strncmp(_gps_read_buf + 3, "GSV", 3)) {
 427                                                 if (_conn_state == RCVR_UP)
 428                                                         channel_parse_gsv(_gps_read_buf + 7);
 429                                         } else if (!strncmp(_gps_read_buf + 3, "RMC", 3))
 430                                                 channel_parse_rmc(_gps_read_buf + 7);
 431                                         else if (!strncmp(_gps_read_buf + 3, "GGA", 3))
 432                                                 channel_parse_gga(_gps_read_buf + 7);
 433                                         else if (!strncmp(_gps_read_buf + 3, "GSA", 3))
 434                                                 channel_parse_gsa(_gps_read_buf + 7);
 435                                         else g_print("Unknown NMEA: [%s]\n", _gps_read_buf);
 436                                         if (_gps_info)
 437                                                 gps_display_data();
 438                                         if (_satdetails_on)
 439                                                 gps_display_details();
 440                                 } else {
 441                                         /* There was a checksum, and it was bad. */
 442                                         g_printerr
 443                                             ("%s: Bad checksum in NMEA sentence:\n%s\n",
 444                                              __PRETTY_FUNCTION__,
 445                                              _gps_read_buf);
 446                                 }
 447                         }
 448
 449                         /* If eol is at or after (_gps_read_buf_curr - 1) */
 450                         if (eol >= (_gps_read_buf_curr - 1)) {
 451                                 /* Last read was a newline - reset read buffer */
 452                                 _gps_read_buf_curr = _gps_read_buf;
 453                                 *_gps_read_buf_curr = '\0';
 454                         } else {
 455                                 /* Move the next line to the front of the buffer. */
 456                                 memmove(_gps_read_buf, eol + 1, _gps_read_buf_curr - eol);      /* include terminating 0 */
 457                                 /* Subtract _curr so that it's pointing at the new \0. */
 458                                 _gps_read_buf_curr -= (eol - _gps_read_buf + 1);
 459                         }
 460                 }
 461         }
 462
 463         vprintf("%s(): return TRUE\n", __PRETTY_FUNCTION__);
 464         return TRUE;
 465 }