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 #ifdef WITH_HILDON
  19 #include <hildon-widgets/hildon-banner.h>
  20 #endif
  21
  22 #include "utils.h"
  23 #include "bt.h"
  24 #include "gps.h"
  25 #include "mapper-types.h"
  26 #include "ui-common.h"
  27 #include "map.h"
  28 #include "track.h"
  29 #include "route.h"
  30
  31 #include "settings.h"
  32
  33 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 void channel_parse_rmc(gchar * sentence)
  56 {
  57         /* Recommended Minimum Navigation Information C
  58          *  1) UTC Time
  59          *  2) Status, V=Navigation receiver warning A=Valid
  60          *  3) Latitude
  61          *  4) N or S
  62          *  5) Longitude
  63          *  6) E or W
  64          *  7) Speed over ground, knots
  65          *  8) Track made good, degrees true
  66          *  9) Date, ddmmyy
  67          * 10) Magnetic Variation, degrees
  68          * 11) E or W
  69          * 12) FAA mode indicator (NMEA 2.3 and later)
  70          * 13) Checksum
  71          */
  72         gchar *token, *dpoint, *gpsdate = NULL;
  73         gdouble tmpd = 0.f;
  74         guint tmpi = 0;
  75         gboolean newly_fixed = FALSE;
  76         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
  77
  78 #define DELIM ","
  79
  80         /* Parse time. */
  81         token = strsep(&sentence, DELIM);
  82         if (token && *token)
  83                 gpsdate = token;
  84
  85         token = strsep(&sentence, DELIM);
  86         /* Token is now Status. */
  87         if (token && *token == 'A') {
  88                 /* Data is valid. */
  89                 if (_conn_state != RCVR_FIXED) {
  90                         newly_fixed = TRUE;
  91                         set_conn_state(RCVR_FIXED);
  92                 }
  93         } else {
  94                 /* Data is invalid - not enough satellites?. */
  95                 if (_conn_state != RCVR_UP) {
  96                         set_conn_state(RCVR_UP);
  97                         track_add(0, FALSE);
  98                 }
  99         }
 100
 101         /* Parse the latitude. */
 102         token = strsep(&sentence, DELIM);
 103         if (token && *token) {
 104                 dpoint = strchr(token, '.');
 105                 if (!dpoint)    /* handle buggy NMEA */
 106                         dpoint = token + strlen(token);
 107                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 108                 dpoint[-2] = '\0';
 109                 MACRO_PARSE_INT(tmpi, token);
 110                 _gps.lat = tmpi + (tmpd * (1.0 / 60.0));
 111         }
 112
 113         /* Parse N or S. */
 114         token = strsep(&sentence, DELIM);
 115         if (token && *token == 'S')
 116                 _gps.lat = -_gps.lat;
 117
 118         /* Parse the longitude. */
 119         token = strsep(&sentence, DELIM);
 120         if (token && *token) {
 121                 dpoint = strchr(token, '.');
 122                 if (!dpoint)    /* handle buggy NMEA */
 123                         dpoint = token + strlen(token);
 124                 MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
 125                 dpoint[-2] = '\0';
 126                 MACRO_PARSE_INT(tmpi, token);
 127                 _gps.lon = tmpi + (tmpd * (1.0 / 60.0));
 128         }
 129
 130         /* Parse E or W. */
 131         token = strsep(&sentence, DELIM);
 132         if (token && *token == 'W')
 133                 _gps.lon = -_gps.lon;
 134
 135         /* Parse speed over ground, knots. */
 136         token = strsep(&sentence, DELIM);
 137         if (token && *token) {
 138                 MACRO_PARSE_FLOAT(_gps.speed, token);
 139                 if (_gps.fix > 1)
 140                         _gps.maxspeed = MAX(_gps.maxspeed, _gps.speed);
 141         }
 142
 143         /* Parse heading, degrees from true north. */
 144         token = strsep(&sentence, DELIM);
 145         if (token && *token) {
 146                 MACRO_PARSE_FLOAT(_gps.heading, token);
 147         }
 148
 149         /* Parse date. */
 150         token = strsep(&sentence, DELIM);
 151         if (token && *token && gpsdate) {
 152                 struct tm time;
 153                 gpsdate[6] = '\0';      /* Make sure time is 6 chars long. */
 154                 strcat(gpsdate, token);
 155                 strptime(gpsdate, "%H%M%S%d%m%y", &time);
 156                 _pos.time = mktime(&time) + _gmtoffset;
 157         } else
 158                 _pos.time = time(NULL);
 159
 160         /* Add new data to track only if we have a fix */
 161         _track_store=TRUE;
 162
 163         /* XXX: Set filter logic somewhere else */
 164
 165         if ((_conn_state == RCVR_FIXED) && (_track_store==TRUE)) {
 166                 if ( (_gps_filter==TRUE) && (track_drop_cnt<10) ) {
 167                         integerize_data(_vel_offsetx, _vel_offsety, _pos, _gps);
 168                         if ( (_gps.hdop<_filter_hdop || _filter_hdop==0.0) &&
 169                                 (_gps.vdop<_filter_vdop || _filter_vdop==0.0) &&
 170                                 (fabs(_gps.heading-_gps.lheading)>_filter_angle || _filter_angle==0.0 ) &&
 171                                 (_map_location_known==TRUE && (_map_location_dist<_filter_osm || _map_location_dist==0.0)) ) {
 172                                 track_add(_pos.time, newly_fixed);
 173                                 _gps.lheading=_gps.heading;
 174                                 track_drop_cnt=0;
 175                         } else {
 176                                 track_drop_cnt++;
 177                                 g_printf("*** Filtering by: [%s %s %s %s] (%d)\n",
 178                                         _gps.hdop<_filter_hdop ? "HDOP" : "-",
 179                                         _gps.vdop<_filter_vdop ? "VDOP" : "-",
 180                                         (fabs(_gps.heading-_gps.lheading)<_filter_angle) ? "Angle" : "-",
 181                                         (_map_location_known==TRUE && (_map_location_dist<_filter_osm)) ? "OSM" : "-", track_drop_cnt);
 182                         }
 183                         refresh_mark();
 184                 } else {
 185                         integerize_data(_vel_offsetx, _vel_offsety, _pos, _gps);
 186                         track_add(_pos.time, newly_fixed);
 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 =
 382                                     running_total * sqrt(num_sats_used)
 383                                     / num_sats_used / 100.0;
 384                                 BOUND(fraction, 0.0, 1.0);
 385                                 hildon_banner_set_fraction(HILDON_BANNER(_fix_banner), fraction);
 386                         }
 387                         running_total = 0;
 388                         num_sats_used = 0;
 389                 }
 390                 satcnt = 0;
 391
 392                 /* Keep awake while they watch the progress bar. */
 393                 KEEP_DISPLAY_ON();
 394         }
 395
 396         vprintf("%s(): return\n", __PRETTY_FUNCTION__);
 397 }
 398
 399 gboolean
 400 channel_cb_input(GIOChannel * src, GIOCondition condition, gpointer data)
 401 {
 402         gsize bytes_read;
 403         vprintf("%s(%d)\n", __PRETTY_FUNCTION__, condition);
 404
 405         if (G_IO_STATUS_NORMAL == g_io_channel_read_chars(_channel,
 406                                                           _gps_read_buf_curr,
 407                                                           _gps_read_buf_last -
 408                                                           _gps_read_buf_curr,
 409                                                           &bytes_read, NULL)) {
 410                 gchar *eol;
 411
 412                 _gps_read_buf_curr += bytes_read;
 413                 *_gps_read_buf_curr = '\0';     /* append a \0 so we can read as string */
 414                 while ((eol = strchr(_gps_read_buf, '\n'))) {
 415                         gchar *sptr = _gps_read_buf + 1;        /* Skip the $ */
 416                         guint csum = 0;
 417                         if (*_gps_read_buf == '$') {
 418                                 /* This is the beginning of a sentence; okay to parse. */
 419                                 *eol = '\0';    /* overwrite \n with \0 */
 420                                 while (*sptr && *sptr != '*')
 421                                         csum ^= *sptr++;
 422
 423                                 /* If we're at a \0 (meaning there is no checksum), or if the
 424                                  * checksum is good, then parse the sentence. */
 425                                 if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
 426                                         if (*sptr)
 427                                                 *sptr = '\0';   /* take checksum out of the buffer. */
 428                                         if (!strncmp(_gps_read_buf + 3, "GSV", 3)) {
 429                                                 if (_conn_state == RCVR_UP)
 430                                                         channel_parse_gsv(_gps_read_buf + 7);
 431                                         } else if (!strncmp(_gps_read_buf + 3, "RMC", 3))
 432                                                 channel_parse_rmc(_gps_read_buf + 7);
 433                                         else if (!strncmp(_gps_read_buf + 3, "GGA", 3))
 434                                                 channel_parse_gga(_gps_read_buf + 7);
 435                                         else if (!strncmp(_gps_read_buf + 3, "GSA", 3))
 436                                                 channel_parse_gsa(_gps_read_buf + 7);
 437                                         else g_print("Unknown NMEA: [%s]\n", _gps_read_buf);
 438                                         if (_gps_info)
 439                                                 gps_display_data();
 440                                         if (_satdetails_on)
 441                                                 gps_display_details();
 442                                 } else {
 443                                         /* There was a checksum, and it was bad. */
 444                                         g_printerr
 445                                             ("%s: Bad checksum in NMEA sentence:\n%s\n",
 446                                              __PRETTY_FUNCTION__,
 447                                              _gps_read_buf);
 448                                 }
 449                         }
 450
 451                         /* If eol is at or after (_gps_read_buf_curr - 1) */
 452                         if (eol >= (_gps_read_buf_curr - 1)) {
 453                                 /* Last read was a newline - reset read buffer */
 454                                 _gps_read_buf_curr = _gps_read_buf;
 455                                 *_gps_read_buf_curr = '\0';
 456                         } else {
 457                                 /* Move the next line to the front of the buffer. */
 458                                 memmove(_gps_read_buf, eol + 1, _gps_read_buf_curr - eol);      /* include terminating 0 */
 459                                 /* Subtract _curr so that it's pointing at the new \0. */
 460                                 _gps_read_buf_curr -= (eol - _gps_read_buf + 1);
 461                         }
 462                 }
 463         }
 464
 465         vprintf("%s(): return TRUE\n", __PRETTY_FUNCTION__);
 466         return TRUE;
 467 }