driver_zodiac.c

   1 /*
   2  * Handle the Rockwell binary packet format supported by the old Zodiac chipset
   3  *
   4  * This file is Copyright (c) 2010 by the GPSD project
   5  * BSD terms apply: see the file COPYING in the distribution root for details.
   6  */
   7 #include <sys/types.h>
   8 #include <stdio.h>
   9 #include <stdlib.h>
  10 #include <string.h>
  11 #ifndef S_SPLINT_S
  12 #include <unistd.h>
  13 #endif /* S_SPLINT_S */
  14 #include <math.h>
  15 #include "gpsd.h"
  16
  17 #include "bits.h"
  18
  19 #ifdef ZODIAC_ENABLE
  20 struct header
  21 {
  22     unsigned short sync;
  23     unsigned short id;
  24     unsigned short ndata;
  25     unsigned short flags;
  26     unsigned short csum;
  27 };
  28
  29 static unsigned short zodiac_checksum(unsigned short *w, int n)
  30 {
  31     unsigned short csum = 0;
  32
  33     while (n-- > 0)
  34         csum += *(w++);
  35     return -csum;
  36 }
  37
  38 /* zodiac_spew - Takes a message type, an array of data words, and a length
  39    for the array, and prepends a 5 word header (including checksum).
  40    The data words are expected to be checksummed */
  41 #if defined (WORDS_BIGENDIAN)
  42 /* data is assumed to contain len/2 unsigned short words
  43  * we change the endianness to little, when needed.
  44  */
  45 static int end_write(int fd, void *d, int len)
  46 {
  47     char buf[BUFSIZ];
  48     char *p = buf;
  49     char *data = (char *)d;
  50     size_t n = (size_t) len;
  51
  52     while (n > 0) {
  53         *p++ = *(data + 1);
  54         *p++ = *data;
  55         data += 2;
  56         n -= 2;
  57     }
  58     return write(fd, buf, len);
  59 }
  60 #else
  61 #define end_write write
  62 #endif /* WORDS_BIGENDIAN */
  63
  64 static ssize_t zodiac_spew(struct gps_device_t *session, unsigned short type,
  65                            unsigned short *dat, int dlen)
  66 {
  67     struct header h;
  68     int i;
  69     char buf[BUFSIZ];
  70
  71     h.sync = 0x81ff;
  72     h.id = (unsigned short)type;
  73     h.ndata = (unsigned short)(dlen - 1);
  74     h.flags = 0;
  75     h.csum = zodiac_checksum((unsigned short *)&h, 4);
  76
  77     if (session->gpsdata.gps_fd != -1) {
  78         size_t hlen, datlen;
  79         hlen = sizeof(h);
  80         datlen = sizeof(unsigned short) * dlen;
  81         if (end_write(session->gpsdata.gps_fd, &h, hlen) != (ssize_t) hlen ||
  82             end_write(session->gpsdata.gps_fd, dat,
  83                       datlen) != (ssize_t) datlen) {
  84             gpsd_report(LOG_RAW, "Reconfigure write failed\n");
  85             return -1;
  86         }
  87     }
  88
  89     (void)snprintf(buf, sizeof(buf),
  90                    "%04x %04x %04x %04x %04x",
  91                    h.sync, h.id, h.ndata, h.flags, h.csum);
  92     for (i = 0; i < dlen; i++)
  93         (void)snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf),
  94                        " %04x", dat[i]);
  95
  96     gpsd_report(LOG_RAW, "Sent Zodiac packet: %s\n", buf);
  97
  98     return 0;
  99 }
 100
 101 static void send_rtcm(struct gps_device_t *session,
 102                       char *rtcmbuf, size_t rtcmbytes)
 103 {
 104     unsigned short data[34];
 105     int n = 1 + (int)(rtcmbytes / 2 + rtcmbytes % 2);
 106
 107     if (session->driver.zodiac.sn++ > 32767)
 108         session->driver.zodiac.sn = 0;
 109
 110     memset(data, 0, sizeof(data));
 111     data[0] = session->driver.zodiac.sn;        /* sequence number */
 112     memcpy(&data[1], rtcmbuf, rtcmbytes);
 113     data[n] = zodiac_checksum(data, n);
 114
 115     (void)zodiac_spew(session, 1351, data, n + 1);
 116 }
 117
 118 static ssize_t zodiac_send_rtcm(struct gps_device_t *session,
 119                                 char *rtcmbuf, size_t rtcmbytes)
 120 {
 121     size_t len;
 122
 123     while (rtcmbytes > 0) {
 124         len = (size_t) (rtcmbytes > 64 ? 64 : rtcmbytes);
 125         send_rtcm(session, rtcmbuf, len);
 126         rtcmbytes -= len;
 127         rtcmbuf += len;
 128     }
 129     return 1;
 130 }
 131
 132 #define getzword(n)     getwordz(session->packet.outbuffer, n)
 133 #define getzlong(n)     getlongz(session->packet.outbuffer, n)
 134
 135 static gps_mask_t handle1000(struct gps_device_t *session)
 136 /* time-position-velocity report */
 137 {
 138     gps_mask_t mask;
 139     double subseconds;
 140     struct tm unpacked_date;
 141     /* ticks                      = getzlong(6); */
 142     /* sequence                   = getzword(8); */
 143     /* measurement_sequence       = getzword(9); */
 144     /*@ -boolops -predboolothers @*/
 145     session->gpsdata.status = (getzword(10) & 0x1c) ? 0 : 1;
 146     if (session->gpsdata.status != 0)
 147         session->newdata.mode = (getzword(10) & 1) ? MODE_2D : MODE_3D;
 148     else
 149         session->newdata.mode = MODE_NO_FIX;
 150     /*@ +boolops -predboolothers @*/
 151
 152     /* solution_type                 = getzword(11); */
 153     session->gpsdata.satellites_used = (int)getzword(12);
 154     /* polar_navigation              = getzword(13); */
 155     session->context->gps_week = (unsigned short)getzword(14);
 156     /* gps_seconds                   = getzlong(15); */
 157     /* gps_nanoseconds               = getzlong(17); */
 158     unpacked_date.tm_mday = (int)getzword(19);
 159     unpacked_date.tm_mon = (int)getzword(20) - 1;
 160     unpacked_date.tm_year = (int)getzword(21) - 1900;
 161     unpacked_date.tm_hour = (int)getzword(22);
 162     unpacked_date.tm_min = (int)getzword(23);
 163     unpacked_date.tm_sec = (int)getzword(24);
 164     subseconds = (int)getzlong(25) / 1e9;
 165     /*@ -compdef */
 166     session->newdata.time = (double)mkgmtime(&unpacked_date) + subseconds;
 167     /*@ +compdef */
 168     /*@ -type @*/
 169     session->newdata.latitude = ((long)getzlong(27)) * RAD_2_DEG * 1e-8;
 170     session->newdata.longitude = ((long)getzlong(29)) * RAD_2_DEG * 1e-8;
 171     /*
 172      * The Rockwell Jupiter TU30-D140 reports altitude as uncorrected height
 173      * above WGS84 geoid.  The Zodiac binary protocol manual does not
 174      * specify whether word 31 is geodetic or WGS 84.
 175      */
 176     session->newdata.altitude = ((long)getzlong(31)) * 1e-2;
 177     /*@ +type @*/
 178     session->gpsdata.separation = ((short)getzword(33)) * 1e-2;
 179     session->newdata.altitude -= session->gpsdata.separation;
 180     session->newdata.speed = (int)getzlong(34) * 1e-2;
 181     session->newdata.track = (int)getzword(36) * RAD_2_DEG * 1e-3;
 182     session->mag_var = ((short)getzword(37)) * RAD_2_DEG * 1e-4;
 183     session->newdata.climb = ((short)getzword(38)) * 1e-2;
 184     /* map_datum                   = getzword(39); */
 185     /*
 186      * The manual says these are 1-sigma.  Device reports only eph, circular
 187      * error; no harm in assigning it to both x and y components.
 188      */
 189     session->newdata.epx = session->newdata.epy =
 190         (int)getzlong(40) * 1e-2 * (1 / sqrt(2)) * GPSD_CONFIDENCE;
 191     session->newdata.epv = (int)getzlong(42) * 1e-2 * GPSD_CONFIDENCE;
 192     session->newdata.ept = (int)getzlong(44) * 1e-2 * GPSD_CONFIDENCE;
 193     session->newdata.eps = (int)getzword(46) * 1e-2 * GPSD_CONFIDENCE;
 194     /* clock_bias                  = (int)getzlong(47) * 1e-2; */
 195     /* clock_bias_sd               = (int)getzlong(49) * 1e-2; */
 196     /* clock_drift                 = (int)getzlong(51) * 1e-2; */
 197     /* clock_drift_sd              = (int)getzlong(53) * 1e-2; */
 198
 199     mask =
 200         TIME_IS | LATLON_IS | ALTITUDE_IS | CLIMB_IS | SPEED_IS | TRACK_IS |
 201         STATUS_IS | MODE_IS;
 202     gpsd_report(LOG_DATA,
 203                 "1000: time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f speed=%.2f climb=%.2f mode=%d status=%d mask=%s\n",
 204                 session->newdata.time, session->newdata.latitude,
 205                 session->newdata.longitude, session->newdata.altitude,
 206                 session->newdata.track, session->newdata.speed,
 207                 session->newdata.climb, session->newdata.mode,
 208                 session->gpsdata.status, gpsd_maskdump(mask));
 209     return mask;
 210 }
 211
 212 static gps_mask_t handle1002(struct gps_device_t *session)
 213 /* satellite signal quality report */
 214 {
 215     int i, j, status, prn;
 216
 217     /* ticks                      = getzlong(6); */
 218     /* sequence                   = getzword(8); */
 219     /* measurement_sequence       = getzword(9); */
 220     /*@+charint@*/
 221     int gps_week = getzword(10);
 222     int gps_seconds = getzlong(11);
 223     /* gps_nanoseconds            = getzlong(13); */
 224     /*@-charint@*/
 225     session->context->gps_week = (unsigned short)gps_week;
 226     session->gpsdata.satellites_used = 0;
 227     memset(session->gpsdata.used, 0, sizeof(session->gpsdata.used));
 228     for (i = 0; i < ZODIAC_CHANNELS; i++) {
 229         /*@ -type @*/
 230         session->driver.zodiac.Zv[i] = status = (int)getzword(15 + (3 * i));
 231         session->driver.zodiac.Zs[i] = prn = (int)getzword(16 + (3 * i));
 232         /*@ +type @*/
 233
 234         if (status & 1)
 235             session->gpsdata.used[session->gpsdata.satellites_used++] = prn;
 236         for (j = 0; j < ZODIAC_CHANNELS; j++) {
 237             if (session->gpsdata.PRN[j] != prn)
 238                 continue;
 239             session->gpsdata.ss[j] = (float)getzword(17 + (3 * i));
 240             break;
 241         }
 242     }
 243     session->context->gps_week = (unsigned short)gps_week;
 244     session->context->gps_tow = (double)gps_seconds;
 245     session->gpsdata.skyview_time =
 246         gpstime_to_unix(gps_week, session->context->gps_tow);
 247     gpsd_report(LOG_DATA, "1002: visible=%d used=%d mask={SATELLITE|USED}\n",
 248                 session->gpsdata.satellites_visible,
 249                 session->gpsdata.satellites_used);
 250     return SATELLITE_IS | USED_IS;
 251 }
 252
 253 static gps_mask_t handle1003(struct gps_device_t *session)
 254 /* skyview report */
 255 {
 256     int i, n;
 257
 258     /* The Polaris (and probably the DAGR) emit some strange variant of
 259      * this message which causes gpsd to crash filtering on impossible
 260      * number of satellites avoids this */
 261     n = (int)getzword(14);
 262     if ((n < 0) || (n > 12))
 263         return 0;
 264
 265     gpsd_zero_satellites(&session->gpsdata);
 266
 267     /* ticks              = getzlong(6); */
 268     /* sequence           = getzword(8); */
 269     session->gpsdata.dop.gdop = (unsigned int)getzword(9) * 1e-2;
 270     session->gpsdata.dop.pdop = (unsigned int)getzword(10) * 1e-2;
 271     session->gpsdata.dop.hdop = (unsigned int)getzword(11) * 1e-2;
 272     session->gpsdata.dop.vdop = (unsigned int)getzword(12) * 1e-2;
 273     session->gpsdata.dop.tdop = (unsigned int)getzword(13) * 1e-2;
 274     session->gpsdata.satellites_visible = n;
 275
 276     for (i = 0; i < ZODIAC_CHANNELS; i++) {
 277         if (i < session->gpsdata.satellites_visible) {
 278             session->gpsdata.PRN[i] = (int)getzword(15 + (3 * i));
 279             session->gpsdata.azimuth[i] =
 280                 (int)(((short)getzword(16 + (3 * i))) * RAD_2_DEG * 1e-4);
 281             if (session->gpsdata.azimuth[i] < 0)
 282                 session->gpsdata.azimuth[i] += 360;
 283             session->gpsdata.elevation[i] =
 284                 (int)(((short)getzword(17 + (3 * i))) * RAD_2_DEG * 1e-4);
 285         } else {
 286             session->gpsdata.PRN[i] = 0;
 287             session->gpsdata.azimuth[i] = 0;
 288             session->gpsdata.elevation[i] = 0;
 289         }
 290     }
 291     session->gpsdata.skyview_time = NAN;
 292     gpsd_report(LOG_DATA, "NAVDOP: visible=%d gdop=%.2f pdop=%.2f "
 293                 "hdop=%.2f vdop=%.2f tdop=%.2f mask={SATELLITE|DOP}\n",
 294                 session->gpsdata.satellites_visible,
 295                 session->gpsdata.dop.gdop,
 296                 session->gpsdata.dop.hdop,
 297                 session->gpsdata.dop.vdop,
 298                 session->gpsdata.dop.pdop, session->gpsdata.dop.tdop);
 299     return SATELLITE_IS | DOP_IS;
 300 }
 301
 302 static void handle1005(struct gps_device_t *session UNUSED)
 303 /* fix quality report */
 304 {
 305     /* ticks              = getzlong(6); */
 306     /* sequence           = getzword(8); */
 307     int numcorrections = (int)getzword(12);
 308
 309     if (session->newdata.mode == MODE_NO_FIX)
 310         session->gpsdata.status = STATUS_NO_FIX;
 311     else if (numcorrections == 0)
 312         session->gpsdata.status = STATUS_FIX;
 313     else
 314         session->gpsdata.status = STATUS_DGPS_FIX;
 315 }
 316
 317 static gps_mask_t handle1011(struct gps_device_t *session)
 318 /* version report */
 319 {
 320     /*
 321      * This is UNTESTED -- but harmless if buggy.  Added to support
 322      * client querying of the ID with firmware version in 2006.
 323      * The Zodiac is supposed to send one of these messages on startup.
 324      */
 325     getstringz(session->subtype, session->packet.outbuffer, 19, 28);    /* software version field */
 326     gpsd_report(LOG_DATA, "1011: subtype=%s mask={DEVICEID}\n",
 327                 session->subtype);
 328     return DEVICEID_IS;
 329 }
 330
 331
 332 static void handle1108(struct gps_device_t *session)
 333 /* leap-second correction report */
 334 {
 335     /* ticks              = getzlong(6); */
 336     /* sequence           = getzword(8); */
 337     /* utc_week_seconds   = getzlong(14); */
 338     /* leap_nanoseconds   = getzlong(17); */
 339     if ((int)(getzword(19) & 3) == 3)
 340         session->context->leap_seconds = (int)getzword(16);
 341 }
 342
 343 static gps_mask_t zodiac_analyze(struct gps_device_t *session)
 344 {
 345     char buf[BUFSIZ];
 346     int i;
 347     unsigned int id =
 348         (unsigned int)((session->packet.outbuffer[3] << 8) |
 349                        session->packet.outbuffer[2]);
 350
 351     if (session->packet.type != ZODIAC_PACKET) {
 352         const struct gps_type_t **dp;
 353         gpsd_report(LOG_PROG, "zodiac_analyze packet type %d\n",
 354                     session->packet.type);
 355         // Wrong packet type ?
 356         // Maybe find a trigger just in case it's an Earthmate
 357         gpsd_report(LOG_RAW + 4, "Is this a trigger: %s ?\n",
 358                     (char *)session->packet.outbuffer);
 359
 360         for (dp = gpsd_drivers; *dp; dp++) {
 361             char *trigger = (*dp)->trigger;
 362
 363             if (trigger != NULL
 364                 && strncmp((char *)session->packet.outbuffer, trigger,
 365                            strlen(trigger)) == 0
 366                 && isatty(session->gpsdata.gps_fd) != 0) {
 367                 gpsd_report(LOG_PROG, "found %s.\n", trigger);
 368
 369                 (void)gpsd_switch_driver(session, (*dp)->type_name);
 370                 return 0;
 371             }
 372         }
 373         return 0;
 374     }
 375
 376     buf[0] = '\0';
 377     for (i = 0; i < (int)session->packet.outbuflen; i++)
 378         (void)snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf),
 379                        "%02x", (unsigned int)session->packet.outbuffer[i]);
 380     gpsd_report(LOG_RAW, "Raw Zodiac packet type %d length %zd: %s\n",
 381                 id, session->packet.outbuflen, buf);
 382
 383     if (session->packet.outbuflen < 10)
 384         return 0;
 385
 386     (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), "%u",
 387                    id);
 388
 389     /*
 390      * Normal cycle for these devices is 1001 1002.
 391      * We count 1001 as end of cycle because 1002 doesn't
 392      * carry fix information.
 393      */
 394     session->cycle_end_reliable = true;
 395
 396     switch (id) {
 397     case 1000:
 398         return handle1000(session) | (CLEAR_IS | REPORT_IS);
 399     case 1002:
 400         return handle1002(session);
 401     case 1003:
 402         return handle1003(session);
 403     case 1005:
 404         handle1005(session);
 405         return 0;
 406     case 1011:
 407         return handle1011(session);
 408     case 1108:
 409         handle1108(session);
 410         return 0;
 411     default:
 412         return 0;
 413     }
 414 }
 415
 416 #ifdef ALLOW_CONTROLSEND
 417 static ssize_t zodiac_control_send(struct gps_device_t *session,
 418                                    char *msg, size_t len)
 419 {
 420     unsigned short *shortwords = (unsigned short *)msg;
 421
 422     /* and if len isn't even, it's your own fault */
 423     return zodiac_spew(session, shortwords[0], shortwords + 1,
 424                        (int)(len / 2 - 1));
 425 }
 426 #endif /* ALLOW_CONTROLSEND */
 427
 428 #ifdef ALLOW_RECONFIGURE
 429 static bool zodiac_speed_switch(struct gps_device_t *session,
 430                                 speed_t speed, char parity, int stopbits)
 431 {
 432     unsigned short data[15];
 433
 434     if (session->driver.zodiac.sn++ > 32767)
 435         session->driver.zodiac.sn = 0;
 436
 437     switch (parity) {
 438     case 'E':
 439     case 2:
 440         parity = (char)2;
 441         break;
 442     case 'O':
 443     case 1:
 444         parity = (char)1;
 445         break;
 446     case 'N':
 447     case 0:
 448     default:
 449         parity = (char)0;
 450         break;
 451     }
 452
 453     memset(data, 0, sizeof(data));
 454     /* data is the part of the message starting at word 6 */
 455     data[0] = session->driver.zodiac.sn;        /* sequence number */
 456     data[1] = 1;                /* port 1 data valid */
 457     data[2] = (unsigned short)parity;   /* port 1 character width (8 bits) */
 458     data[3] = (unsigned short)(stopbits - 1);   /* port 1 stop bits (1 stopbit) */
 459     data[4] = 0;                /* port 1 parity (none) */
 460     data[5] = (unsigned short)(round(log((double)speed / 300) / M_LN2) + 1);    /* port 1 speed */
 461     data[14] = zodiac_checksum(data, 14);
 462
 463     (void)zodiac_spew(session, 1330, data, 15);
 464     return true;                /* it would be nice to error-check this */
 465 }
 466 #endif /* ALLOW_RECONFIGURE */
 467
 468 #ifdef NTPSHM_ENABLE
 469 static double zodiac_ntp_offset(struct gps_device_t *session)
 470 {
 471     /* Removing/changing the magic number below is likely to disturb
 472      * the handling of the 1pps signal from the gps device. The regression
 473      * tests and simple gps applications do not detect this. A live test
 474      * with the 1pps signal active is required. */
 475     return 1.1;
 476 }
 477 #endif /* NTPSHM_ENABLE */
 478
 479 /* this is everything we export */
 480 /* *INDENT-OFF* */
 481 const struct gps_type_t zodiac_binary =
 482 {
 483     .type_name      = "Zodiac binary",  /* full name of type */
 484     .packet_type    = ZODIAC_PACKET,    /* associated lexer packet type */
 485     .trigger        = NULL,             /* no trigger */
 486     .channels       = 12,               /* consumer-grade GPS */
 487     .probe_detect   = NULL,             /* no probe */
 488     .get_packet     = generic_get,      /* use the generic packet getter */
 489     .parse_packet   = zodiac_analyze,   /* parse message packets */
 490     .rtcm_writer    = zodiac_send_rtcm, /* send DGPS correction */
 491     .event_hook     = NULL,             /* no configuration */
 492 #ifdef ALLOW_RECONFIGURE
 493     .speed_switcher = zodiac_speed_switch,/* we can change baud rate */
 494     .mode_switcher  = NULL,             /* no mode switcher */
 495     .rate_switcher  = NULL,             /* no sample-rate switcher */
 496     .min_cycle      = 1,                /* not relevant, no rate switch */
 497 #endif /* ALLOW_RECONFIGURE */
 498 #ifdef ALLOW_CONTROLSEND
 499     .control_send   = zodiac_control_send,      /* for gpsctl and friends */
 500 #endif /* ALLOW_CONTROLSEND */
 501 #ifdef NTPSHM_ENABLE
 502     .ntp_offset     = zodiac_ntp_offset,        /* compute NTO fudge factor */
 503 #endif /* NTPSHM_ENABLE */
 504 };
 505 /* *INDENT-ON* */
 506
 507 #endif /* ZODIAC_ENABLE */