driver_oncore.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 #include <sys/types.h>
   6 #include <stdio.h>
   7 #include <stdlib.h>
   8 #include <string.h>
   9 #include <math.h>
  10 #include <ctype.h>
  11 #ifndef S_SPLINT_S
  12 #include <unistd.h>
  13 #endif /* S_SPLINT_S */
  14 #include <time.h>
  15 #include <stdio.h>
  16
  17 #include "gpsd.h"
  18 #if defined(ONCORE_ENABLE) && defined(BINARY_ENABLE)
  19
  20 #include "bits.h"
  21
  22 /*@ +charint @*/
  23 static char enableEa[] = { 'E', 'a', 1 };
  24 static char enableBb[] = { 'B', 'b', 1 };
  25 static char getfirmware[] = { 'C', 'j' };
  26 static char enableEn[] =
  27     { 'E', 'n', 1, 0, 100, 100, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
  28 /*static char enableAt2[]       = { 'A', 't', 2, };*/
  29 static unsigned char pollAs[] =
  30     { 'A', 's', 0x7f, 0xff, 0xff, 0xff, 0x7f, 0xff, 0xff, 0xff, 0x7f, 0xff,
  31     0xff, 0xff, 0xff
  32 };
  33 static unsigned char pollAt[] = { 'A', 't', 0xff };
  34 static unsigned char pollAy[] = { 'A', 'y', 0xff, 0xff, 0xff, 0xff };
  35 static char pollBo[] = { 'B', 'o', 0x01 };
  36
  37 /*@ -charint @*/
  38
  39 /*
  40  * These routines are specific to this driver
  41  */
  42
  43 static gps_mask_t oncore_parse_input(struct gps_device_t *);
  44 static gps_mask_t oncore_dispatch(struct gps_device_t *, unsigned char *,
  45                                   size_t);
  46 static gps_mask_t oncore_msg_navsol(struct gps_device_t *, unsigned char *,
  47                                     size_t);
  48 static gps_mask_t oncore_msg_utc_offset(struct gps_device_t *,
  49                                         unsigned char *, size_t);
  50 static gps_mask_t oncore_msg_pps_delay(struct gps_device_t *, unsigned char *,
  51                                        size_t);
  52 static gps_mask_t oncore_msg_svinfo(struct gps_device_t *, unsigned char *,
  53                                     size_t);
  54 static gps_mask_t oncore_msg_time_raim(struct gps_device_t *, unsigned char *,
  55                                        size_t);
  56 static gps_mask_t oncore_msg_firmware(struct gps_device_t *, unsigned char *,
  57                                       size_t);
  58
  59 /*
  60  * These methods may be called elsewhere in gpsd
  61  */
  62 static ssize_t oncore_control_send(struct gps_device_t *, char *, size_t);
  63 static void oncore_event_hook(struct gps_device_t *, event_t);
  64 static bool oncore_set_speed(struct gps_device_t *, speed_t, char, int);
  65 static void oncore_set_mode(struct gps_device_t *, int);
  66
  67 /*
  68  * Decode the navigation solution message
  69  */
  70 static gps_mask_t
  71 oncore_msg_navsol(struct gps_device_t *session, unsigned char *buf,
  72                   size_t data_len)
  73 {
  74     gps_mask_t mask;
  75     unsigned char flags;
  76     double lat, lon, alt;
  77     float speed, track, dop;
  78     unsigned int i, j, st, nsv, off;
  79     int Bbused;
  80     struct tm unpacked_date;
  81     unsigned int nsec;
  82
  83     if (data_len != 76)
  84         return 0;
  85
  86     mask = ONLINE_IS;
  87     gpsd_report(LOG_IO, "oncore NAVSOL - navigation data\n");
  88
  89     flags = (unsigned char)getub(buf, 72);
  90
  91     /*@ -predboolothers @*/
  92     if (flags & 0x20) {
  93         session->gpsdata.status = STATUS_FIX;
  94         session->newdata.mode = MODE_3D;
  95     } else if (flags & 0x10) {
  96         session->gpsdata.status = STATUS_FIX;
  97         session->newdata.mode = MODE_2D;
  98     } else {
  99         gpsd_report(LOG_WARN, "oncore NAVSOL no fix - flags 0x%02x\n", flags);
 100         session->newdata.mode = MODE_NO_FIX;
 101         session->gpsdata.status = STATUS_NO_FIX;
 102     }
 103     mask |= MODE_IS;
 104     /*@ +predboolothers @*/
 105
 106     /* Unless we have seen non-zero utc offset data, the time is GPS time
 107      * and not UTC time.  Do not use it.
 108      */
 109     if (session->context->leap_seconds) {
 110         unpacked_date.tm_mon = (int)getub(buf, 4) - 1;
 111         unpacked_date.tm_mday = (int)getub(buf, 5);
 112         unpacked_date.tm_year = (int)getbeuw(buf, 6) - 1900;
 113         unpacked_date.tm_hour = (int)getub(buf, 8);
 114         unpacked_date.tm_min = (int)getub(buf, 9);
 115         unpacked_date.tm_sec = (int)getub(buf, 10);
 116         nsec = (uint) getbeul(buf, 11);
 117
 118         /*@ -unrecog */
 119         session->newdata.time = (double)timegm(&unpacked_date) + nsec * 1e-9;
 120         /*@ +unrecog */
 121         mask |= TIME_IS;
 122         gpsd_report(LOG_IO,
 123                     "oncore NAVSOL - time: %04d-%02d-%02d %02d:%02d:%02d.%09d\n",
 124                     unpacked_date.tm_year + 1900, unpacked_date.tm_mon + 1,
 125                     unpacked_date.tm_mday, unpacked_date.tm_hour,
 126                     unpacked_date.tm_min, unpacked_date.tm_sec, nsec);
 127     }
 128
 129     /*@-type@*/
 130     lat = getbesl(buf, 15) / 3600000.0f;
 131     lon = getbesl(buf, 19) / 3600000.0f;
 132     alt = getbesl(buf, 23) / 100.0f;
 133     speed = getbeuw(buf, 31) / 100.0f;
 134     track = getbeuw(buf, 33) / 10.0f;
 135     dop = getbeuw(buf, 35) / 10.0f;
 136     /*@+type@*/
 137
 138     gpsd_report(LOG_IO,
 139                 "oncore NAVSOL - %lf %lf %.2lfm-%.2lfm | %.2fm/s %.1fdeg dop=%.1f\n",
 140                 lat, lon, alt, wgs84_separation(lat, lon), speed, track,
 141                 (float)dop);
 142
 143     session->newdata.latitude = lat;
 144     session->newdata.longitude = lon;
 145     session->gpsdata.separation =
 146         wgs84_separation(session->newdata.latitude,
 147                          session->newdata.longitude);
 148     session->newdata.altitude = alt - session->gpsdata.separation;
 149     session->newdata.speed = speed;
 150     session->newdata.track = track;
 151
 152     mask |= LATLON_IS | ALTITUDE_IS | SPEED_IS | TRACK_IS;
 153
 154     gpsd_zero_satellites(&session->gpsdata);
 155     /* Merge the satellite information from the Bb message. */
 156     Bbused = 0;
 157     nsv = 0;
 158     for (i = st = 0; i < 8; i++) {
 159         int sv, mode, sn, status;
 160
 161         off = 40 + 4 * i;
 162         sv = (int)getub(buf, off);
 163         mode = (int)getub(buf, off + 1);
 164         sn = (int)getub(buf, off + 2);
 165         status = (int)getub(buf, off + 3);
 166
 167         gpsd_report(LOG_IO, "%2d %2d %2d %3d %02x\n", i, sv, mode, sn,
 168                     status);
 169
 170         if (sn) {
 171             session->gpsdata.PRN[st] = sv;
 172             session->gpsdata.ss[st] = (double)sn;
 173             for (j = 0; (int)j < session->driver.oncore.visible; j++)
 174                 if (session->driver.oncore.PRN[j] == sv) {
 175                     session->gpsdata.elevation[st] =
 176                         session->driver.oncore.elevation[j];
 177                     session->gpsdata.azimuth[st] =
 178                         session->driver.oncore.azimuth[j];
 179                     Bbused |= 1 << j;
 180                     break;
 181                 }
 182             st++;
 183             if (status & 0x80)
 184                 session->gpsdata.used[nsv++] = sv;
 185         }
 186     }
 187     for (j = 0; (int)j < session->driver.oncore.visible; j++)
 188         /*@ -boolops @*/
 189         if (!(Bbused & (1 << j))) {
 190             session->gpsdata.PRN[st] = session->driver.oncore.PRN[j];
 191             session->gpsdata.elevation[st] =
 192                 session->driver.oncore.elevation[j];
 193             session->gpsdata.azimuth[st] = session->driver.oncore.azimuth[j];
 194             st++;
 195         }
 196     /*@ +boolops @*/
 197     session->gpsdata.skyview_time = session->newdata.time;
 198     session->gpsdata.satellites_used = (int)nsv;
 199     session->gpsdata.satellites_visible = (int)st;
 200
 201     mask |= SATELLITE_IS | USED_IS;
 202
 203     /* Some messages can only be polled.  As they are not so
 204      * important, would be enough to poll e.g. one message per cycle.
 205      */
 206     (void)oncore_control_send(session, (char *)pollAs, sizeof(pollAs));
 207     (void)oncore_control_send(session, (char *)pollAt, sizeof(pollAt));
 208     (void)oncore_control_send(session, (char *)pollAy, sizeof(pollAy));
 209     (void)oncore_control_send(session, pollBo, sizeof(pollBo));
 210
 211     gpsd_report(LOG_DATA,
 212                 "NAVSOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f speed=%.2f track=%.2f mode=%d status=%d visible=%d used=%d mask=%s\n",
 213                 session->newdata.time, session->newdata.latitude,
 214                 session->newdata.longitude, session->newdata.altitude,
 215                 session->newdata.speed, session->newdata.track,
 216                 session->newdata.mode, session->gpsdata.status,
 217                 session->gpsdata.satellites_used,
 218                 session->gpsdata.satellites_visible, gpsd_maskdump(mask));
 219     return mask;
 220 }
 221
 222 /**
 223  * GPS Leap Seconds = UTC offset
 224  */
 225 static gps_mask_t
 226 oncore_msg_utc_offset(struct gps_device_t *session, unsigned char *buf,
 227                       size_t data_len)
 228 {
 229     int utc_offset;
 230
 231     if (data_len != 8)
 232         return 0;
 233
 234     gpsd_report(LOG_IO, "oncore UTCTIME - leap seconds\n");
 235     utc_offset = (int)getub(buf, 4);
 236     if (utc_offset == 0)
 237         return 0;               /* that part of almanac not received yet */
 238
 239     session->context->leap_seconds = utc_offset;
 240     session->context->valid |= LEAP_SECOND_VALID;
 241     return 0;                   /* no flag for leap seconds update */
 242 }
 243
 244 /**
 245  * PPS delay
 246  */
 247 static gps_mask_t
 248 oncore_msg_pps_delay(struct gps_device_t *session, unsigned char *buf,
 249                      size_t data_len)
 250 {
 251     double pps_delay;
 252
 253     if (data_len != 11)
 254         return 0;
 255
 256     gpsd_report(LOG_IO, "oncore PPS delay\n");
 257     pps_delay = getbesl(buf, 4) / 1000000.0;
 258
 259     session->driver.oncore.pps_delay = pps_delay;
 260     return 0;
 261 }
 262
 263 /**
 264  * GPS Satellite Info
 265  */
 266 static gps_mask_t
 267 oncore_msg_svinfo(struct gps_device_t *session, unsigned char *buf,
 268                   size_t data_len)
 269 {
 270     unsigned int i, nchan;
 271     unsigned int off;
 272     int sv, el, az;
 273     int j;
 274
 275     if (data_len != 92)
 276         return 0;
 277
 278     gpsd_report(LOG_IO, "oncore SVINFO - satellite data\n");
 279     nchan = (unsigned int)getub(buf, 4);
 280     gpsd_report(LOG_IO, "oncore SVINFO - %d satellites:\n", nchan);
 281     /* Then we clamp the value to not read outside the table. */
 282     if (nchan > 12)
 283         nchan = 12;
 284     session->driver.oncore.visible = (int)nchan;
 285     for (i = 0; i < nchan; i++) {
 286         /* get info for one channel/satellite */
 287         off = 5 + 7 * i;
 288
 289         sv = (int)getub(buf, off);
 290         el = (int)getub(buf, off + 3);
 291         az = (int)getbeuw(buf, off + 4);
 292
 293         gpsd_report(LOG_IO, "%2d %2d %2d %3d\n", i, sv, el, az);
 294
 295         /* Store for use when Ea messages come. */
 296         session->driver.oncore.PRN[i] = sv;
 297         session->driver.oncore.elevation[i] = el;
 298         session->driver.oncore.azimuth[i] = az;
 299         /* If it has an entry in the satellite list, update it! */
 300         for (j = 0; j < session->gpsdata.satellites_visible; j++)
 301             if (session->gpsdata.PRN[j] == sv) {
 302                 session->gpsdata.elevation[j] = el;
 303                 session->gpsdata.azimuth[j] = az;
 304             }
 305     }
 306
 307     gpsd_report(LOG_DATA, "SVINFO: mask={SATELLITE}\n");
 308     return SATELLITE_IS;
 309 }
 310
 311 /**
 312  * GPS Time RAIM
 313  */
 314 static gps_mask_t
 315 oncore_msg_time_raim(struct gps_device_t *session UNUSED,
 316                      unsigned char *buf UNUSED, size_t data_len UNUSED)
 317 {
 318     return 0;
 319 }
 320
 321 /**
 322  * GPS Firmware
 323  */
 324 static gps_mask_t
 325 oncore_msg_firmware(struct gps_device_t *session UNUSED,
 326                     unsigned char *buf UNUSED, size_t data_len UNUSED)
 327 {
 328     return 0;
 329 }
 330
 331 #define ONCTYPE(id2,id3) ((((unsigned int)id2)<<8)|(id3))
 332
 333 /**
 334  * Parse the data from the device
 335  */
 336 /*@ +charint @*/
 337 gps_mask_t oncore_dispatch(struct gps_device_t * session, unsigned char *buf,
 338                            size_t len)
 339 {
 340     unsigned int type;
 341
 342     if (len == 0)
 343         return 0;
 344
 345     type = ONCTYPE(buf[2], buf[3]);
 346
 347     /* we may need to dump the raw packet */
 348     gpsd_report(LOG_RAW, "raw oncore packet type 0x%04x length %zd: %s\n",
 349                 type, len, gpsd_hexdump_wrapper(buf, len, LOG_WARN));
 350
 351     (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag),
 352                    "MOT-%c%c", type >> 8, type & 0xff);
 353
 354     session->cycle_end_reliable = true;
 355
 356     switch (type) {
 357     case ONCTYPE('B', 'b'):
 358         return oncore_msg_svinfo(session, buf, len);
 359     case ONCTYPE('E', 'a'):
 360         return oncore_msg_navsol(session, buf, len) | (CLEAR_IS | REPORT_IS);
 361     case ONCTYPE('E', 'n'):
 362         return oncore_msg_time_raim(session, buf, len);
 363     case ONCTYPE('C', 'j'):
 364         return oncore_msg_firmware(session, buf, len);
 365     case ONCTYPE('B', 'o'):
 366         return oncore_msg_utc_offset(session, buf, len);
 367     case ONCTYPE('A', 's'):
 368         return 0;               /* position hold mode */
 369     case ONCTYPE('A', 't'):
 370         return 0;               /* position hold position */
 371     case ONCTYPE('A', 'y'):
 372         return oncore_msg_pps_delay(session, buf, len);
 373
 374     default:
 375         /* FIX-ME: This gets noisy in a hurry. Change once your driver works */
 376         gpsd_report(LOG_WARN, "unknown packet id @@%c%c length %zd: %s\n",
 377                     type >> 8, type & 0xff, len, gpsd_hexdump_wrapper(buf,
 378                                                                       len,
 379                                                                       LOG_WARN));
 380         return 0;
 381     }
 382 }
 383
 384 /*@ -charint @*/
 385
 386 /**********************************************************
 387  *
 388  * Externally called routines below here
 389  *
 390  **********************************************************/
 391
 392 /**
 393  * Write data to the device, doing any required padding or checksumming
 394  */
 395 /*@ +charint -usedef -compdef @*/
 396 static ssize_t oncore_control_send(struct gps_device_t *session,
 397                                    char *msg, size_t msglen)
 398 {
 399     size_t i;
 400     char checksum = 0;
 401
 402     session->msgbuf[0] = '@';
 403     session->msgbuf[1] = '@';
 404     for (i = 0; i < msglen; i++) {
 405         checksum ^= session->msgbuf[i + 2] = msg[i];
 406     }
 407     session->msgbuf[msglen + 2] = checksum;
 408     session->msgbuf[msglen + 3] = '\r';
 409     session->msgbuf[msglen + 4] = '\n';
 410     session->msgbuflen = msglen + 5;
 411
 412     gpsd_report(LOG_IO, "writing oncore control type %c%c:%s\n",
 413                 msg[0], msg[1], gpsd_hexdump_wrapper(session->msgbuf,
 414                                                      session->msgbuflen,
 415                                                      LOG_IO));
 416     return gpsd_write(session, session->msgbuf, session->msgbuflen);
 417 }
 418
 419 /*@ -charint +usedef +compdef @*/
 420
 421 static void oncore_event_hook(struct gps_device_t *session, event_t event)
 422 {
 423     if (event == event_wakeup)
 424         (void)oncore_control_send(session, getfirmware, sizeof(getfirmware));
 425
 426     /*
 427      * FIX-ME: It might not be necessary to call this on reactivate.
 428      * Experiment to see if the holds its settings through a close.
 429      */
 430     if (event == event_identified || event == event_reactivate) {
 431         (void)oncore_control_send(session, enableEa, sizeof(enableEa));
 432         (void)oncore_control_send(session, enableBb, sizeof(enableBb));
 433         (void)oncore_control_send(session, enableEn, sizeof(enableEn));
 434         /*(void)oncore_control_send(session,enableAt2,sizeof(enableAt2)); */
 435         /*(void)oncore_control_send(session,pollAs,sizeof(pollAs)); */
 436         (void)oncore_control_send(session, pollBo, sizeof(pollBo));
 437     }
 438 }
 439
 440 #ifdef NTPSHM_ENABLE
 441 static double oncore_ntp_offset(struct gps_device_t *session)
 442 {
 443     /*
 444      * Only one sentence (NAVSOL) ships time.  0.175 seems best at
 445      * 9600 for UT+, not sure what the fudge should be at other baud
 446      * rates or for other models.
 447      */
 448     return 0.175;
 449 }
 450 #endif /* NTPSHM_ENABLE */
 451
 452 #ifdef ALLOW_RECONFIGURE
 453 static bool oncore_set_speed(struct gps_device_t *session UNUSED,
 454                              speed_t speed UNUSED,
 455                              char parity UNUSED, int stopbits UNUSED)
 456 {
 457     /*
 458      * Set port operating mode, speed, parity, stopbits etc. here.
 459      * Note: parity is passed as 'N'/'E'/'O', but you should program
 460      * defensively and allow 0/1/2 as well.
 461      */
 462     return false;
 463 }
 464
 465 /*
 466  * Switch between NMEA and binary mode, if supported
 467  */
 468 static void oncore_set_mode(struct gps_device_t *session, int mode)
 469 {
 470     if (mode == MODE_NMEA) {
 471         /* send the mode switch control string */
 472         /* oncore_to_nmea(session->gpsdata.gps_fd,session->gpsdata.baudrate); */
 473         session->gpsdata.dev.driver_mode = MODE_NMEA;
 474         /*
 475          * Anticipatory switching works only when the packet getter is the
 476          * generic one and it recognizes packets of the type this driver
 477          * is expecting.  This should be the normal case.
 478          */
 479         (void)gpsd_switch_driver(session, "Generic NMEA");
 480     } else {
 481         session->back_to_nmea = false;
 482         session->gpsdata.dev.driver_mode = MODE_BINARY;
 483     }
 484 }
 485 #endif /* ALLOW_RECONFIGURE */
 486
 487 static gps_mask_t oncore_parse_input(struct gps_device_t *session)
 488 {
 489     gps_mask_t st;
 490
 491     if (session->packet.type == ONCORE_PACKET) {
 492         st = oncore_dispatch(session, session->packet.outbuffer,
 493                              session->packet.outbuflen);
 494         session->gpsdata.dev.driver_mode = MODE_BINARY;
 495         return st;
 496 #ifdef NMEA_ENABLE
 497     } else if (session->packet.type == NMEA_PACKET) {
 498         st = nmea_parse((char *)session->packet.outbuffer, session);
 499         session->gpsdata.dev.driver_mode = MODE_NMEA;
 500         return st;
 501 #endif /* NMEA_ENABLE */
 502     } else
 503         return 0;
 504 }
 505
 506 /* This is everything we export */
 507 /* *INDENT-OFF* */
 508 const struct gps_type_t oncore_binary = {
 509     /* Full name of type */
 510     .type_name        = "oncore binary",
 511     /* associated lexer packet type */
 512     .packet_type      = ONCORE_PACKET,
 513     /* Response string that identifies device (not active) */
 514     .trigger          = NULL,
 515     /* Number of satellite channels supported by the device */
 516     .channels         = 12,
 517     /* Startup-time device detector */
 518     .probe_detect     = NULL,
 519     /* Wakeup to be done before each baud hunt */
 520     .get_packet       = generic_get,
 521     /* Parse message packets */
 522     .parse_packet     = oncore_parse_input,
 523     /* RTCM handler (using default routine) */
 524     .rtcm_writer      = pass_rtcm,
 525     /* Fire on various lifetime events */
 526     .event_hook     = oncore_event_hook,
 527 #ifdef ALLOW_RECONFIGURE
 528     /* Speed (baudrate) switch */
 529     .speed_switcher   = oncore_set_speed,
 530     /* Switch to NMEA mode */
 531     .mode_switcher    = oncore_set_mode,
 532     /* Message delivery rate switcher (not active) */
 533     .rate_switcher    = NULL,
 534     /* Minimum cycle time of the device */
 535     .min_cycle        = 1,
 536     /* Undo actions at configure_event time */
 537 #endif /* ALLOW_RECONFIGURE */
 538 #ifdef ALLOW_CONTROLSEND
 539     /* Control string sender - should provide checksum and headers/trailer */
 540     .control_send   = oncore_control_send,
 541 #endif /* ALLOW_CONTROLSEND */
 542 #ifdef NTPSHM_ENABLE
 543     .ntp_offset = oncore_ntp_offset,
 544 #endif /* NTPSHM_ENABLE */
 545 };
 546 /* *INDENT-ON* */
 547 #endif /* defined(ONCORE_ENABLE) && defined(BINARY_ENABLE) */