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 "gps.h"
  43 #include "mapper-types.h"
  44 #include "ui-common.h"
  45 #include "map.h"
  46 #include "track.h"
  47 #include "route.h"
  48 #include "gps-panels.h"
  49 #include "settings.h"
  50 #include "gps-conn.h"
  51 #include "gtkgps.h"
  52 #include "gtkcompass.h"
  53
  54 static void
  55 gps_nmea_parse_rmc(Gps *gps, 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 (gps->io.conn != RCVR_FIXED) {
  90                         newly_fixed = TRUE;
  91                         gps_conn_set_state(gps, RCVR_FIXED);
  92                 }
  93         } else {
  94                 /* Data is invalid - not enough satellites?. */
  95                 if (gps->io.conn != RCVR_UP) {
  96                         gps_conn_set_state(gps, RCVR_UP);
  97                         track_add(NULL, 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->data.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->data.lat = -gps->data.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->data.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->data.lon = -gps->data.lon;
 134
 135         /* Parse speed over ground, knots. */
 136         token = strsep(&sentence, DELIM);
 137         if (token && *token) {
 138                 MACRO_PARSE_FLOAT(gps->data.speed, token);
 139                 if (gps->data.fix > FIX_NOFIX) {
 140                         gps->data.maxspeed=MAX(gps->data.maxspeed, gps->data.speed);
 141                 }
 142         }
 143
 144         /* Parse heading, degrees from true north. */
 145         token = strsep(&sentence, DELIM);
 146         if (token && *token) {
 147                 MACRO_PARSE_FLOAT(gps->data.heading, token);
 148         }
 149
 150         /* Parse date. */
 151         token = strsep(&sentence, DELIM);
 152         if (token && *token && gpsdate) {
 153                 struct tm time;
 154                 gpsdate[6] = '\0';      /* Make sure time is 6 chars long. */
 155                 strcat(gpsdate, token);
 156                 strptime(gpsdate, "%H%M%S%d%m%y", &time);
 157                 gps->data.time = mktime(&time) + _gmtoffset;
 158         } else
 159                 gps->data.time = time(NULL);
 160
 161         /* Add new data to track only if we have a fix */
 162         _track_store=TRUE;
 163
 164         gps_data_integerize(&gps->data);
 165         gps_display_data_speed(info_banner.speed, gps->data.speed);
 166
 167         if ((gps->io.conn==RCVR_FIXED) && (_track_store==TRUE) && filter_check(&filter, &gps->data, &map_loc)==TRUE) {
 168                 track_add(&_gps->data, newly_fixed);
 169                 gps->data.lheading=gps->data.heading;
 170                 map_refresh_mark();
 171         }
 172 }
 173
 174 static void
 175 gps_nmea_parse_gga(Gps *gps, gchar * sentence)
 176 {
 177         /* GGA          Global Positioning System Fix Data
 178            1. Fix Time
 179            2. Latitude
 180            3. N or S
 181            4. Longitude
 182            5. E or W
 183            6. Fix quality
 184            0 = invalid
 185            1 = GPS fix (SPS)
 186            2 = DGPS fix
 187            3 = PPS fix
 188            4 = Real Time Kinematic
 189            5 = Float RTK
 190            6 = estimated (dead reckoning) (2.3 feature)
 191            7 = Manual input mode
 192            8 = Simulation mode
 193            7. Number of satellites being tracked
 194            8. Horizontal dilution of position
 195            9. Altitude, Meters, above mean sea level
 196            10. Alt unit (meters)
 197            11. Height of geoid (mean sea level) above WGS84 ellipsoid
 198            12. unit
 199            13. (empty field) time in seconds since last DGPS update
 200            14. (empty field) DGPS station ID number
 201            15. the checksum data
 202          */
 203         gchar *token;
 204         vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 205
 206 #define DELIM ","
 207
 208         /* Skip Fix time */
 209         token = strsep(&sentence, DELIM);
 210         /* Skip latitude */
 211         token = strsep(&sentence, DELIM);
 212         /* Skip N or S */
 213         token = strsep(&sentence, DELIM);
 214         /* Skip longitude */
 215         token = strsep(&sentence, DELIM);
 216         /* Skip S or W */
 217         token = strsep(&sentence, DELIM);
 218
 219         /* Parse Fix quality */
 220         token = strsep(&sentence, DELIM);
 221         if (token && *token)
 222                 MACRO_PARSE_INT(gps->data.fixquality, token);
 223
 224         /* Skip number of satellites */
 225         token = strsep(&sentence, DELIM);
 226
 227         /* Parse Horizontal dilution of position */
 228         token = strsep(&sentence, DELIM);
 229         if (token && *token)
 230                 MACRO_PARSE_INT(gps->data.hdop, token);
 231
 232         /* Altitude */
 233         token = strsep(&sentence, DELIM);
 234         if (token && *token) {
 235                 MACRO_PARSE_FLOAT(gps->data.altitude, token);
 236         } else
 237                 gps->data.altitude = NAN;
 238 }
 239
 240 static void
 241 gps_nmea_parse_gsa(Gps *gps, gchar * sentence)
 242 {
 243         /* GPS DOP and active satellites
 244          *  1) Auto selection of 2D or 3D fix (M = manual)
 245          *  2) 3D fix - values include: 1 = no fix, 2 = 2D, 3 = 2D
 246          *  3) PRNs of satellites used for fix
 247          *     (space for 12)
 248          *  4) PDOP (dilution of precision)
 249          *  5) Horizontal dilution of precision (HDOP)
 250          *  6) Vertical dilution of precision (VDOP)
 251          *  7) Checksum
 252          */
 253         gchar *token;
 254         guint i,si;
 255         gint satforfix[12];
 256
 257         g_debug("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
 258
 259 #define DELIM ","
 260
 261         /* Skip Auto selection. */
 262         token = strsep(&sentence, DELIM);
 263
 264         /* 3D fix. */
 265         token = strsep(&sentence, DELIM);
 266         if (token && *token)
 267                 MACRO_PARSE_INT(gps->data.fix, token);
 268
 269         gps->data.satinuse = 0;
 270         for (i = 0; i < 12; i++) {
 271                 gint fprn;
 272                 token = strsep(&sentence, DELIM);
 273                 if (token && *token)
 274                         fprn=atoi(token);
 275                 else
 276                         fprn=-1;
 277                 satforfix[i]=fprn;
 278                 gps->data.sat[i].fix=FALSE;
 279         }
 280
 281         for (i=0;i<12;i++)
 282                 for (si=0;si<12;si++) {
 283                         if (gps->data.sat[i].prn==satforfix[si])
 284                                 gps->data.sat[i].fix=TRUE;
 285         }
 286
 287         /* PDOP */
 288         token = strsep(&sentence, DELIM);
 289         if (token && *token)
 290                 MACRO_PARSE_FLOAT(gps->data.pdop, token);
 291
 292         /* HDOP */
 293         token = strsep(&sentence, DELIM);
 294         if (token && *token)
 295                 MACRO_PARSE_FLOAT(gps->data.hdop, token);
 296
 297         /* VDOP */
 298         token = strsep(&sentence, DELIM);
 299         if (token && *token)
 300                 MACRO_PARSE_FLOAT(gps->data.vdop, token);
 301 }
 302
 303 static void
 304 gps_nmea_parse_gsv(Gps *gps, gchar * sentence)
 305 {
 306         /* Must be GSV - Satellites in view
 307          *  1) total number of messages
 308          *  2) message number
 309          *  3) satellites in view
 310          *  4) satellite number
 311          *  5) elevation in degrees (0-90)
 312          *  6) azimuth in degrees to true north (0-359)
 313          *  7) SNR in dB (0-99)
 314          *  more satellite infos like 4)-7)
 315          *  n) checksum
 316          */
 317         gchar *token;
 318         guint msgcnt = 0, nummsgs = 0;
 319         static guint running_total = 0;
 320         static guint num_sats_used = 0;
 321         static guint satcnt = 0;
 322         /* g_printf("%s(): %s\n", __PRETTY_FUNCTION__, sentence); */
 323
 324         /* Parse number of messages. */
 325         token = strsep(&sentence, DELIM);
 326         if (token && *token)
 327                 MACRO_PARSE_INT(nummsgs, token);
 328
 329         /* Parse message number. */
 330         token = strsep(&sentence, DELIM);
 331         if (token && *token)
 332                 MACRO_PARSE_INT(msgcnt, token);
 333
 334         /* Parse number of satellites in view. */
 335         token = strsep(&sentence, DELIM);
 336         if (token && *token) {
 337                 MACRO_PARSE_INT(gps->data.satinview, token);
 338                 if (gps->data.satinview > 12)   /* Handle buggy NMEA. */
 339                         gps->data.satinview = 12;
 340         }
 341
 342         /* Loop until there are no more satellites to parse. */
 343         while (sentence && satcnt < 12) {
 344                 /* Get token for Satellite Number. */
 345                 token = strsep(&sentence, DELIM);
 346                 if (token && *token)
 347                         gps->data.sat[satcnt].prn = atoi(token);
 348
 349                 /* Get token for elevation in degrees (0-90). */
 350                 token = strsep(&sentence, DELIM);
 351                 if (token && *token)
 352                         gps->data.sat[satcnt].elevation = atoi(token);
 353
 354                 /* Get token for azimuth in degrees to true north (0-359). */
 355                 token = strsep(&sentence, DELIM);
 356                 if (token && *token)
 357                         gps->data.sat[satcnt].azimuth = atoi(token);
 358
 359                 /* Get token for SNR. */
 360                 token = strsep(&sentence, DELIM);
 361                 if (token && *token && (gps->data.sat[satcnt].snr = atoi(token))) {
 362                         /* SNR is non-zero - add to total and count as used. */
 363                         running_total += gps->data.sat[satcnt].snr;
 364                         num_sats_used++;
 365                 }
 366                 satcnt++;
 367         }
 368
 369         if (msgcnt == nummsgs) {
 370                 /*  This is the last message. Calculate signal strength. */
 371                 if (num_sats_used) {
 372                         if (gps->io.conn==RCVR_UP) {
 373                                 gdouble fraction = running_total * sqrt(num_sats_used) / num_sats_used / 100.0;
 374                                 BOUND(fraction, 0.0, 1.0);
 375                                 gps_conn_set_progress(gps, fraction);
 376                         }
 377                         running_total=0;
 378                         num_sats_used=0;
 379                 }
 380                 satcnt = 0;
 381
 382                 /* Keep awake while they watch the progress bar. */
 383                 KEEP_DISPLAY_ON();
 384         }
 385 }
 386
 387 gboolean
 388 gps_nmea_parse(gchar *data)
 389 {
 390 Gps *gps=_gps;
 391
 392 if (!strncmp(data + 3, "GSV", 3)) {
 393         if (gps->io.conn==RCVR_UP)
 394                 gps_nmea_parse_gsv(gps, data + 7);
 395 } else if (!strncmp(data + 3, "RMC", 3))
 396         gps_nmea_parse_rmc(gps, data + 7);
 397 else if (!strncmp(data + 3, "GGA", 3))
 398         gps_nmea_parse_gga(gps, data + 7);
 399 else if (!strncmp(data + 3, "GSA", 3))
 400         gps_nmea_parse_gsa(gps, data + 7);
 401 else g_debug("Unknown NMEA: [%s]\n", data);
 402 g_free(data);
 403
 404 if (_gps_info)
 405         gps_display_data(&gps->data);
 406
 407 if (_satdetails_on)
 408         gps_display_details(&gps->data);
 409
 410 return FALSE;
 411 }