err.no Git - mapper/blob - src/gps-nmea-parse.c

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