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