driver_navcom.c

   1 /*
   2  * Driver for Navcom receivers using propietary NCT messages, a binary protocol.
   3  *
   4  * Vendor website: http://www.navcomtech.com/
   5  * Technical references: http://www.navcomtech.com/support/docs.cfm
   6  *
   7  * Tested with two SF-2040G models
   8  *
   9  * At this stage, this driver implements the following commands:
  10  *
  11  * 0x20: Data Request (tell the unit which responses you want)
  12  * 0x3f: LED Configuration (controls the front panel LEDs -- for testing)
  13  * 0x1c: Test Support Block (again, blinks the front panel lights)
  14  *
  15  * and it understands the following responses:
  16  *
  17  * 0x06: Acknowledgement (without error)
  18  * 0x15: Negative Acknowledge
  19  * 0x86: Channel Status
  20  * 0xae: Identification Block
  21  * 0xb0: Raw Meas. Data Block
  22  * 0xb1: PVT Block
  23  * 0xb5: Pseudorange Noise Statistics
  24  * 0xd3: LBM DSP Status Block
  25  * 0xef: Clock Drift and Offset
  26  *
  27  * By Diego Berge. Contact via web form at http://www.navlost.eu/contact
  28  *
  29  * This file is Copyright (c) 2010 by the GPSD project
  30  * BSD terms apply: see the file COPYING in the distribution root for details.
  31  */
  32
  33 #include <stdio.h>
  34 #include <stdlib.h>
  35 #include <string.h>
  36 #include <math.h>
  37 #include <ctype.h>
  38 #ifndef S_SPLINT_S
  39 #include <unistd.h>
  40 #endif /* S_SPLINT_S */
  41 #include <time.h>
  42 #include <sys/types.h>
  43 #include <inttypes.h>
  44 #include <stdio.h>
  45 #include "gpsd.h"
  46
  47 #if defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE)
  48 #include "bits.h"
  49
  50 /* Have data which is 24 bits long */
  51 #define getlesl24(buf,off)  (int32_t)(((uint32_t)getub((buf), (off)+2)<<24 | (uint32_t)getub((buf), (off)+1)<<16 | (uint32_t)getub((buf), (off))<<8)>>8)
  52 #define getleul24(buf,off) (uint32_t)(((uint32_t)getub((buf), (off)+2)<<24 | (uint32_t)getub((buf), (off)+1)<<16 | (uint32_t)getub((buf), (off))<<8)>>8)
  53
  54 /* And just to be difficult, Navcom is little endian but the GPS data stream
  55    is big endian.  Some messages contain raw GPS data */
  56 #define getlesw_be(buf, off)    (int16_t)((((uint16_t)getub(buf, (off)) << 8) \
  57                                     | (uint16_t)getub(buf, (off)+1)))
  58 #define getleuw_be(buf, off)    (uint16_t)((((uint16_t)getub(buf, (off)) << 8) \
  59                                     | (uint16_t)getub(buf, (off)+1)))
  60 #define getlesl_be(buf, off)    (int32_t)((((uint16_t)getleuw_be(buf, (off)) << 16) \
  61                                     | getleuw_be(buf, (off)+2)))
  62 #define getleul_be(buf, off)    (uint32_t)((((uint16_t)getleuw_be(buf, (off)) << 16) \
  63                                     | getleuw_be(buf, (off)+2)))
  64 #define getlesL_be(buf, off)    (int64_t)((((uint64_t)getleul_be(buf, (off)) << 32) \
  65                                     | getleul_be(buf, (off)+4)))
  66 #define getleuL_be(buf, off)    (uint64_t)((((uint64_t)getleul_be(buf, (off)) << 32) \
  67                                     | getleul_be(buf, (off)+4)))
  68 #define getlesl24_be(buf,off)     (int32_t)(((uint32_t)getub((buf), (off))<<24 \
  69                                     | (uint32_t)getub((buf), (off)+1)<<16 \
  70                                     | (uint32_t)getub((buf), (off)+2)<<8)>>8)
  71
  72 #define NAVCOM_CHANNELS 12
  73
  74 static uint8_t checksum(unsigned char *buf, size_t len)
  75 {
  76     size_t n;
  77     uint8_t csum = (uint8_t) 0x00;
  78     for (n = 0; n < len; n++)
  79         csum ^= buf[n];
  80     return csum;
  81 }
  82
  83 static bool navcom_send_cmd(struct gps_device_t *session, unsigned char *cmd,
  84                             size_t len)
  85 {
  86     gpsd_report(LOG_RAW, "Navcom: command dump: %s\n",
  87                 gpsd_hexdump_wrapper(cmd, len, LOG_RAW));
  88     return (gpsd_write(session, cmd, len) == (ssize_t) len);
  89 }
  90
  91 /* Data Request */
  92 static void navcom_cmd_0x20(struct gps_device_t *session, uint8_t block_id,
  93                             uint16_t rate)
  94 {
  95     unsigned char msg[18];
  96     putbyte(msg, 0, 0x02);
  97     putbyte(msg, 1, 0x99);
  98     putbyte(msg, 2, 0x66);
  99     putbyte(msg, 3, 0x20);      /* Cmd ID */
 100     putleword(msg, 4, 0x000e);  /* Length */
 101     putbyte(msg, 6, 0x00);      /* Action */
 102     putbyte(msg, 7, 0x01);      /* Count of blocks */
 103     putbyte(msg, 8, block_id);  /* Data Block ID */
 104     putbyte(msg, 9, 0x02);      /* Logical Ports */
 105     putleword(msg, 10, rate);   /* Data rate */
 106     putbyte(msg, 12, 0x71);
 107     putbyte(msg, 13, 0x00);
 108     putleword(msg, 14, 0x0000);
 109     putbyte(msg, 16, checksum(msg + 3, 13));
 110     putbyte(msg, 17, 0x03);
 111     (void)navcom_send_cmd(session, msg, 18);
 112     gpsd_report(LOG_PROG,
 113                 "Navcom: sent command 0x20 (Data Request) "
 114                 "- data block id = %02x at rate %02x\n", block_id, rate);
 115 }
 116
 117 /*@ unused @*/
 118 /* Changes the LED settings in the receiver */
 119 static void UNUSED navcom_cmd_0x3f(struct gps_device_t *session)
 120 {
 121     unsigned char msg[12];
 122     putbyte(msg, 0, 0x02);
 123     putbyte(msg, 1, 0x99);
 124     putbyte(msg, 2, 0x66);
 125     putbyte(msg, 3, 0x3f);      /* Cmd ID */
 126     putleword(msg, 4, 0x0008);
 127     putbyte(msg, 6, 0x01);      /* Action */
 128     putbyte(msg, 7, 0x00);      /* Reserved */
 129     putbyte(msg, 8, 0x02);      /* Link LED setting */
 130     putbyte(msg, 9, 0x0a);      /* Battery LED setting */
 131     putbyte(msg, 10, checksum(msg + 3, 7));
 132     putbyte(msg, 11, 0x03);
 133     (void)navcom_send_cmd(session, msg, 12);
 134     gpsd_report(LOG_PROG,
 135                 "Navcom: sent command 0x3f (LED Configuration Block)\n");
 136 }
 137
 138 /* Test Support Block - Blinks the LEDs */
 139 static void navcom_cmd_0x1c(struct gps_device_t *session, uint8_t mode,
 140                             uint8_t length)
 141 {
 142     unsigned char msg[12];
 143     putbyte(msg, 0, 0x02);
 144     putbyte(msg, 1, 0x99);
 145     putbyte(msg, 2, 0x66);
 146     putbyte(msg, 3, 0x1c);      /* Cmd ID */
 147     putleword(msg, 4, 0x0008);
 148     putbyte(msg, 6, 0x04);      /* Use ACK/NAK */
 149     putbyte(msg, 7, mode);      /* 0x01 or 0x02 */
 150     putbyte(msg, 8, length);    /* Only if mode == 0x01 */
 151     putbyte(msg, 9, 0x00);
 152     putbyte(msg, 10, checksum(msg + 3, 7));
 153     putbyte(msg, 11, 0x03);
 154     (void)navcom_send_cmd(session, msg, 12);
 155     gpsd_report(LOG_PROG, "Navcom: sent command 0x1c (Test Support Block)\n");
 156     gpsd_report(LOG_IO,
 157                 "Navcom: command 0x1c mode = %02x, length = %u\n",
 158                 mode, length);
 159 }
 160
 161 #ifdef ALLOW_RECONFIGURE
 162 /* Serial Port Configuration */
 163 static void navcom_cmd_0x11(struct gps_device_t *session,
 164                             uint8_t port_selection)
 165 {
 166     /* NOTE - We only allow changing one port at a time,
 167      * although the message supports doing both at once. */
 168     unsigned char msg[12];
 169     putbyte(msg, 0, 0x02);
 170     putbyte(msg, 1, 0x99);
 171     putbyte(msg, 2, 0x66);
 172     putbyte(msg, 3, 0x11);      /* Cmd ID */
 173     putleword(msg, 4, 0x0008);  /* Length */
 174     putbyte(msg, 6, 0x04);      /* Action - Use ACK/NAK) */
 175     putbyte(msg, 7, port_selection);
 176     putbyte(msg, 8, 0x00);      /* Reserved */
 177     putbyte(msg, 9, 0x00);      /* Reserved */
 178     putbyte(msg, 10, checksum(msg + 3, 7));
 179     putbyte(msg, 11, 0x03);
 180     (void)navcom_send_cmd(session, msg, 12);
 181     gpsd_report(LOG_PROG,
 182                 "Navcom: sent command 0x11 (Serial Port Configuration)\n");
 183     gpsd_report(LOG_IO,
 184                 "Navcom: serial port selection: 0x%02x\n", port_selection);
 185 }
 186 #endif /* ALLOW_RECONFIGURE */
 187
 188 static void navcom_event_hook(struct gps_device_t *session, event_t event)
 189 {
 190     if (event == event_wakeup) {
 191         /* NOTE - This allows us to know into which of the unit's various
 192          * serial ports we are connected.
 193          * Its value gets updated every time we receive a 0x06 (Ack)
 194          * message.  Note that if commands are being fed into the
 195          * unit from more than one port (which is entirely possible
 196          * although not necessarily a bright idea), there is a good
 197          * chance that we might misidentify our port */
 198         /*@ -type @*/
 199         navcom_cmd_0x1c(session, 0x02, 0);      /* Test Support Block */
 200         navcom_cmd_0x20(session, 0xae, 0x0000); /* Identification Block */
 201         navcom_cmd_0x20(session, 0x86, 0x000a); /* Channel Status */
 202         /*@ +type @*/
 203     }
 204     /* Request the following messages: */
 205     /*
 206      * FIX-ME: It might not be necessary to call this on reactivate.
 207      * Experiment to see if the holds its settings through a close.
 208      */
 209     if (event == event_identified || event == event_reactivate) {
 210         /*@ +charint @*/
 211         navcom_cmd_0x1c(session, 0x01, 5);      /* Blink LEDs on receiver */
 212         navcom_cmd_0x20(session, 0xae, 0x1770); /* Identification Block - send every 10 min */
 213         navcom_cmd_0x20(session, 0xb1, 0x4000); /* PVT Block */
 214         navcom_cmd_0x20(session, 0xb5, 0x00c8); /* Pseudorange Noise Statistics - send every 20s */
 215         navcom_cmd_0x20(session, 0xb0, 0x4000); /* Raw Meas Data Block */
 216         navcom_cmd_0x20(session, 0x81, 0x0000); /* Packed Ephemeris Data - send once */
 217         navcom_cmd_0x20(session, 0x81, 0x4000); /* Packed Ephemeris Data */
 218         navcom_cmd_0x20(session, 0x86, 0x4000); /* Channel Status */
 219         navcom_cmd_0x20(session, 0x83, 0x4000); /* Ionosphere and UTC Data */
 220         navcom_cmd_0x20(session, 0xef, 0x0bb8); /* Clock Drift - send every 5 min */
 221         /*@ -charint @*/
 222     }
 223 }
 224
 225 /* Ionosphere and UTC Data */
 226 static gps_mask_t handle_0x83(struct gps_device_t *session)
 227 {
 228     /* NOTE - At the present moment this is only being used
 229      * for determining the GPS-UTC time difference,
 230      * for which the iono data is not needed as far
 231      * as we are concerned.  However, I am still
 232      * reporting it (if debuglevel >= LOG_IO) as a
 233      * matter of interest */
 234 /* 2^-30 */
 235 #define SF_A0 (0.000000000931322574615478515625)
 236 /* 2^-50 */
 237 #define SF_A1 (0.000000000000000888178419700125)
 238 /* 2^12 */
 239 #define SF_TOT (4096)
 240 /* 2^-30 */
 241 #define SF_ALPHA0 (0.000000000931322574615478515625)
 242 /* 2^-27 */
 243 #define SF_ALPHA1 (0.000000007450580596923828125)
 244 /* 2^-24 */
 245 #define SF_ALPHA2 (0.000000059604644775390625)
 246 /* 2^-24 */
 247 #define SF_ALPHA3 (0.000000059604644775390625)
 248 /* 2^11 */
 249 #define SF_BETA0 (2048)
 250 /* 2^14 */
 251 #define SF_BETA1 (16384)
 252 /* 2^16 */
 253 #define SF_BETA2 (65536)
 254 /* 2^16 */
 255 #define SF_BETA3 (65536)
 256     unsigned char *buf = session->packet.outbuffer + 3;
 257     uint16_t week = getleuw(buf, 3);
 258     uint32_t tow = getleul(buf, 5);
 259     int8_t alpha0 = getsb(buf, 9);
 260     int8_t alpha1 = getsb(buf, 10);
 261     int8_t alpha2 = getsb(buf, 11);
 262     int8_t alpha3 = getsb(buf, 12);
 263     int8_t beta0 = getsb(buf, 13);
 264     int8_t beta1 = getsb(buf, 14);
 265     int8_t beta2 = getsb(buf, 15);
 266     int8_t beta3 = getsb(buf, 16);
 267     int32_t a1 = getlesl(buf, 17);
 268     int32_t a0 = getlesl(buf, 21);
 269     uint8_t tot = getub(buf, 25);
 270     uint8_t wnt = getub(buf, 26);
 271     int8_t dtls = getsb(buf, 27);
 272     uint8_t wnlsf = getub(buf, 28);
 273     uint8_t dn = getub(buf, 29);
 274     int8_t dtlsf = getsb(buf, 30);
 275
 276     /*@ +charint +relaxtypes @*/
 277     /* Ref.: ICD-GPS-200C 20.3.3.5.2.4 */
 278     if ((week % 256) * 604800 + tow / 1000.0 < wnlsf * 604800 + dn * 86400) {
 279         /* Effectivity time is in the future, use dtls */
 280         session->context->leap_seconds = (int)dtls;
 281     } else {
 282         /* Effectivity time is not in the future, use dtlsf */
 283         session->context->leap_seconds = (int)dtlsf;
 284     }
 285     /*@ -relaxtypes -charint @*/
 286
 287     //gpstime_to_unix((int)week, tow/1000.0) - session->context->leap_seconds;
 288
 289     gpsd_report(LOG_PROG,
 290                 "Navcom: received packet type 0x83 (Ionosphere and UTC Data)\n");
 291     gpsd_report(LOG_IO, "Navcom: Scaled parameters follow:\n");
 292     gpsd_report(LOG_IO,
 293                 "Navcom: GPS Week: %u, GPS Time of Week: %u (GPS Time: %f)\n",
 294                 week, tow, week * 604800 + tow / 1000.0);
 295     gpsd_report(LOG_IO,
 296                 "Navcom: a0: %12.4E, a1: %12.4E, a2: %12.4E, a3: %12.4E, "
 297                 "b0: %12.4E, b1: %12.4E, b2: %12.4E, b3: %12.4E\n",
 298                 (double)alpha0 * SF_ALPHA0, (double)alpha1 * SF_ALPHA1,
 299                 (double)alpha2 * SF_ALPHA2, (double)alpha3 * SF_ALPHA3,
 300                 (double)beta0 * SF_BETA0, (double)beta1 * SF_BETA1,
 301                 (double)beta2 * SF_BETA2, (double)beta3 * SF_BETA3);
 302     gpsd_report(LOG_IO,
 303                 "Navcom: A0: %19.12E, A1: %19.12E\n", (double)a0 * SF_A0,
 304                 (double)a1 * SF_A1);
 305     gpsd_report(LOG_IO,
 306                 "Navcom: UTC Ref. Time: %lu, UTC Ref. Week: %u, dTls: %d\n",
 307                 (unsigned long)tot * SF_TOT, wnt, dtls);
 308     gpsd_report(LOG_IO,
 309                 "Navcom: Week of leap seconds: %u, Day number of leap seconds: %u, dTlsf: %d\n",
 310                 wnlsf, dn, dtlsf);
 311
 312     return 0;                   /* No flag for update of leap seconds (Not part of a fix) */
 313
 314 #undef SF_A0
 315 #undef SF_A1
 316 #undef SF_TOT
 317 #undef SF_ALPHA0
 318 #undef SF_ALPHA1
 319 #undef SF_ALPHA2
 320 #undef SF_ALPHA3
 321 #undef SF_BETA0
 322 #undef SF_BETA1
 323 #undef SF_BETA2
 324 #undef SF_BETA3
 325 }
 326
 327 /* Acknowledgement (without error) */
 328 static gps_mask_t handle_0x06(struct gps_device_t *session)
 329 {
 330     unsigned char *buf = session->packet.outbuffer + 3;
 331     uint8_t cmd_id = getub(buf, 3);
 332     uint8_t port = getub(buf, 4);
 333     session->driver.navcom.physical_port = port;        /* This tells us which serial port was used last */
 334     gpsd_report(LOG_PROG,
 335                 "Navcom: received packet type 0x06 (Acknowledgement (without error))\n");
 336     /*@ -type @*/
 337     gpsd_report(LOG_IO,
 338                 "Navcom: acknowledged command id 0x%02x on port %c\n",
 339                 cmd_id, (port == 0 ? 'A' : (port == 1 ? 'B' : '?')));
 340     /*@ +type @*/
 341     return 0;                   /* Nothing updated */
 342 }
 343
 344 /* Negative Acknowledge */
 345 static gps_mask_t handle_0x15(struct gps_device_t *session)
 346 {
 347     size_t n;
 348     unsigned char *buf = session->packet.outbuffer + 3;
 349     size_t msg_len = (size_t) getleuw(buf, 1);
 350     /*@ -type @*/
 351     uint8_t port, cmd_id = getub(buf, 3);
 352     gpsd_report(LOG_PROG,
 353                 "Navcom: received packet type 0x15 (Negative Acknowledge)\n");
 354     for (n = 4; n < (msg_len - 2); n += 2) {
 355         uint8_t err_id = getub(buf, n);
 356         uint8_t err_desc = getub(buf, n + 1);
 357         gpsd_report(LOG_IO,
 358                     "Navcom: error id = 0x%02x, error description = 0x%02x\n",
 359                     err_id, err_desc);
 360     }
 361     port = getub(buf, n);
 362     gpsd_report(LOG_IO,
 363                 "Navcom: negative acknowledge was for command id 0x%02x on port %c\n",
 364                 cmd_id, (port == 0 ? 'A' : (port == 1 ? 'B' : '?')));
 365     /*@ -type @*/
 366     return 0;                   /* Nothing updated */
 367 }
 368
 369 /* PVT Block */
 370 static gps_mask_t handle_0xb1(struct gps_device_t *session)
 371 {
 372     gps_mask_t mask;
 373     unsigned int n;
 374     unsigned char *buf = session->packet.outbuffer + 3;
 375     uint16_t week;
 376     uint32_t tow;
 377     uint32_t sats_used;
 378     int32_t lat, lon;
 379     /* Resolution of lat/lon values (2^-11) */
 380 #define LL_RES (0.00048828125)
 381     uint8_t lat_fraction, lon_fraction;
 382     /* Resolution of lat/lon fractions (2^-15) */
 383 #define LL_FRAC_RES (0.000030517578125)
 384     uint8_t nav_mode;
 385     int32_t ellips_height, altitude;
 386     /* Resolution of height and altitude values (2.0^-10) */
 387 #define EL_RES (0.0009765625)
 388     double vel_north, vel_east, vel_up;
 389     /* Resolution of velocity values (2.0^-10) */
 390 #define VEL_RES (0.0009765625)
 391     double track;
 392     uint8_t fom, gdop, pdop, hdop, vdop, tdop, tfom;
 393     double eph;
 394     /* This value means "undefined" */
 395 #define DOP_UNDEFINED (255)
 396
 397     int16_t ant_height_adj;
 398     int32_t set_delta_up;
 399     /* Resolution of delta north, east, and up,
 400      * and ant. height adjustment values (1mm) */
 401 #define D_RES (0.001)
 402
 403 #ifdef __UNUSED__
 404     /* Other values provided by the PVT block which we
 405      * may want to provide in the future.  At the present
 406      * moment, the gpsd protocol does not have a mechanism
 407      * to make this available to the user */
 408     uint8_t dgps_conf;
 409     uint16_t max_dgps_age;
 410     uint8_t ext_nav_mode;
 411     int32_t set_delta_north, set_delta_east;
 412     uint8_t nav_failure_code;
 413 #endif /* __UNUSED__ */
 414
 415     /* Timestamp */
 416     week = (uint16_t) getleuw(buf, 3);
 417     session->context->gps_week = week;
 418     tow = (uint32_t) getleul(buf, 5);
 419     session->context->gps_tow = tow / 1000.0;
 420     session->newdata.time =
 421         gpstime_to_unix((int)week, session->context->gps_tow)
 422         - session->context->leap_seconds;
 423
 424     /* Satellites used */
 425     sats_used = (uint32_t) getleul(buf, 9);
 426     session->gpsdata.satellites_used = 0;
 427     for (n = 0; n < 31; n++) {
 428         if ((sats_used & (0x01 << n)) != 0)
 429             session->gpsdata.used[session->gpsdata.satellites_used++] =
 430                 (int)(n + 1);
 431     }
 432
 433     /* Get latitude, longitude */
 434     lat = getlesl(buf, 13);
 435     lon = getlesl(buf, 17);
 436     lat_fraction = (uint8_t) (getub(buf, 21) >> 4);
 437     lon_fraction = (uint8_t) (getub(buf, 21) & 0x0f);
 438
 439     session->newdata.latitude =
 440         (double)(lat * LL_RES + lat_fraction * LL_FRAC_RES) / 3600;
 441     session->newdata.longitude =
 442         (double)(lon * LL_RES + lon_fraction * LL_FRAC_RES) / 3600;
 443
 444     /* Nav mode */
 445     nav_mode = (uint8_t) getub(buf, 22);
 446     if (-nav_mode & 0x80) {
 447         session->gpsdata.status = STATUS_NO_FIX;
 448         session->newdata.mode = MODE_NO_FIX;
 449     } else {
 450         session->newdata.mode = (nav_mode & 0x40 ? MODE_3D : MODE_2D);
 451         session->gpsdata.status =
 452             (nav_mode & 0x03 ? STATUS_DGPS_FIX : STATUS_FIX);
 453     }
 454
 455     /* Height Data */
 456     ellips_height = getlesl(buf, 23);
 457     altitude = getlesl(buf, 27);
 458
 459     ant_height_adj = getlesw(buf, 51);
 460     set_delta_up = getlesl(buf, 79);
 461
 462     session->newdata.altitude = (double)(altitude * EL_RES)
 463         + (ant_height_adj * D_RES) + (set_delta_up * D_RES);
 464     session->gpsdata.separation = (double)(ellips_height - altitude) * EL_RES
 465         + (ant_height_adj * D_RES) + (set_delta_up * D_RES);
 466
 467     /* Speed Data */
 468     vel_north = (double)getlesl24(buf, 31);
 469     vel_east = (double)getlesl24(buf, 34);
 470     /* vel_up = getlesl24(buf, 37); */
 471     vel_up = (double)getlesl24(buf, 37);
 472
 473     track = atan2(vel_east, vel_north);
 474     if (track < 0)
 475         track += 2 * GPS_PI;
 476     session->newdata.track = track * RAD_2_DEG;
 477     /*@ -evalorder @*/
 478     session->newdata.speed =
 479         sqrt(pow(vel_east, 2) + pow(vel_north, 2)) * VEL_RES;
 480     /*@ +evalorder @*/
 481     session->newdata.climb = vel_up * VEL_RES;
 482
 483     /* Quality indicators */
 484     /*@ -type @*/
 485     fom = getub(buf, 40);
 486     gdop = getub(buf, 41);
 487     pdop = getub(buf, 42);
 488     hdop = getub(buf, 43);
 489     vdop = getub(buf, 44);
 490     tdop = getub(buf, 45);
 491     tfom = getub(buf, 46);
 492
 493     /* Get two-sigma horizontal circular error estimate */
 494     eph = fom / 100.0 * 1.96;
 495     /* approximate epx and epy errors from it */
 496     session->newdata.epx = session->newdata.epy = eph / sqrt(2);
 497     session->newdata.ept = tfom * 1.96 /*Two sigma */ ;
 498
 499     if (gdop != DOP_UNDEFINED)
 500         session->gpsdata.dop.gdop = gdop / 10.0;
 501     if (pdop != DOP_UNDEFINED)
 502         session->gpsdata.dop.pdop = pdop / 10.0;
 503     if (hdop != DOP_UNDEFINED)
 504         session->gpsdata.dop.hdop = hdop / 10.0;
 505     if (vdop != DOP_UNDEFINED)
 506         session->gpsdata.dop.vdop = vdop / 10.0;
 507     if (tdop != DOP_UNDEFINED)
 508         session->gpsdata.dop.tdop = tdop / 10.0;
 509     /*@ +type @*/
 510
 511     gpsd_report(LOG_PROG, "Navcom: received packet type 0xb1 (PVT Report)\n");
 512     gpsd_report(LOG_IO, "Navcom: navigation mode %s (0x%02x) - %s - %s\n",
 513                 (-nav_mode & 0x80 ? "invalid" : "valid"), nav_mode,
 514                 (nav_mode & 0x40 ? "3D" : "2D"),
 515                 (nav_mode & 0x03 ? "DGPS" : "GPS"));
 516     gpsd_report(LOG_IO,
 517                 "Navcom: latitude = %f, longitude = %f, altitude = %f, geoid = %f\n",
 518                 session->newdata.latitude, session->newdata.longitude,
 519                 session->newdata.altitude, session->gpsdata.separation);
 520     gpsd_report(LOG_IO,
 521                 "Navcom: velocities: north = %f, east = %f, up = %f (track = %f, speed = %f)\n",
 522                 vel_north * VEL_RES, vel_east * VEL_RES, vel_up * VEL_RES,
 523                 session->newdata.track, session->newdata.speed);
 524 #undef D_RES
 525 #undef LL_RES
 526 #undef LL_FRAC_RES
 527 #undef EL_RES
 528 #undef VEL_RES
 529 #undef DOP_UNDEFINED
 530
 531     mask = LATLON_IS | ALTITUDE_IS | CLIMB_IS | SPEED_IS | TRACK_IS
 532         | TIME_IS | STATUS_IS | MODE_IS | USED_IS | HERR_IS | VERR_IS
 533         | TIMERR_IS | DOP_IS;
 534     gpsd_report(LOG_DATA, "PVT 0xb1: time=%.2f, lat=%.2f lon=%.2f alt=%.f "
 535                 "speed=%.2f track=%.2f climb=%.2f mode=%d status=%d "
 536                 "epx=%.2f epy=%.2f epv=%.2f "
 537                 "gdop=%.2f pdop=%.2f hdop=%.2f vdop=%.2f tdop=%.2f "
 538                 "mask={LATLON|ALTITUDE|CLIMB|SPEED|TRACK|TIME|STATUS|MODE|"
 539                 "USED|HERR|VERR|TIMERR|DOP}\n",
 540                 session->newdata.time,
 541                 session->newdata.latitude,
 542                 session->newdata.longitude,
 543                 session->newdata.altitude,
 544                 session->newdata.speed,
 545                 session->newdata.track,
 546                 session->newdata.climb,
 547                 session->newdata.mode,
 548                 session->gpsdata.status,
 549                 session->newdata.epx,
 550                 session->newdata.epy,
 551                 session->newdata.epv,
 552                 session->gpsdata.dop.gdop,
 553                 session->gpsdata.dop.pdop,
 554                 session->gpsdata.dop.hdop,
 555                 session->gpsdata.dop.vdop, session->gpsdata.dop.tdop);
 556     return mask;
 557 }
 558
 559 /* Packed Ephemeris Data */
 560 static gps_mask_t handle_0x81(struct gps_device_t *session)
 561 {
 562     /* Scale factors for everything */
 563     /* 2^-31 */
 564 #define SF_TGD       (.000000000465661287307739257812)
 565     /* 2^4 */
 566 #define SF_TOC     (16)
 567     /* 2^-55 */
 568 #define SF_AF2       (.000000000000000027755575615628)
 569     /* 2^-43 */
 570 #define SF_AF1       (.000000000000113686837721616029)
 571     /* 2^-31 */
 572 #define SF_AF0       (.000000000465661287307739257812)
 573     /* 2^-5 */
 574 #define SF_CRS       (.031250000000000000000000000000)
 575     /* 2^-43 */
 576 #define SF_DELTA_N   (.000000000000113686837721616029)
 577     /* 2^-31 */
 578 #define SF_M0   (.000000000465661287307739257812)
 579     /* 2^-29 */
 580 #define SF_CUC       (.000000001862645149230957031250)
 581     /* 2^-33 */
 582 #define SF_E     (.000000000116415321826934814453)
 583     /* 2^-29 */
 584 #define SF_CUS       (.000000001862645149230957031250)
 585     /* 2^-19 */
 586 #define SF_SQRT_A    (.000001907348632812500000000000)
 587     /* 2^4 */
 588 #define SF_TOE     (16)
 589     /* 2^-29 */
 590 #define SF_CIC       (.000000001862645149230957031250)
 591     /* 2^-31 */
 592 #define SF_OMEGA0    (.000000000465661287307739257812)
 593     /* 2^-29 */
 594 #define SF_CIS       (.000000001862645149230957031250)
 595     /* 2^-31 */
 596 #define SF_I0   (.000000000465661287307739257812)
 597     /* 2^-5 */
 598 #define SF_CRC       (.031250000000000000000000000000)
 599     /* 2^-31 */
 600 #define SF_OMEGA     (.000000000465661287307739257812)
 601     /* 2^-43 */
 602 #define SF_OMEGADOT  (.000000000000113686837721616029)
 603     /* 2^-43 */
 604 #define SF_IDOT      (.000000000000113686837721616029)
 605
 606     unsigned char *buf = session->packet.outbuffer + 3;
 607     uint8_t prn = getub(buf, 3);
 608     uint16_t week = getleuw(buf, 4);
 609     uint32_t tow = getleul(buf, 6);
 610     uint16_t iodc = getleuw(buf, 10);
 611     /* And now the fun starts... everything that follows is
 612      * raw GPS data minus parity */
 613     /* Subframe 1, words 3 to 10 minus parity */
 614     uint16_t wn = (getleuw_be(buf, 12) & 0xffc0) >> 6;
 615     uint8_t cl2 = (getub(buf, 13) & 0x30) >> 4;
 616     uint8_t ura = getub(buf, 13) & 0x0f;
 617     uint8_t svh = (getub(buf, 14) & 0xfc) >> 2;
 618     /* We already have IODC from earlier in the message, so
 619      * we do not decode again */
 620 /*    uint16_t iodc = (getub(buf, 14)&0x03)<<8;*/
 621     uint8_t l2pd = (getub(buf, 15) & 0x80) >> 7;
 622     int8_t tgd = getsb(buf, 26);
 623 /*    iodc |= getub(buf, 27);*/
 624     uint16_t toc = getleuw_be(buf, 28);
 625     int8_t af2 = getsb(buf, 30);
 626     int16_t af1 = getlesw_be(buf, 31);
 627     /*@ -shiftimplementation @*/
 628     int32_t af0 = getlesl24_be(buf, 33) >> 2;
 629     /*@ +shiftimplementation @*/
 630     /* Subframe 2, words 3 to 10 minus parity */
 631     uint8_t iode = getub(buf, 36);
 632     int16_t crs = getlesw_be(buf, 37);
 633     int16_t delta_n = getlesw_be(buf, 39);
 634     int32_t m0 = getlesl_be(buf, 41);
 635     int16_t cuc = getlesw_be(buf, 45);
 636     uint32_t e = getleul_be(buf, 47);
 637     int16_t cus = getlesw_be(buf, 51);
 638     uint32_t sqrt_a = getleul_be(buf, 53);
 639     uint16_t toe = getleuw_be(buf, 57);
 640     /* NOTE - Fit interval & AODO not collected */
 641     /* Subframe 3, words 3 to 10 minus parity */
 642     int16_t cic = getlesw_be(buf, 60);
 643     int32_t Omega0 = getlesl_be(buf, 62);
 644     int16_t cis = getlesw_be(buf, 66);
 645     int32_t i0 = getlesl_be(buf, 68);
 646     int16_t crc = getlesw_be(buf, 72);
 647     int32_t omega = getlesl_be(buf, 74);
 648     int32_t Omegadot = getlesl24_be(buf, 78);
 649     /*@ -predboolothers @*/
 650     /* Question: What is the proper way of shifting a signed int 2 bits to
 651      * the right, preserving sign? Answer: integer division by 4. */
 652     int16_t idot =
 653         (int16_t) (((getlesw_be(buf, 82) & 0xfffc) /
 654                     4) | (getub(buf, 82) & 80 ? 0xc000 : 0x0000));
 655     /*@ +predboolothers @*/
 656     char time_str[24];
 657     session->context->gps_week = (unsigned short)wn;
 658     session->context->gps_tow = (double)(toc * SF_TOC);
 659     /* leap second? */
 660     (void)unix_to_iso8601(gpstime_to_unix((int)wn, session->context->gps_tow),
 661                           time_str, sizeof(time_str));
 662
 663     gpsd_report(LOG_PROG,
 664                 "Navcom: received packet type 0x81 (Packed Ephemeris Data)\n");
 665     gpsd_report(LOG_IO,
 666                 "Navcom: PRN: %u, Epoch: %u (%s), SV clock bias/drift/drift rate: %#19.12E/%#19.12E/%#19.12E\n",
 667                 prn, toc * SF_TOC, time_str, ((double)af0) * SF_AF0,
 668                 ((double)af1) * SF_AF1, ((double)af2) * SF_AF2);
 669     gpsd_report(LOG_IO,
 670                 "Navcom: IODE (!AODE): %u Crs: %19.12e, Delta n: %19.12e, M0: %19.12e\n",
 671                 iode, (double)crs * SF_CRS,
 672                 (double)delta_n * SF_DELTA_N * GPS_PI,
 673                 (double)m0 * SF_M0 * GPS_PI);
 674     gpsd_report(LOG_IO,
 675                 "Navcom: Cuc: %19.12e, Eccentricity: %19.12e, Cus: %19.12e, A^1/2: %19.12e\n",
 676                 (double)cuc * SF_CUC, (double)e * SF_E, (double)cus * SF_CUS,
 677                 (double)sqrt_a * SF_SQRT_A);
 678     gpsd_report(LOG_IO,
 679                 "Navcom: TOE: %u, Cic: %19.12e, Omega %19.12e, Cis: %19.12e\n",
 680                 toe * SF_TOE, (double)cic * SF_CIC,
 681                 (double)Omega0 * SF_OMEGA0 * GPS_PI, (double)cis * SF_CIS);
 682     gpsd_report(LOG_IO,
 683                 "Navcom: i0: %19.12e, Crc: %19.12e, omega: %19.12e, Omega dot: %19.12e\n",
 684                 (double)i0 * SF_I0 * GPS_PI, (double)crc * SF_CRC,
 685                 (double)omega * SF_OMEGA * GPS_PI,
 686                 (double)Omegadot * SF_OMEGADOT * GPS_PI);
 687     gpsd_report(LOG_IO,
 688                 "Navcom: IDOT: %19.12e, Codes on L2: 0x%x, GPS Week: %u, L2 P data flag: %x\n",
 689                 (double)idot * SF_IDOT * GPS_PI, cl2,
 690                 week - (week % 1024) + wn, l2pd);
 691     gpsd_report(LOG_IO,
 692                 "Navcom: SV accuracy: 0x%x, SV health: 0x%x, TGD: %f, IODC (!AODC): %u\n",
 693                 ura, svh, (double)tgd * SF_TGD, iodc);
 694     gpsd_report(LOG_IO, "Navcom: Transmission time: %u\n", tow);
 695
 696 #undef SF_TGD
 697 #undef SF_TOC
 698 #undef SF_AF2
 699 #undef SF_AF1
 700 #undef SF_AF0
 701 #undef SF_CRS
 702 #undef SF_DELTA_N
 703 #undef SF_M0
 704 #undef SF_CUC
 705 #undef SF_E
 706 #undef SF_CUS
 707 #undef SF_SQRT_A
 708 #undef SF_TOE
 709 #undef SF_CIC
 710 #undef SF_OMEGA0
 711 #undef SF_CIS
 712 #undef SF_I0
 713 #undef SF_CRC
 714 #undef SF_OMEGA
 715 #undef SF_OMEGADOT
 716 #undef SF_IDOT
 717
 718     return 0;
 719 }
 720
 721 /* Channel Status */
 722 static gps_mask_t handle_0x86(struct gps_device_t *session)
 723 {
 724     size_t n, i;
 725     uint8_t prn, tracking_status, ele, ca_snr, p2_snr, log_channel,
 726         hw_channel, s;
 727     uint16_t azm, dgps_age;
 728     unsigned char *buf = session->packet.outbuffer + 3;
 729     size_t msg_len = (size_t) getleuw(buf, 1);
 730     uint16_t week = getleuw(buf, 3);
 731     uint32_t tow = getleul(buf, 5);
 732     uint8_t eng_status = getub(buf, 9);
 733     uint16_t sol_status = getleuw(buf, 10);
 734     uint8_t sats_visible = getub(buf, 12);
 735     //uint8_t sats_tracked = getub(buf, 13);
 736     uint8_t sats_used = getub(buf, 14);
 737     //uint8_t pdop = getub(buf, 15);
 738
 739     /* Timestamp and PDOP */
 740     session->context->gps_week = (unsigned short)week;
 741     session->context->gps_tow = (double)tow / 1000.0f;
 742     /*@ ignore @*//*@ splint is confused @ */
 743     session->gpsdata.skyview_time =
 744         gpstime_to_unix((int)week, session->context->gps_tow)
 745         - session->context->leap_seconds;
 746     /*@ end @*/
 747     /* Give this driver a single point of truth about DOPs */
 748     //session->gpsdata.dop.pdop = (int)pdop / 10.0;
 749
 750     /* Satellite count */
 751     session->gpsdata.satellites_visible = (int)sats_visible;
 752     session->gpsdata.satellites_used = (int)sats_used;
 753
 754     /* Fix mode */
 755     switch (sol_status & 0x05) {
 756     case 0x05:
 757         session->gpsdata.status = STATUS_DGPS_FIX;
 758         break;
 759     case 0x01:
 760         session->gpsdata.status = STATUS_FIX;
 761         break;
 762     default:
 763         session->gpsdata.status = STATUS_NO_FIX;
 764     }
 765
 766     /*@ -predboolothers @*/
 767     gpsd_report(LOG_IO,
 768                 "Navcom: engine status = 0x%x, almanac = %s, time = 0x%x, pos = 0x%x\n",
 769                 eng_status & 0x07, (eng_status & 0x08 ? "valid" : "invalid"),
 770                 eng_status & 0x30 >> 4, eng_status & 0xc0 >> 6);
 771     /*@ +predboolothers @*/
 772
 773     /* Satellite details */
 774     i = 0;
 775     for (n = 17; n < msg_len; n += 14) {
 776         if (i >= MAXCHANNELS) {
 777             gpsd_report(LOG_ERROR,
 778                         "Navcom: packet type 0x86: too many satellites!\n");
 779             gpsd_zero_satellites(&session->gpsdata);
 780             return 0;
 781         }
 782         prn = getub(buf, n);
 783         tracking_status = getub(buf, n + 1);
 784         log_channel = getub(buf, n + 2);
 785         ele = getub(buf, n + 5);
 786         azm = getleuw(buf, n + 6);
 787         ca_snr = getub(buf, n + 8);
 788         p2_snr = getub(buf, n + 10);
 789         dgps_age = getleuw(buf, n + 11);
 790         hw_channel = getub(buf, n + 13);
 791         s = (unsigned char)0;
 792         /*@ -predboolothers +charint @*/
 793         /* NOTE - In theory, I think one would check for hw channel number to
 794          * see if one is dealing with a GPS or other satellite, but the
 795          * channel numbers reported bear no resemblance to what the spec
 796          * says should be.  So I check for the fact that if all three
 797          * values below are zero, one is not interested on this satellite */
 798         if (!(ele == 0 && azm == 0 && dgps_age == 0)) {
 799             session->gpsdata.PRN[i] = (int)prn;
 800             session->gpsdata.elevation[i] = (int)ele;
 801             session->gpsdata.azimuth[i] = (int)azm;
 802             /*@ ignore @*//* splint is confused */
 803             s = session->gpsdata.ss[i++] = (p2_snr ? p2_snr : ca_snr) / 4.0;
 804             /*@ end @*/
 805         }
 806         gpsd_report(LOG_IO,
 807                     "Navcom: prn = %3u, ele = %02u, azm = %03u, snr = %d (%s), "
 808                     "dgps age = %.1fs, log ch = %d, hw ch = 0x%02x\n",
 809                     prn, ele, azm, s, (p2_snr ? "P2" : "C/A"),
 810                     (double)dgps_age * 0.1, log_channel & 0x3f, hw_channel);
 811         gpsd_report(LOG_IO,
 812                     "Navcom:        sol. valid = %c, clock = %s, pos. = %s, "
 813                     "height = %s, err. code = 0x%x\n",
 814                     (sol_status & 0x01 ? 'Y' : 'N'),
 815                     (sol_status & 0x02 ? "stable" : "unstable"),
 816                     (sol_status & 0x04 ? "dgps" : "unaided"),
 817                     (sol_status & 0x08 ? "solved" : "constrained"),
 818                     (sol_status & 0x01 ? 0x00 : sol_status & 0x0f00 >> 8));
 819         /*@ +predboolothers -charint @*/
 820     }
 821
 822     gpsd_report(LOG_DATA,
 823                 "CS 0x86: visible=%d, used=%d, mask={SATELLITE|STATUS}\n",
 824                 session->gpsdata.satellites_visible,
 825                 session->gpsdata.satellites_used);
 826     return SATELLITE_IS | STATUS_IS;
 827 }
 828
 829 /* Raw Meas. Data Block */
 830 static gps_mask_t handle_0xb0(struct gps_device_t *session)
 831 {
 832     /* L1 wavelength (299792458m/s / 1575420000Hz) */
 833 #define LAMBDA_L1 (.190293672798364880476317426464)
 834     size_t n;
 835     unsigned char *buf = session->packet.outbuffer + 3;
 836     size_t msg_len = (size_t) getleuw(buf, 1);
 837     uint16_t week = getleuw(buf, 3);
 838     uint32_t tow = getleul(buf, 5);
 839     uint8_t tm_slew_acc = getub(buf, 9);
 840     uint8_t status = getub(buf, 10);
 841
 842     char time_str[24];
 843     session->context->gps_week = (unsigned short)week;
 844     session->context->gps_tow = (double)tow / 1000.0;
 845     (void)
 846         unix_to_iso8601(gpstime_to_unix((int)week, session->context->gps_tow),
 847                         time_str, sizeof(time_str));
 848
 849     gpsd_report(LOG_PROG,
 850                 "Navcom: received packet type 0xb0 (Raw Meas. Data Block)\n");
 851     /*@ -predboolothers @*/
 852     gpsd_report(LOG_IO,
 853                 "Navcom: Epoch = %s, time slew accumulator = %u (1/1023mS), status = 0x%02x "
 854                 "(%sclock %s - %u blocks follow)\n",
 855                 time_str,
 856                 tm_slew_acc, status,
 857                 (status & 0x80 ? "channel time set - " : ""),
 858                 (status & 0x40 ? "stable" : "not stable"), status & 0x0f);
 859     /*@ +predboolothers @*/
 860     for (n = 11; n < msg_len - 1; n += 16) {
 861         uint8_t sv_status = getub(buf, n);
 862         uint8_t ch_status = getub(buf, n + 1);
 863         uint32_t ca_pseudorange = getleul(buf, n + 2);
 864         /* integer division by 16 is a sign-preserving right shift of 4 bits */
 865         int32_t l1_phase = getlesl24(buf, n + 6) / 16;
 866         uint8_t l1_slips = (uint8_t) (getlesl24(buf, n + 6) & 0x0f);
 867         int16_t p1_ca_pseudorange = getlesw(buf, n + 9);
 868         int16_t p2_ca_pseudorange = getlesw(buf, n + 11);
 869         int32_t l2_phase = getlesl24(buf, n + 13) / 16;
 870         uint8_t l2_slips = (uint8_t) (getlesl24(buf, n + 13) & 0x0f);
 871         /*@ -predboolothers +charint @*/
 872         double c1 =
 873             (sv_status & 0x80 ? (double)ca_pseudorange / 16.0 *
 874              LAMBDA_L1 : NAN);
 875         double l1 =
 876             (sv_status & 0x80 ? (double)ca_pseudorange / 16.0 +
 877              (double)l1_phase / 256.0 : NAN);
 878         double l2 =
 879             (sv_status & 0x20
 880              ? ((double)ca_pseudorange / 16.0 +
 881                 (double)p2_ca_pseudorange / 16.0) * (120.0 / 154.0)
 882              + (double)l2_phase / 256.0 : NAN);
 883         double p1 =
 884             (sv_status & 0x40 ? c1 +
 885              (double)p1_ca_pseudorange / 16.0 * LAMBDA_L1 : NAN);
 886         double p2 =
 887             (sv_status & 0x20 ? c1 +
 888              (double)p2_ca_pseudorange / 16.0 * LAMBDA_L1 : NAN);
 889         gpsd_report(LOG_IO + 1,
 890                     "Navcom: >> sv status = 0x%02x (PRN %u - C/A & L1 %s - P1 %s - P2 & L2 %s)\n",
 891                     sv_status, (sv_status & 0x1f),
 892                     (sv_status & 0x80 ? "valid" : "invalid"),
 893                     (sv_status & 0x40 ? "valid" : "invalid"),
 894                     (sv_status & 0x20 ? "valid" : "invalid"));
 895         gpsd_report(LOG_IO + 1,
 896                     "Navcom: >>> ch status = 0x%02x (Logical channel: %u - CA C/No: %u dBHz) "
 897                     "sL1: %u, sL2: %u\n", ch_status, ch_status & 0x0f,
 898                     ((ch_status & 0xf0) >> 4) + 35, l1_slips, l2_slips);
 899         gpsd_report(LOG_IO + 1,
 900                     "Navcom: >>> C1: %14.3f, L1: %14.3f, L2: %14.3f, P1: %14.3f, P2: %14.3f\n",
 901                     c1, l1, l2, p1, p2);
 902         /*@ +predboolothers -charint @*/
 903     }
 904 #undef LAMBDA_L1
 905     return 0;                   /* Raw measurements not yet implemented in gpsd */
 906 }
 907
 908 /* Pseudorange Noise Statistics */
 909 static gps_mask_t handle_0xb5(struct gps_device_t *session)
 910 {
 911     if (sizeof(double) == 8) {
 912         gps_mask_t mask = TIME_IS;
 913         union long_double l_d;
 914         unsigned char *buf = session->packet.outbuffer + 3;
 915         uint16_t week = getleuw(buf, 3);
 916         uint32_t tow = getleul(buf, 5);
 917         double rms = getled(buf, 9);
 918 #ifdef __UNUSED__
 919         /* Reason why it's unused is these figures do not agree
 920          * with those obtained from the PVT report (handle_0xb1).
 921          * The figures from 0xb1 do agree with the values reported
 922          * by Navcom's PC utility */
 923         double ellips_maj = getled(buf, 17);
 924         double ellips_min = getled(buf, 25);
 925         double ellips_azm = getled(buf, 33);
 926         double lat_sd = getled(buf, 41);
 927         double lon_sd = getled(buf, 49);
 928         double alt_sd = getled(buf, 57);
 929         double hrms = sqrt(pow(lat_sd, 2) + pow(lon_sd, 2));
 930 #endif /*  __UNUSED__ */
 931         session->gpsdata.epe = rms * 1.96;
 932         mask |= PERR_IS;
 933 #ifdef __UNUSED__
 934         session->newdata.eph = hrms * 1.96;
 935         session->newdata.epv = alt_sd * 1.96;
 936         mask |= (HERR_IS | VERR_IS);
 937 #endif /*  __UNUSED__ */
 938         session->context->gps_week = (unsigned short)week;
 939         session->context->gps_tow = (double)tow / 1000.0f;
 940         /*@ ignore @*//*@ splint is confused @ */
 941         session->newdata.time =
 942             gpstime_to_unix((int)week, session->context->gps_tow)
 943             - session->context->leap_seconds;
 944         /*@ end @*/
 945         gpsd_report(LOG_PROG,
 946                     "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics)\n");
 947         gpsd_report(LOG_IO, "Navcom: epe = %f\n", session->gpsdata.epe);
 948         return mask;
 949     } else {
 950         /* Ignore this message block */
 951         if (!session->driver.navcom.warned) {
 952             gpsd_report(LOG_WARN,
 953                         "Navcom: received packet type 0xb5 (Pseudorange Noise Statistics) ignored "
 954                         " - sizeof(double) == 64 bits required\n");
 955             session->driver.navcom.warned = true;
 956         }
 957         return 0;               /* Block ignored - wrong sizeof(double) */
 958     }
 959 }
 960
 961 /* LBM DSP Status Block */
 962 static gps_mask_t handle_0xd3(struct gps_device_t *session UNUSED)
 963 {
 964     /* This block contains status information about the
 965      * unit's L-band (Inmarsat) module.  There is nothing
 966      * interesting in it for our purposes so we do not deal
 967      * with it.  This callback is purely to a) stop
 968      * "unrecognised packet" messages appearing in the log
 969      * and b) explain what it is for the curious */
 970     return 0;                   /* Nothing updated */
 971 }
 972
 973 /* Identification Block */
 974 static gps_mask_t handle_0xae(struct gps_device_t *session)
 975 {
 976     /*@-modobserver@*/
 977     char *engconfstr, *asicstr;
 978     unsigned char *buf = session->packet.outbuffer + 3;
 979     size_t msg_len = (size_t) getleuw(buf, 1);
 980     uint8_t engconf = getub(buf, 3);
 981     uint8_t asic = getub(buf, 4);
 982     uint8_t swvermaj = getub(buf, 5);
 983     uint8_t swvermin = getub(buf, 6);
 984     uint16_t dcser = getleuw(buf, 7);
 985     uint8_t dcclass = getub(buf, 9);
 986     uint16_t rfcser = getleuw(buf, 10);
 987     uint8_t rfcclass = getub(buf, 12);
 988     /*@ -stringliteralnoroomfinalnull -type @*/
 989     uint8_t softtm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
 990     uint8_t bootstr[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
 991     uint8_t ioptm[17] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
 992     /*@ +stringliteralnoroomfinalnull +type @*/
 993     uint8_t iopvermaj = (uint8_t) 0x00;
 994     uint8_t iopvermin = (uint8_t) 0x00;
 995     uint8_t picver = (uint8_t) 0x00;
 996     uint8_t slsbn = (uint8_t) 0x00;
 997     uint8_t iopsbn = (uint8_t) 0x00;
 998     memcpy(softtm, &buf[13], 16);
 999     memcpy(bootstr, &buf[29], 16);
1000     if (msg_len == 0x0037) {    /* No IOP */
1001         slsbn = getub(buf, 53);
1002     } else {                    /* IOP Present */
1003         iopvermaj = getub(buf, 53);
1004         iopvermin = getub(buf, 54);
1005         memcpy(ioptm, &buf[55], 16);
1006         picver = getub(buf, 71);
1007         slsbn = getub(buf, 72);
1008         iopsbn = getub(buf, 73);
1009     }
1010
1011     switch (engconf) {
1012     case 0x00:
1013         engconfstr = "Unknown/Undefined";
1014         break;
1015     case 0x01:
1016         engconfstr = "NCT 2000 S";
1017         break;
1018     case 0x02:
1019         engconfstr = "NCT 2000 D";
1020         break;
1021     case 0x03:
1022         engconfstr = "Startfire Single";
1023         break;
1024     case 0x04:
1025         engconfstr = "Starfire Dual";
1026         break;
1027     case 0x05:
1028         engconfstr = "Pole Mount RTK (Internal Radio)";
1029         break;
1030     case 0x06:
1031         engconfstr = "Pole Mount GIS (LBM)";
1032         break;
1033     case 0x07:
1034         engconfstr = "Black Box RTK (Internal Radio)";
1035         break;
1036     case 0x08:
1037         engconfstr = "Black Box GIS (LBM)";
1038         break;
1039     case 0x80:
1040         engconfstr = "R100";
1041         break;
1042     case 0x81:
1043         engconfstr = "R200";
1044         break;
1045     case 0x82:
1046         engconfstr = "R210";
1047         break;
1048     case 0x83:
1049         engconfstr = "R300";
1050         break;
1051     case 0x84:
1052         engconfstr = "R310";
1053         break;
1054     default:
1055         engconfstr = "?";
1056     }
1057
1058     switch (asic) {
1059     case 0x01:
1060         asicstr = "A-ASIC";
1061         break;
1062     case 0x02:
1063         asicstr = "B-ASIC";
1064         break;
1065     case 0x03:
1066         asicstr = "C-ASIC";
1067         break;
1068     case 0x04:
1069         asicstr = "M-ASIC";
1070         break;
1071     default:
1072         asicstr = "?";
1073     }
1074
1075     gpsd_report(LOG_PROG,
1076                 "Navcom: received packet type 0xae (Identification Block)\n");
1077     if (msg_len == 0x0037) {
1078         gpsd_report(LOG_INF, "Navcom: ID Data: "
1079                     "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, "
1080                     "Build ID: %s, Boot software: %s\n",
1081                     engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser,
1082                     dcclass, rfcser, rfcclass, softtm, bootstr);
1083     } else {
1084         gpsd_report(LOG_INF, "Navcom: ID Data: "
1085                     "%s %s Ver. %u.%u.%u, DC S/N: %u.%u, RF S/N: %u.%u, "
1086                     "Build ID: %s, Boot software: %s, "
1087                     "IOP Ver.: %u.%u.%u, PIC: %u, IOP Build ID: %s\n",
1088                     engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser,
1089                     dcclass, rfcser, rfcclass, softtm, bootstr, iopvermaj,
1090                     iopvermin, iopsbn, picver, ioptm);
1091     }
1092
1093     /*@ -formattype @*/
1094     (void)snprintf(session->subtype, sizeof(session->subtype),
1095                    "%s %s Ver. %u.%u.%u S/N %u.%u %u.%u",
1096                    engconfstr, asicstr, swvermaj, swvermin, slsbn, dcser,
1097                    dcclass, rfcser, rfcclass);
1098     /*@ +formattype @*/
1099     return DEVICEID_IS;
1100     /*@+modobserver@*/
1101 }
1102
1103 /* Clock Drift and Offset */
1104 static gps_mask_t handle_0xef(struct gps_device_t *session)
1105 {
1106     unsigned char *buf = session->packet.outbuffer + 3;
1107     //uint16_t week = getleuw(buf, 3);
1108     //uint32_t tow = getleul(buf, 5);
1109     int8_t osc_temp = getsb(buf, 9);
1110     uint8_t nav_status = getub(buf, 10);
1111     union long_double l_d;
1112     double nav_clock_offset;
1113     union int_float i_f;
1114     float nav_clock_drift;
1115     float osc_filter_drift_est;
1116     int32_t time_slew = (int32_t) getlesl(buf, 27);
1117     if (sizeof(double) == 8) {
1118         nav_clock_offset = getled(buf, 11);
1119     } else {
1120         nav_clock_offset = NAN;
1121     }
1122     if (sizeof(float) == 4) {
1123         nav_clock_drift = getlef(buf, 19);
1124         osc_filter_drift_est = getlef(buf, 23);
1125     } else {
1126         nav_clock_drift = NAN;
1127         osc_filter_drift_est = NAN;
1128     }
1129
1130     //gpstime_to_unix((int)week, tow/1000.0) - session->context->leap_seconds;
1131
1132     gpsd_report(LOG_IO,
1133                 "Navcom: oscillator temp. = %d, nav. status = 0x%02x, "
1134                 "nav. clock offset = %f, nav. clock drift = %f, "
1135                 "osc. filter drift est. = %f, acc.time slew value = %d\n",
1136                 osc_temp, nav_status, nav_clock_offset, nav_clock_drift,
1137                 osc_filter_drift_est, time_slew);
1138     gpsd_report(LOG_DATA,
1139                 "CDO 0xef: time=%.2f mask={TIME}\n", session->newdata.time);
1140     return 0;
1141 }
1142
1143
1144 /*@ +charint @*/
1145 gps_mask_t navcom_parse(struct gps_device_t * session, unsigned char *buf,
1146                         size_t len)
1147 {
1148     unsigned char cmd_id;
1149     unsigned char *payload;
1150     unsigned int msg_len;
1151
1152     if (len == 0)
1153         return 0;
1154
1155     cmd_id = (unsigned char)getub(buf, 3);
1156     payload = &buf[6];
1157     msg_len = (uint) getleuw(buf, 4);
1158
1159     /*@ -usedef -compdef @*/
1160     gpsd_report(LOG_RAW, "Navcom: packet type 0x%02x, length %d: %s\n",
1161                 cmd_id, msg_len, gpsd_hexdump_wrapper(buf, len, LOG_RAW));
1162     /*@ +usedef +compdef @*/
1163
1164     (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag),
1165                    "0x%02x", cmd_id);
1166
1167     session->cycle_end_reliable = true;
1168
1169     switch (cmd_id) {
1170     case 0x06:
1171         return handle_0x06(session);
1172     case 0x15:
1173         return handle_0x15(session);
1174     case 0x81:
1175         return handle_0x81(session);
1176     case 0x83:
1177         return handle_0x83(session);
1178     case 0x86:
1179         return handle_0x86(session);
1180     case 0xae:
1181         return handle_0xae(session);
1182     case 0xb0:
1183         return handle_0xb0(session);
1184     case 0xb1:
1185         return handle_0xb1(session) | (CLEAR_IS | REPORT_IS);
1186     case 0xb5:
1187         return handle_0xb5(session);
1188     case 0xd3:
1189         return handle_0xd3(session);
1190     case 0xef:
1191         return handle_0xef(session);
1192     default:
1193         gpsd_report(LOG_PROG,
1194                     "Navcom: received packet type 0x%02x, length %d - unknown or unimplemented\n",
1195                     cmd_id, msg_len);
1196         return 0;
1197     }
1198 }
1199
1200 /*@ -charint @*/
1201
1202 static gps_mask_t navcom_parse_input(struct gps_device_t *session)
1203 {
1204     gps_mask_t st;
1205
1206     if (session->packet.type == NAVCOM_PACKET) {
1207         st = navcom_parse(session, session->packet.outbuffer,
1208                           session->packet.outbuflen);
1209         session->gpsdata.dev.driver_mode = MODE_BINARY; /* binary */
1210         return st;
1211 #ifdef NMEA_ENABLE
1212     } else if (session->packet.type == NMEA_PACKET) {
1213         st = nmea_parse((char *)session->packet.outbuffer, session);
1214         session->gpsdata.dev.driver_mode = MODE_NMEA;   /* NMEA */
1215         return st;
1216 #endif /* NMEA_ENABLE */
1217     } else
1218         return 0;
1219 }
1220
1221 #ifdef ALLOW_CONTROLSEND
1222 static ssize_t navcom_control_send(struct gps_device_t *session,
1223                                    char *buf, size_t len)
1224 {
1225     /*@ +ignoresigns -mayaliasunique @*/
1226     putbyte(session->msgbuf, 0, 0x02);
1227     putbyte(session->msgbuf, 1, 0x99);
1228     putbyte(session->msgbuf, 2, 0x66);
1229     putbyte(session->msgbuf, 3, buf[0]);        /* Cmd ID */
1230     putleword(session->msgbuf, 4, len + 4);     /* Length */
1231     memcpy(session->msgbuf, buf + 6, len - 1);
1232     putbyte(session->msgbuf, 6 + len,
1233             checksum((unsigned char *)session->msgbuf + 3, len + 5));
1234     putbyte(session->msgbuf, 7 + len, 0x03);
1235     session->msgbuflen = len + 9;
1236     /*@ -ignoresigns +mayaliasunique @*/
1237     gpsd_report(LOG_RAW, "Navcom: control dump: %s\n",
1238                 gpsd_hexdump_wrapper(session->msgbuf, session->msgbuflen,
1239                                      LOG_RAW));
1240     return gpsd_write(session, session->msgbuf, session->msgbuflen);
1241 }
1242 #endif /* ALLOW_CONTROLSEND */
1243
1244 #ifdef ALLOW_RECONFIGURE
1245 static bool navcom_speed(struct gps_device_t *session,
1246                          speed_t speed, char parity, int stopbits)
1247 {
1248     /* parity and stopbit switching aren't implemented */
1249     if (parity != session->gpsdata.dev.parity
1250         || stopbits != (int)session->gpsdata.dev.parity) {
1251         return false;
1252     } else {
1253         uint8_t port, port_selection;
1254         uint8_t baud;
1255         if (session->driver.navcom.physical_port == (uint8_t) 0xFF) {
1256             /* We still don't know which port we're connected to */
1257             return false;
1258         }
1259         /*@ +charint @*/
1260         switch (speed) {
1261             /* NOTE - The spec says that certain baud combinations
1262              * on ports A and B are not allowed, those are
1263              * 1200/115200, 2400/57600, and 2400/115200.
1264              * To try and minimise the possibility of those
1265              * occurring, we do not allow baud rates below
1266              * 4800.  We could also disallow 57600 and 115200
1267              * to totally prevent this, but I do not consider
1268              * that reasonable.  Finding which baud speed the
1269              * other port is set at would also be too much
1270              * trouble, so we do not do it. */
1271         case 4800:
1272             baud = 0x04;
1273             break;
1274         case 9600:
1275             baud = 0x06;
1276             break;
1277         case 19200:
1278             baud = 0x08;
1279             break;
1280         case 38400:
1281             baud = 0x0a;
1282             break;
1283         case 57600:
1284             baud = 0x0c;
1285             break;
1286         case 115200:
1287             baud = 0x0e;
1288             break;
1289         default:
1290             /* Unsupported speed */
1291             return false;
1292         }
1293         /*@ -charint @*/
1294
1295         /* Proceed to construct our message */
1296         port = session->driver.navcom.physical_port;
1297         /*@i1@*/ port_selection = (port ? port : (uint8_t) 0xff) | baud;
1298
1299         /* Send it off */
1300         navcom_cmd_0x11(session, port_selection);
1301
1302         /* And cheekily return true, even though we have
1303          * no way to know if the speed change succeeded
1304          * until and if we receive an ACK (message 0x06),
1305          * which will be at the new baud speed if the
1306          * command was successful.  Bottom line, the client
1307          * should requery gpsd to see if the new speed is
1308          * different than the old one */
1309         return true;
1310     }
1311 }
1312 #endif /* ALLOW_RECONFIGURE */
1313
1314 /* this is everything we export */
1315 /* *INDENT-OFF* */
1316 const struct gps_type_t navcom_binary =
1317 {
1318     .type_name      = "Navcom binary",          /* full name of type */
1319     .packet_type    = NAVCOM_PACKET,            /* lexer packet type */
1320     .trigger        = "\x02\x99\x66",           /* packet leader */
1321     .channels       = NAVCOM_CHANNELS,          /* 12 L1 + 12 L2 + 2 Inmarsat L-Band */
1322     .probe_detect   = NULL,                     /* no probe */
1323     .get_packet     = generic_get,              /* use generic one */
1324     .parse_packet   = navcom_parse_input,       /* parse message packets */
1325     .rtcm_writer    = pass_rtcm,                /* send RTCM data straight */
1326     .event_hook     = navcom_event_hook,        /* lifetime event handler */
1327 #ifdef ALLOW_RECONFIGURE
1328     .speed_switcher = navcom_speed,             /* we do change baud rates */
1329     .mode_switcher  = NULL,                     /* there is not a mode switcher */
1330     .rate_switcher  = NULL,                     /* no sample-rate switcher */
1331     .min_cycle      = 1,                        /* ignore, no rate switch */
1332 #endif /* ALLOW_RECONFIGURE */
1333 #ifdef ALLOW_CONTROLSEND
1334     .control_send   = navcom_control_send,      /* how to send a control string */
1335 #endif /* ALLOW_CONTROLSEND */
1336 #ifdef NTPSHM_ENABLE
1337     .ntp_offset     = NULL,             /* no method for NTP fudge factor */
1338 #endif /* NTPSHM_ ENABLE */
1339 };
1340 /* *INDENT-ON* */
1341
1342 #endif /* defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE) */