driver_italk.c

   1 /*
   2  * This file is Copyright (c) 2010 by the GPSD project
   3  * BSD terms apply: see the file COPYING in the distribution root for details.
   4  *
   5  * Driver for the iTalk binary protocol used by FasTrax
   6  */
   7 #include <sys/types.h>
   8 #include <stdio.h>
   9 #include <stdlib.h>
  10 #include <string.h>
  11 #include <math.h>
  12 #include <ctype.h>
  13 #ifndef S_SPLINT_S
  14 #include <unistd.h>
  15 #endif /* S_SPLINT_S */
  16 #include <time.h>
  17 #include <stdio.h>
  18
  19 #include "gpsd.h"
  20 #if defined(ITRAX_ENABLE) && defined(BINARY_ENABLE)
  21
  22 #include "bits.h"
  23 #include "driver_italk.h"
  24
  25 static gps_mask_t italk_parse(struct gps_device_t *, unsigned char *, size_t);
  26 static gps_mask_t decode_itk_navfix(struct gps_device_t *, unsigned char *,
  27                                     size_t);
  28 static gps_mask_t decode_itk_prnstatus(struct gps_device_t *, unsigned char *,
  29                                        size_t);
  30 static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *,
  31                                           unsigned char *, size_t);
  32 static gps_mask_t decode_itk_subframe(struct gps_device_t *, unsigned char *,
  33                                       size_t);
  34
  35 static gps_mask_t decode_itk_navfix(struct gps_device_t *session,
  36                                     unsigned char *buf, size_t len)
  37 {
  38     unsigned int tow;
  39     unsigned short gps_week, flags, cflags, pflags;
  40     gps_mask_t mask = 0;
  41     double epx, epy, epz, evx, evy, evz, eph;
  42     double t;
  43
  44     if (len != 296) {
  45         gpsd_report(LOG_PROG, "ITALK: bad NAV_FIX (len %zu, should be 296)\n",
  46                     len);
  47         return -1;
  48     }
  49
  50     flags = (ushort) getleuw(buf, 7 + 4);
  51     cflags = (ushort) getleuw(buf, 7 + 6);
  52     pflags = (ushort) getleuw(buf, 7 + 8);
  53
  54     session->gpsdata.status = STATUS_NO_FIX;
  55     session->newdata.mode = MODE_NO_FIX;
  56     mask = ONLINE_IS | MODE_IS | STATUS_IS | CLEAR_IS;
  57
  58     /* just bail out if this fix is not marked valid */
  59     if (0 != (pflags & FIX_FLAG_MASK_INVALID)
  60         || 0 == (flags & FIXINFO_FLAG_VALID))
  61         return mask;
  62
  63     gps_week = (ushort) getlesw(buf, 7 + 82);
  64     session->context->gps_week = gps_week;
  65     tow = (uint) getleul(buf, 7 + 84);
  66     session->context->gps_tow = tow / 1000.0;
  67     t = gpstime_to_unix((int)gps_week, session->context->gps_tow)
  68         - session->context->leap_seconds;
  69     session->newdata.time = t;
  70     mask |= TIME_IS;
  71
  72     epx = (double)(getlesl(buf, 7 + 96) / 100.0);
  73     epy = (double)(getlesl(buf, 7 + 100) / 100.0);
  74     epz = (double)(getlesl(buf, 7 + 104) / 100.0);
  75     evx = (double)(getlesl(buf, 7 + 186) / 1000.0);
  76     evy = (double)(getlesl(buf, 7 + 190) / 1000.0);
  77     evz = (double)(getlesl(buf, 7 + 194) / 1000.0);
  78     ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation,
  79                      epx, epy, epz, evx, evy, evz);
  80     mask |= LATLON_IS | ALTITUDE_IS | SPEED_IS | TRACK_IS | CLIMB_IS;
  81     eph = (double)(getlesl(buf, 7 + 252) / 100.0);
  82     /* eph is a circular error, sqrt(epx**2 + epy**2) */
  83     session->newdata.epx = session->newdata.epy = eph / sqrt(2);
  84     session->newdata.eps = (double)(getlesl(buf, 7 + 254) / 100.0);
  85
  86 #define MAX(a,b) (((a) > (b)) ? (a) : (b))
  87     session->gpsdata.satellites_used =
  88         (int)MAX(getleuw(buf, 7 + 12), getleuw(buf, 7 + 14));
  89     mask |= USED_IS;
  90
  91     if (flags & FIX_CONV_DOP_VALID) {
  92         session->gpsdata.dop.hdop = (double)(getleuw(buf, 7 + 56) / 100.0);
  93         session->gpsdata.dop.gdop = (double)(getleuw(buf, 7 + 58) / 100.0);
  94         session->gpsdata.dop.pdop = (double)(getleuw(buf, 7 + 60) / 100.0);
  95         session->gpsdata.dop.vdop = (double)(getleuw(buf, 7 + 62) / 100.0);
  96         session->gpsdata.dop.tdop = (double)(getleuw(buf, 7 + 64) / 100.0);
  97         mask |= DOP_IS;
  98     }
  99
 100     if ((pflags & FIX_FLAG_MASK_INVALID) == 0
 101         && (flags & FIXINFO_FLAG_VALID) != 0) {
 102         if (pflags & FIX_FLAG_3DFIX)
 103             session->newdata.mode = MODE_3D;
 104         else
 105             session->newdata.mode = MODE_2D;
 106
 107         if (pflags & FIX_FLAG_DGPS_CORRECTION)
 108             session->gpsdata.status = STATUS_DGPS_FIX;
 109         else
 110             session->gpsdata.status = STATUS_FIX;
 111     }
 112
 113     gpsd_report(LOG_DATA,
 114                 "NAV_FIX: time=%.2f, lat=%.2f lon=%.2f alt=%.f speed=%.2f track=%.2f climb=%.2f mode=%d status=%d gdop=%.2f pdop=%.2f hdop=%.2f vdop=%.2f tdop=%.2f mask=%s\n",
 115                 session->newdata.time, session->newdata.latitude,
 116                 session->newdata.longitude, session->newdata.altitude,
 117                 session->newdata.speed, session->newdata.track,
 118                 session->newdata.climb, session->newdata.mode,
 119                 session->gpsdata.status, session->gpsdata.dop.gdop,
 120                 session->gpsdata.dop.pdop, session->gpsdata.dop.hdop,
 121                 session->gpsdata.dop.vdop, session->gpsdata.dop.tdop,
 122                 gpsd_maskdump(mask));
 123     return mask;
 124 }
 125
 126 static gps_mask_t decode_itk_prnstatus(struct gps_device_t *session,
 127                                        unsigned char *buf, size_t len)
 128 {
 129     unsigned int i, tow, nsv, nchan, st;
 130     unsigned short gps_week;
 131     double t;
 132     gps_mask_t mask;
 133
 134     if (len < 62) {
 135         gpsd_report(LOG_PROG, "ITALK: runt PRN_STATUS (len=%zu)\n", len);
 136         mask = 0;
 137     } else {
 138         gps_week = (ushort) getleuw(buf, 7 + 4);
 139         session->context->gps_week = gps_week;
 140         tow = (uint) getleul(buf, 7 + 6);
 141         session->context->gps_tow = tow / 1000.0;
 142         t = gpstime_to_unix((int)gps_week, session->context->gps_tow)
 143             - session->context->leap_seconds;
 144         session->gpsdata.skyview_time = t;
 145
 146         gpsd_zero_satellites(&session->gpsdata);
 147         nsv = 0;
 148         nchan = (unsigned int)getleuw(buf, 7 + 50);
 149         if (nchan > MAX_NR_VISIBLE_PRNS)
 150             nchan = MAX_NR_VISIBLE_PRNS;
 151         for (i = st = 0; i < nchan; i++) {
 152             unsigned int off = 7 + 52 + 10 * i;
 153             unsigned short flags;
 154
 155             flags = (ushort) getleuw(buf, off);
 156             session->gpsdata.ss[i] = (float)(getleuw(buf, off + 2) & 0xff);
 157             session->gpsdata.PRN[i] = (int)getleuw(buf, off + 4) & 0xff;
 158             session->gpsdata.elevation[i] = (int)getlesw(buf, off + 6) & 0xff;
 159             session->gpsdata.azimuth[i] = (int)getlesw(buf, off + 8) & 0xff;
 160             if (session->gpsdata.PRN[i]) {
 161                 st++;
 162                 if (flags & PRN_FLAG_USE_IN_NAV)
 163                     session->gpsdata.used[nsv++] = session->gpsdata.PRN[i];
 164             }
 165         }
 166         session->gpsdata.satellites_visible = (int)st;
 167         session->gpsdata.satellites_used = (int)nsv;
 168         mask = USED_IS | SATELLITE_IS;;
 169
 170         gpsd_report(LOG_DATA,
 171                     "PRN_STATUS: time=%.2f visible=%d used=%d mask={USED|SATELLITE}\n",
 172                     session->newdata.time,
 173                     session->gpsdata.satellites_visible,
 174                     session->gpsdata.satellites_used);
 175     }
 176
 177     return mask;
 178 }
 179
 180 static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *session,
 181                                           unsigned char *buf, size_t len)
 182 {
 183     unsigned int tow;
 184     int leap;
 185     unsigned short gps_week, flags;
 186     double t;
 187
 188     if (len != 64) {
 189         gpsd_report(LOG_PROG,
 190                     "ITALK: bad UTC_IONO_MODEL (len %zu, should be 64)\n",
 191                     len);
 192         return 0;
 193     }
 194
 195     flags = (ushort) getleuw(buf, 7);
 196     if (0 == (flags & UTC_IONO_MODEL_UTCVALID))
 197         return 0;
 198
 199     leap = (int)getleuw(buf, 7 + 24);
 200     if (session->context->leap_seconds < leap)
 201         session->context->leap_seconds = leap;
 202
 203     gps_week = (ushort) getleuw(buf, 7 + 36);
 204     session->context->gps_week = gps_week;
 205     tow = (uint) getleul(buf, 7 + 38);
 206     session->context->gps_tow = tow / 1000.0;
 207     t = gpstime_to_unix((int)gps_week, session->context->gps_tow)
 208         - session->context->leap_seconds;
 209     session->newdata.time = t;
 210
 211     gpsd_report(LOG_DATA,
 212                 "UTC_IONO_MODEL: time=%.2f mask={TIME}\n",
 213                 session->newdata.time);
 214     return TIME_IS;
 215 }
 216
 217 static gps_mask_t decode_itk_subframe(struct gps_device_t *session,
 218                                       unsigned char *buf, size_t len)
 219 {
 220     unsigned short flags, prn, sf;
 221     unsigned int i, words[10];
 222
 223     if (len != 64) {
 224         gpsd_report(LOG_PROG,
 225                     "ITALK: bad SUBFRAME (len %zu, should be 64)\n", len);
 226         return 0;
 227     }
 228
 229     flags = (ushort) getleuw(buf, 7 + 4);
 230     prn = (ushort) getleuw(buf, 7 + 6);
 231     sf = (ushort) getleuw(buf, 7 + 8);
 232     gpsd_report(LOG_PROG, "iTalk 50B SUBFRAME prn %u sf %u - decode %s %s\n",
 233                 prn, sf,
 234                 flags & SUBFRAME_WORD_FLAG_MASK ? "error" : "ok",
 235                 flags & SUBFRAME_GPS_PREAMBLE_INVERTED ? "(inverted)" : "");
 236     if (flags & SUBFRAME_WORD_FLAG_MASK)
 237         return 0;       // don't try decode an erroneous packet
 238
 239     /*
 240      * Timo says "SUBRAME message contains decoded navigation message subframe
 241      * words with parity checking done but parity bits still present."
 242      */
 243     for (i = 0; i < 10; i++)
 244         words[i] =
 245             (unsigned int)(getleul(buf, 7 + 14 + 4 * i) >> 6) & 0xffffff;
 246
 247     gpsd_interpret_subframe(session, words);
 248     return ONLINE_IS;
 249 }
 250
 251 static gps_mask_t decode_itk_pseudo(struct gps_device_t *session,
 252                                       unsigned char *buf, size_t len)
 253 {
 254     unsigned short gps_week, flags, n, i;
 255     unsigned int tow;
 256     union long_double l_d;
 257     double t;
 258
 259     n = (ushort) getleuw(buf, 7 + 4);
 260     if ((n < 1) || (n > MAXCHANNELS)){
 261         gpsd_report(LOG_INF, "ITALK: bad PSEUDO channel count\n");
 262         return 0;
 263     }
 264
 265     if (len != (size_t)((n+1)*36)) {
 266         gpsd_report(LOG_PROG,
 267                     "ITALK: bad PSEUDO len %zu\n", len);
 268     }
 269
 270     gpsd_report(LOG_PROG, "iTalk PSEUDO [%u]\n", n);
 271     flags = (unsigned short)getleuw(buf, 7 + 6);
 272     if ((flags & 0x3) != 0x3)
 273         return 0; // bail if measurement time not valid.
 274
 275     gps_week = (ushort) getleuw(buf, 7 + 8);
 276     tow = (uint) getleul(buf, 7 + 38);
 277     session->context->gps_week = gps_week;
 278     session->context->gps_tow = tow / 1000.0;
 279     t = gpstime_to_unix((int)gps_week, session->context->gps_tow)
 280         - session->context->leap_seconds;
 281
 282     /*@-type@*/
 283     for (i = 0; i < n; i++){
 284         session->gpsdata.PRN[i] = getleuw(buf, 7 + 26 + (i*36)) & 0xff;
 285         session->gpsdata.ss[i] = getleuw(buf, 7 + 26 + (i*36 + 2)) & 0x3f;
 286         session->gpsdata.raw.satstat[i] = getleul(buf, 7 + 26 + (i*36 + 4));
 287         session->gpsdata.raw.pseudorange[i] = getled(buf, 7 + 26 + (i*36 + 8));
 288         session->gpsdata.raw.doppler[i] = getled(buf, 7 + 26 + (i*36 + 16));
 289         session->gpsdata.raw.carrierphase[i] = getleuw(buf, 7 + 26 + (i*36 + 28));
 290
 291         session->gpsdata.raw.mtime[i] = t;
 292         session->gpsdata.raw.codephase[i] = NAN;
 293         session->gpsdata.raw.deltarange[i] = NAN;
 294     }
 295     /*@+type@*/
 296     return RAW_IS;
 297 }
 298
 299 /*@ +charint @*/
 300 static gps_mask_t italk_parse(struct gps_device_t *session,
 301                               unsigned char *buf, size_t len)
 302 {
 303     unsigned int type;
 304     gps_mask_t mask = 0;
 305
 306     if (len == 0)
 307         return 0;
 308
 309     type = (uint) getub(buf, 4);
 310     /* we may need to dump the raw packet */
 311     gpsd_report(LOG_RAW, "raw italk packet type 0x%02x length %zu: %s\n",
 312                 type, len, gpsd_hexdump_wrapper(buf, len, LOG_RAW));
 313
 314     session->cycle_end_reliable = true;
 315
 316     switch (type) {
 317     case ITALK_NAV_FIX:
 318         gpsd_report(LOG_IO, "iTalk NAV_FIX len %zu\n", len);
 319         mask = decode_itk_navfix(session, buf, len) | (CLEAR_IS | REPORT_IS);
 320         break;
 321     case ITALK_PRN_STATUS:
 322         gpsd_report(LOG_IO, "iTalk PRN_STATUS len %zu\n", len);
 323         mask = decode_itk_prnstatus(session, buf, len);
 324         break;
 325     case ITALK_UTC_IONO_MODEL:
 326         gpsd_report(LOG_IO, "iTalk UTC_IONO_MODEL len %zu\n", len);
 327         mask = decode_itk_utcionomodel(session, buf, len);
 328         break;
 329
 330     case ITALK_ACQ_DATA:
 331         gpsd_report(LOG_IO, "iTalk ACQ_DATA len %zu\n", len);
 332         break;
 333     case ITALK_TRACK:
 334         gpsd_report(LOG_IO, "iTalk TRACK len %zu\n", len);
 335         break;
 336     case ITALK_PSEUDO:
 337         gpsd_report(LOG_IO, "iTalk PSEUDO len %zu\n", len);
 338         mask = decode_itk_pseudo(session, buf, len);
 339         break;
 340     case ITALK_RAW_ALMANAC:
 341         gpsd_report(LOG_IO, "iTalk RAW_ALMANAC len %zu\n", len);
 342         break;
 343     case ITALK_RAW_EPHEMERIS:
 344         gpsd_report(LOG_IO, "iTalk RAW_EPHEMERIS len %zu\n", len);
 345         break;
 346     case ITALK_SUBFRAME:
 347         mask = decode_itk_subframe(session, buf, len);
 348         break;
 349     case ITALK_BIT_STREAM:
 350         gpsd_report(LOG_IO, "iTalk BIT_STREAM len %zu\n", len);
 351         break;
 352
 353     case ITALK_AGC:
 354     case ITALK_SV_HEALTH:
 355     case ITALK_PRN_PRED:
 356     case ITALK_FREQ_PRED:
 357     case ITALK_DBGTRACE:
 358     case ITALK_START:
 359     case ITALK_STOP:
 360     case ITALK_SLEEP:
 361     case ITALK_STATUS:
 362     case ITALK_ITALK_CONF:
 363     case ITALK_SYSINFO:
 364     case ITALK_ITALK_TASK_ROUTE:
 365     case ITALK_PARAM_CTRL:
 366     case ITALK_PARAMS_CHANGED:
 367     case ITALK_START_COMPLETED:
 368     case ITALK_STOP_COMPLETED:
 369     case ITALK_LOG_CMD:
 370     case ITALK_SYSTEM_START:
 371     case ITALK_STOP_SEARCH:
 372     case ITALK_SEARCH:
 373     case ITALK_PRED_SEARCH:
 374     case ITALK_SEARCH_DONE:
 375     case ITALK_TRACK_DROP:
 376     case ITALK_TRACK_STATUS:
 377     case ITALK_HANDOVER_DATA:
 378     case ITALK_CORE_SYNC:
 379     case ITALK_WAAS_RAWDATA:
 380     case ITALK_ASSISTANCE:
 381     case ITALK_PULL_FIX:
 382     case ITALK_MEMCTRL:
 383     case ITALK_STOP_TASK:
 384         gpsd_report(LOG_IO,
 385                     "iTalk not processing packet: id 0x%02x length %zu\n",
 386                     type, len);
 387         break;
 388     default:
 389         gpsd_report(LOG_IO, "iTalk unknown packet: id 0x%02x length %zu\n",
 390                     type, len);
 391     }
 392     (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag),
 393                        "ITK-%02x", type);
 394
 395     return mask | ONLINE_IS;
 396 }
 397
 398 /*@ -charint @*/
 399
 400 static gps_mask_t italk_parse_input(struct gps_device_t *session)
 401 {
 402     gps_mask_t st;
 403
 404     if (session->packet.type == ITALK_PACKET) {
 405         st = italk_parse(session, session->packet.outbuffer,
 406                          session->packet.outbuflen);
 407         session->gpsdata.dev.driver_mode = MODE_BINARY; /* binary */
 408         return st;
 409 #ifdef NMEA_ENABLE
 410     } else if (session->packet.type == NMEA_PACKET) {
 411         st = nmea_parse((char *)session->packet.outbuffer, session);
 412         session->gpsdata.dev.driver_mode = MODE_NMEA;   /* NMEA */
 413         return st;
 414 #endif /* NMEA_ENABLE */
 415     } else
 416         return 0;
 417 }
 418
 419 #ifdef ALLOW_CONTROLSEND
 420 /*@ +charint -usedef -compdef @*/
 421 static ssize_t italk_control_send(struct gps_device_t *session,
 422                                   char *msg, size_t msglen)
 423 {
 424     ssize_t status;
 425
 426     /*@ -mayaliasunique @*/
 427     session->msgbuflen = msglen;
 428     (void)memcpy(session->msgbuf, msg, msglen);
 429     /*@ +mayaliasunique @*/
 430     /* we may need to dump the message */
 431     gpsd_report(LOG_IO, "writing italk control type %02x:%s\n",
 432                 msg[0], gpsd_hexdump_wrapper(msg, msglen, LOG_IO));
 433     status = write(session->gpsdata.gps_fd, msg, msglen);
 434     (void)tcdrain(session->gpsdata.gps_fd);
 435     return status;
 436 }
 437
 438 /*@ -charint +usedef +compdef @*/
 439 #endif /* ALLOW_CONTROLSEND */
 440
 441 static bool italk_set_mode(struct gps_device_t *session UNUSED,
 442                            speed_t speed UNUSED,
 443                            char parity UNUSED, int stopbits UNUSED,
 444                            bool mode UNUSED)
 445 {
 446 #ifdef __NOT_YET__
 447     /*@ +charint @*/
 448     char msg[] = { 0, };
 449
 450     /* HACK THE MESSAGE */
 451
 452     return (italk_control_send(session, msg, sizeof(msg)) != -1);
 453     /*@ +charint @*/
 454 #endif
 455
 456     return false;               /* until this actually works */
 457 }
 458
 459 #ifdef ALLOW_RECONFIGURE
 460 static bool italk_speed(struct gps_device_t *session,
 461                         speed_t speed, char parity, int stopbits)
 462 {
 463     return italk_set_mode(session, speed, parity, stopbits, true);
 464 }
 465
 466 static void italk_mode(struct gps_device_t *session, int mode)
 467 {
 468     if (mode == MODE_NMEA) {
 469         (void)italk_set_mode(session,
 470                              session->gpsdata.dev.baudrate,
 471                              (char)session->gpsdata.dev.parity,
 472                              (int)session->gpsdata.dev.stopbits, false);
 473     }
 474 }
 475 #endif /* ALLOW_RECONFIGURE */
 476
 477 static void italk_event_hook(struct gps_device_t *session, event_t event)
 478 {
 479     /*
 480      * FIX-ME: It might not be necessary to call this on reactivate.
 481      * Experiment to see if the holds its settings through a close.
 482      */
 483     if ((event == event_identified || event == event_reactivate)
 484         && session->packet.type == NMEA_PACKET)
 485         (void)italk_set_mode(session, session->gpsdata.dev.baudrate,
 486                              (char)session->gpsdata.dev.parity,
 487                              (int)session->gpsdata.dev.stopbits, true);
 488 }
 489
 490 #ifdef __not_yet__
 491 static void italk_ping(struct gps_device_t *session)
 492 /* send a "ping". it may help us detect an itrax more quickly */
 493 {
 494     char *ping = "<?>";
 495     (void)gpsd_write(session, ping, 3);
 496 }
 497 #endif /* __not_yet__ */
 498
 499 /* *INDENT-OFF* */
 500 const struct gps_type_t italk_binary =
 501 {
 502     .type_name      = "iTalk binary",   /* full name of type */
 503     .packet_type    = ITALK_PACKET,     /* associated lexer packet type */
 504     .trigger        = NULL,             /* recognize the type */
 505     .channels       = 12,               /* consumer-grade GPS */
 506     .probe_detect   = NULL,             /* how to detect at startup time */
 507     .get_packet     = generic_get,      /* use generic packet grabber */
 508     .parse_packet   = italk_parse_input,/* parse message packets */
 509     .rtcm_writer    = pass_rtcm,        /* send RTCM data straight */
 510     .event_hook     = italk_event_hook, /* lifetime event handler */
 511 #ifdef ALLOW_RECONFIGURE
 512     .speed_switcher = italk_speed,      /* we can change baud rates */
 513     .mode_switcher  = italk_mode,       /* there is a mode switcher */
 514     .rate_switcher  = NULL,             /* no sample-rate switcher */
 515     .min_cycle      = 1,                /* not relevant, no rate switch */
 516 #endif /* ALLOW_RECONFIGURE */
 517 #ifdef ALLOW_CONTROLSEND
 518     .control_send   = italk_control_send,       /* how to send a control string */
 519 #endif /* ALLOW_CONTROLSEND */
 520 #ifdef NTPSHM_ENABLE
 521     .ntp_offset     = NULL,             /* no method for NTP fudge factor */
 522 #endif /* NTPSHM_ ENABLE */
 523 };
 524 /* *INDENT-ON* */
 525 #endif /* defined(ITRAX_ENABLE) && defined(BINARY_ENABLE) */