driver_garmin_txt.c

   1 /*
   2  * Handle the Garmin simple text format supported by some Garmins.
   3  * Tested with the 'Garmin eTrex Legend' device working in 'Text Out' mode.
   4  *
   5  * Protocol info from:
   6  *       http://gpsd.berlios.de/vendor-docs/garmin/garmin_simpletext.txt
   7  *       http://www.garmin.com/support/commProtocol.html
   8  *
   9  * Code by: Petr Slansky <slansky@usa.net>
  10  * all rights abandoned, a thank would be nice if you use this code.
  11  *
  12  * -D 3 = packet trace
  13  * -D 4 = packet details
  14  * -D 5 = more packet details
  15  * -D 6 = very excessive details
  16  *
  17  * limitations:
  18  *  very simple protocol, only very basic information
  19  * TODO
  20  * do not have from garmin:
  21  *      pdop
  22  *      vdop
  23  *      magnetic variation
  24  *      satellite information
  25  *
  26  * This file is Copyright (c) 2010 by the GPSD project
  27  * BSD terms apply: see the file COPYING in the distribution root for details.
  28  *
  29  */
  30
  31 /***************************************************
  32 Garmin Simple Text Output Format:
  33
  34 The simple text (ASCII) output contains time, position, and velocity data in
  35 the fixed width fields (not delimited) defined in the following table:
  36
  37     FIELD DESCRIPTION:      WIDTH:  NOTES:
  38     ----------------------- ------- ------------------------
  39     Sentence start          1       Always '@'
  40     ----------------------- ------- ------------------------
  41    /Year                    2       Last two digits of UTC year
  42   | ----------------------- ------- ------------------------
  43   | Month                   2       UTC month, "01".."12"
  44 T | ----------------------- ------- ------------------------
  45 i | Day                     2       UTC day of month, "01".."31"
  46 m | ----------------------- ------- ------------------------
  47 e | Hour                    2       UTC hour, "00".."23"
  48   | ----------------------- ------- ------------------------
  49   | Minute                  2       UTC minute, "00".."59"
  50   | ----------------------- ------- ------------------------
  51    \Second                  2       UTC second, "00".."59"
  52     ----------------------- ------- ------------------------
  53    /Latitude hemisphere     1       'N' or 'S'
  54   | ----------------------- ------- ------------------------
  55   | Latitude position       7       WGS84 ddmmmmm, with an implied
  56   |                                 decimal after the 4th digit
  57   | ----------------------- ------- ------------------------
  58   | Longitude hemishpere    1       'E' or 'W'
  59   | ----------------------- ------- ------------------------
  60   | Longitude position      8       WGS84 dddmmmmm with an implied
  61 P |                                 decimal after the 5th digit
  62 o | ----------------------- ------- ------------------------
  63 s | Position status         1       'd' if current 2D differential GPS position
  64 i |                                 'D' if current 3D differential GPS position
  65 t |                                 'g' if current 2D GPS position
  66 i |                                 'G' if current 3D GPS position
  67 o |                                 'S' if simulated position
  68 n |                                 '_' if invalid position
  69   | ----------------------- ------- ------------------------
  70   | Horizontal posn error   3       EPH in meters
  71   | ----------------------- ------- ------------------------
  72   | Altitude sign           1       '+' or '-'
  73   | ----------------------- ------- ------------------------
  74   | Altitude                5       Height above or below mean
  75    \                                sea level in meters
  76     ----------------------- ------- ------------------------
  77    /East/West velocity      1       'E' or 'W'
  78   |     direction
  79   | ----------------------- ------- ------------------------
  80   | East/West velocity      4       Meters per second in tenths,
  81   |     magnitude                   ("1234" = 123.4 m/s)
  82 V | ----------------------- ------- ------------------------
  83 e | North/South velocity    1       'N' or 'S'
  84 l |     direction
  85 o | ----------------------- ------- ------------------------
  86 c | North/South velocity    4       Meters per second in tenths,
  87 i |     magnitude                   ("1234" = 123.4 m/s)
  88 t | ----------------------- ------- ------------------------
  89 y | Vertical velocity       1       'U' or 'D' (up/down)
  90   |     direction
  91   | ----------------------- ------- ------------------------
  92   | Vertical velocity       4       Meters per second in hundredths,
  93    \    magnitude                   ("1234" = 12.34 m/s)
  94     ----------------------- ------- ------------------------
  95     Sentence end            2       Carriage return, '0x0D', and
  96                                     line feed, '0x0A'
  97     ----------------------- ------- ------------------------
  98
  99 If a numeric value does not fill its entire field width, the field is padded
 100 with leading '0's (eg. an altitude of 50 meters above MSL will be output as
 101 "+00050").
 102
 103 Any or all of the data in the text sentence (except for the sentence start
 104 and sentence end fields) may be replaced with underscores to indicate
 105 invalid data.
 106
 107 ***************************************************/
 108
 109 #include <sys/types.h>
 110
 111 #include <stdio.h>
 112 #include <stdlib.h>
 113 #include <math.h>
 114
 115 #include <string.h>
 116 #ifndef S_SPLINT_S
 117 #include <unistd.h>
 118 #endif /* S_SPLINT_S */
 119 #include <errno.h>
 120 #include <inttypes.h>
 121
 122 #include "gpsd_config.h"
 123 #if defined (HAVE_SYS_SELECT_H)
 124 #include <sys/select.h>
 125 #endif
 126
 127 #if defined(HAVE_STRINGS_H)
 128 #include <strings.h>
 129 #endif
 130
 131 #include "gpsd.h"
 132 #include "gps.h"
 133 #include "timebase.h"
 134
 135 #ifdef GARMINTXT_ENABLE
 136
 137 /* Simple text message is fixed length, 55 chars text data + 2 characters EOL */
 138 /* buffer for text processing */
 139 #define TXT_BUFFER_SIZE 13
 140
 141 /**************************************************************************
 142  * decode text string to double number, translate prefix to sign
 143  * return 0: OK
 144  *       -1: data error
 145  *       -2: data not valid
 146  *
 147  * examples:
 148
 149  *  gar_decode(cbuf, 9, "EW", 100000.0, &result);
 150  *  E01412345 -> +14.12345
 151
 152  *  gar_decode(cbuf, 9, "EW", 100000.0, &result);
 153  *  W01412345 -> -14.12345
 154
 155  *  gar_decode(cbuf, 3, "", 10.0, &result);
 156  *  123 -> +12.3
 157
 158 **************************************************************************/
 159 static int gar_decode(const char *data, const size_t length, const char *prefix, const double dividor,  /*@out@*/
 160                       double *result)
 161 {
 162     char buf[10];
 163     float sign = 1.0;
 164     int preflen = (int)strlen(prefix);
 165     int offset = 1;             /* assume one character prefix (E,W,S,N,U,D, etc) */
 166     long int intresult;
 167
 168     /* splint is buggy here, thinks buf can be a null pointer */
 169     /*@ -mustdefine -nullderef -nullpass @*/
 170     if (length >= sizeof(buf)) {
 171         gpsd_report(LOG_ERROR, "internal buffer too small\n");
 172         return -1;
 173     }
 174
 175     bzero(buf, (int)sizeof(buf));
 176     (void)strncpy(buf, data, length);
 177     gpsd_report(LOG_RAW + 2, "Decoded string: %s\n", buf);
 178
 179     if (strchr(buf, '_') != NULL) {
 180         /* value is not valid, ignore it */
 181         return -2;
 182     }
 183
 184     /* parse prefix */
 185     do {
 186         if (preflen == 0) {
 187             offset = 0;         /* only number, no prefix */
 188             break;
 189         }
 190         /* second character in prefix is flag for negative number */
 191         if (preflen >= 2) {
 192             if (buf[0] == prefix[1]) {
 193                 sign = -1.0;
 194                 break;
 195             }
 196         }
 197         /* first character in prefix is flag for positive number */
 198         if (preflen >= 1) {
 199             if (buf[0] == prefix[0]) {
 200                 sign = 1.0;
 201                 break;
 202             }
 203         }
 204         gpsd_report(LOG_WARN, "Unexpected char \"%c\" in data \"%s\"\n",
 205                     buf[0], buf);
 206         return -1;
 207     } while (0);
 208
 209     if (strspn(buf + offset, "0123456789") != length - offset) {
 210         gpsd_report(LOG_WARN, "Invalid value %s\n", buf);
 211         return -1;
 212     }
 213     /*@ +mustdefine +nullderef +nullpass @*/
 214
 215     intresult = atol(buf + offset);
 216     if (intresult == 0L)
 217         sign = 0.0;             /*  don't create negative zero */
 218
 219     *result = (double)intresult / dividor * sign;
 220
 221     return 0;                   /* SUCCESS */
 222 }
 223
 224 /**************************************************************************
 225  * decode integer from string, check if the result is in expected range
 226  * return 0: OK
 227  *       -1: data error
 228  *       -2: data not valid
 229 **************************************************************************/
 230 static int gar_int_decode(const char *data, const size_t length,
 231                           const unsigned int min, const unsigned int max,
 232                           /*@out@*/ unsigned int *result)
 233 {
 234     char buf[6];
 235     unsigned int res;
 236
 237     /*@ -mustdefine @*/
 238     if (length >= sizeof(buf)) {
 239         gpsd_report(LOG_ERROR, "internal buffer too small\n");
 240         return -1;
 241     }
 242
 243     bzero(buf, (int)sizeof(buf));
 244     (void)strncpy(buf, data, length);
 245     gpsd_report(LOG_RAW + 2, "Decoded string: %s\n", buf);
 246
 247     if (strchr(buf, '_') != NULL) {
 248         /* value is not valid, ignore it */
 249         return -2;
 250     }
 251
 252     /*@ -nullpass @*//* splint bug */
 253     if (strspn(buf, "0123456789") != length) {
 254         gpsd_report(LOG_WARN, "Invalid value %s\n", buf);
 255         return -1;
 256     }
 257
 258     res = (unsigned)atoi(buf);
 259     if ((res >= min) && (res <= max)) {
 260         *result = res;
 261         return 0;               /* SUCCESS */
 262     } else {
 263         gpsd_report(LOG_WARN, "Value %u out of range <%u, %u>\n", res, min,
 264                     max);
 265         return -1;
 266     }
 267     /*@ +mustdefine +nullpass @*/
 268 }
 269
 270
 271 /**************************************************************************
 272  *
 273  * Entry points begin here
 274  *
 275  **************************************************************************/
 276
 277 gps_mask_t garmintxt_parse(struct gps_device_t * session)
 278 {
 279 /* parse GARMIN Simple Text sentence, unpack it into a session structure */
 280
 281     gps_mask_t mask = 0;
 282
 283     gpsd_report(LOG_PROG, "Garmin Simple Text packet, len %zd\n",
 284                 session->packet.outbuflen);
 285     gpsd_report(LOG_RAW, "%s\n",
 286                 gpsd_hexdump_wrapper(session->packet.outbuffer,
 287                                      session->packet.outbuflen, LOG_RAW));
 288
 289     if (session->packet.outbuflen < 54) {
 290         /* trailing CR and LF can be ignored; ('@' + 54x 'DATA' + '\r\n') has length 57 */
 291         gpsd_report(LOG_WARN, "Message is too short, rejected.\n");
 292         return ONLINE_IS;
 293     }
 294
 295     session->packet.type = GARMINTXT_PACKET;
 296     /* TAG message as GTXT, Garmin Simple Text Message */
 297     strncpy(session->gpsdata.tag, "GTXT", MAXTAGLEN);
 298
 299     /* only one message, set cycle start */
 300     session->cycle_end_reliable = true;
 301     do {
 302         unsigned int result;
 303         char *buf = (char *)session->packet.outbuffer + 1;
 304         gpsd_report(LOG_PROG, "Timestamp: %.12s\n", buf);
 305
 306         /* year */
 307         if (0 != gar_int_decode(buf + 0, 2, 0, 99, &result))
 308             break;
 309         session->driver.garmintxt.date.tm_year =
 310             (CENTURY_BASE + (int)result) - 1900;
 311         /* month */
 312         if (0 != gar_int_decode(buf + 2, 2, 1, 12, &result))
 313             break;
 314         session->driver.garmintxt.date.tm_mon = (int)result - 1;
 315         /* day */
 316         if (0 != gar_int_decode(buf + 4, 2, 1, 31, &result))
 317             break;
 318         session->driver.garmintxt.date.tm_mday = (int)result;
 319         /* hour */
 320         if (0 != gar_int_decode(buf + 6, 2, 0, 23, &result))
 321             break;
 322         session->driver.garmintxt.date.tm_hour = (int)result;   /* mday update?? */
 323         /* minute */
 324         if (0 != gar_int_decode(buf + 8, 2, 0, 59, &result))
 325             break;
 326         session->driver.garmintxt.date.tm_min = (int)result;
 327         /* second */
 328         /* second value can be even 60, occasional leap second */
 329         if (0 != gar_int_decode(buf + 10, 2, 0, 60, &result))
 330             break;
 331         session->driver.garmintxt.date.tm_sec = (int)result;
 332         session->driver.garmintxt.subseconds = 0;
 333         session->newdata.time =
 334             (double)mkgmtime(&session->driver.garmintxt.date) +
 335             session->driver.garmintxt.subseconds;
 336         mask |= TIME_IS;
 337     } while (0);
 338
 339     /* assume that possition is unknown; if the position is known we will fix status information later */
 340     session->newdata.mode = MODE_NO_FIX;
 341     session->gpsdata.status = STATUS_NO_FIX;
 342     mask |= MODE_IS | STATUS_IS | CLEAR_IS | REPORT_IS;
 343
 344     /* process position */
 345
 346     do {
 347         double lat, lon;
 348         unsigned int degfrag;
 349         char status;
 350
 351         /* Latitude, [NS]ddmmmmm */
 352         /* decode degrees of Latitude */
 353         if (0 !=
 354             gar_decode((char *)session->packet.outbuffer + 13, 3, "NS", 1.0,
 355                        &lat))
 356             break;
 357         /* decode minutes of Latitude */
 358         if (0 !=
 359             gar_int_decode((char *)session->packet.outbuffer + 16, 5, 0,
 360                            99999, &degfrag))
 361             break;
 362         lat += degfrag * 100.0 / 60.0 / 100000.0;
 363         session->newdata.latitude = lat;
 364
 365         /* Longitude, [EW]dddmmmmm */
 366         /* decode degrees of Longitude */
 367         if (0 !=
 368             gar_decode((char *)session->packet.outbuffer + 21, 4, "EW", 1.0,
 369                        &lon))
 370             break;
 371         /* decode minutes of Longitude */
 372         if (0 !=
 373             gar_int_decode((char *)session->packet.outbuffer + 25, 5, 0,
 374                            99999, &degfrag))
 375             break;
 376         lon += degfrag * 100.0 / 60.0 / 100000.0;
 377         session->newdata.longitude = lon;
 378
 379         /* fix mode, GPS status, [gGdDS_] */
 380         status = (char)session->packet.outbuffer[30];
 381
 382         switch (status) {
 383         case 'G':
 384         case 'S':               /* 'S' is DEMO mode, assume 3D position */
 385             session->newdata.mode = MODE_3D;
 386             session->gpsdata.status = STATUS_FIX;
 387             break;
 388         case 'D':
 389             session->newdata.mode = MODE_3D;
 390             session->gpsdata.status = STATUS_DGPS_FIX;
 391             break;
 392         case 'g':
 393             session->newdata.mode = MODE_2D;
 394             session->gpsdata.status = STATUS_FIX;
 395             break;
 396         case 'd':
 397             session->newdata.mode = MODE_2D;
 398             session->gpsdata.status = STATUS_DGPS_FIX;
 399             break;
 400         default:
 401             session->newdata.mode = MODE_NO_FIX;
 402             session->gpsdata.status = STATUS_NO_FIX;
 403         }
 404         mask |= MODE_IS | STATUS_IS | LATLON_IS;
 405     } while (0);
 406
 407     /* EPH */
 408     do {
 409         double eph;
 410         if (0 !=
 411             gar_decode((char *)session->packet.outbuffer + 31, 3, "", 1.0,
 412                        &eph))
 413             break;
 414         /* eph is a circular error, sqrt(epx**2 + epy**2) */
 415         session->newdata.epx = session->newdata.epy =
 416             eph * (1 / sqrt(2)) * (GPSD_CONFIDENCE / CEP50_SIGMA);
 417         mask |= HERR_IS;
 418     } while (0);
 419
 420     /* Altitude */
 421     do {
 422         double alt;
 423         if (0 !=
 424             gar_decode((char *)session->packet.outbuffer + 34, 6, "+-", 1.0,
 425                        &alt))
 426             break;
 427         session->newdata.altitude = alt;
 428         mask |= ALTITUDE_IS;
 429     } while (0);
 430
 431     /* Velocity */
 432     do {
 433         double ewvel, nsvel, speed, track;
 434         if (0 !=
 435             gar_decode((char *)session->packet.outbuffer + 40, 5, "EW", 10.0,
 436                        &ewvel))
 437             break;
 438         if (0 !=
 439             gar_decode((char *)session->packet.outbuffer + 45, 5, "NS", 10.0,
 440                        &nsvel))
 441             break;
 442         speed = sqrt(ewvel * ewvel + nsvel * nsvel);    /* is this correct formula? Result is in mps */
 443         session->newdata.speed = speed;
 444         track = atan2(ewvel, nsvel) * RAD_2_DEG;        /* is this correct formula? Result is in degrees */
 445         if (track < 0.0)
 446             track += 360.0;
 447         session->newdata.track = track;
 448         mask |= SPEED_IS | TRACK_IS;
 449     } while (0);
 450
 451
 452     /* Climb (vertical velocity) */
 453     do {
 454         double climb;
 455         if (0 !=
 456             gar_decode((char *)session->packet.outbuffer + 50, 5, "UD", 100.0,
 457                        &climb))
 458             break;
 459         session->newdata.climb = climb; /* climb in mps */
 460         mask |= CLIMB_IS;
 461     } while (0);
 462
 463     gpsd_report(LOG_DATA,
 464                 "GTXT: time=%.2f, lat=%.2f lon=%.2f alt=%.2f speed=%.2f track=%.2f climb=%.2f exp=%.2f epy=%.2f mode=%d status=%d mask=%s\n",
 465                 session->newdata.time, session->newdata.latitude,
 466                 session->newdata.longitude, session->newdata.altitude,
 467                 session->newdata.speed, session->newdata.track,
 468                 session->newdata.climb, session->newdata.epx,
 469                 session->newdata.epy, session->newdata.mode,
 470                 session->gpsdata.status, gpsd_maskdump(mask));
 471     return mask;
 472 }
 473
 474 #endif /* GARMINTXT_ENABLE */