navcom.c

来自「gpsd, a popular GPS daemon.」· C语言 代码 · 共 1,249 行 · 第 1/4 页

/*
 * Driver for Navcom receivers using propietary NCT messages, a binary protocol.
 *
 * Vendor website: http://www.navcomtech.com/
 * Technical references: http://www.navcomtech.com/support/docs.cfm
 *
 * Tested with two SF-2040G models
 *
 * At this stage, this driver implements the following commands:
 *
 * 0x20: Data Request (tell the unit which responses you want)
 * 0x3f: LED Configuration (controls the front panel LEDs -- for testing)
 * 0x1c: Test Support Block (again, blinks the front panel lights)
 *
 * and it understands the following responses:
 *
 * 0x06: Acknowledgement (without error)
 * 0x15: Negative Acknowledge
 * 0x86: Channel Status
 * 0xae: Identification Block
 * 0xb0: Raw Meas. Data Block
 * 0xb1: PVT Block
 * 0xb5: Pseudorange Noise Statistics
 * 0xd3: LBM DSP Status Block
 * 0xef: Clock Drift and Offset
 *
 * FIXME - I'm not too sure of the way I have computed the vertical positional error
 *         I have used FOM as a scaling factor for VDOP, thusly VRMS = FOM/HDOP*VDOP
 *
 * By Diego Berge. Contact via web form at http://www.nippur.net/survey/xuc/contact
 * (the form is in Catalan, but you'll figure it out)
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <unistd.h>
#include <time.h>
#include <sys/types.h>
#include <inttypes.h>
#include <stdio.h>
#include "gpsd_config.h"
#include "gpsd.h"

#if defined(NAVCOM_ENABLE) && defined(BINARY_ENABLE)
#define LITTLE_ENDIAN_PROTOCOL
#include "bits.h"

/* Have data which is 24 bits long */
#define getsl24(buf,off)  (int32_t)(((u_int32_t)getub((buf), (off)+2)<<24 | (u_int32_t)getub((buf), (off)+1)<<16 | (u_int32_t)getub((buf), (off))<<8)>>8)
#define getul24(buf,off) (u_int32_t)(((u_int32_t)getub((buf), (off)+2)<<24 | (u_int32_t)getub((buf), (off)+1)<<16 | (u_int32_t)getub((buf), (off))<<8)>>8)

/* And just to be difficult, Navcom is little endian but the GPS data stream
   is big endian.  Some messages contain raw GPS data */
#define getsw_be(buf, off)	(int16_t)((((u_int16_t)getub(buf, (off)) << 8) \
                                    | (u_int16_t)getub(buf, (off)+1)))
#define getuw_be(buf, off)	(u_int16_t)((((u_int16_t)getub(buf, (off)) << 8) \
                                    | (u_int16_t)getub(buf, (off)+1)))
#define getsl_be(buf, off)	(int32_t)((((u_int16_t)getuw_be(buf, (off)) << 16) \
                                    | getuw_be(buf, (off)+2)))
#define getul_be(buf, off)	(u_int32_t)((((u_int16_t)getuw_be(buf, (off)) << 16) \
                                    | getuw_be(buf, (off)+2)))
#define getsL_be(buf, off)	(int64_t)((((u_int64_t)getul_be(buf, (off)) << 32) \
                                    | getul_be(buf, (off)+4)))
#define getuL_be(buf, off)	(u_int64_t)((((u_int64_t)getul_be(buf, (off)) << 32) \
                                    | getul_be(buf, (off)+4)))
#define getsl24_be(buf,off)     (int32_t)(((u_int32_t)getub((buf), (off))<<24 \
                                    | (u_int32_t)getub((buf), (off)+1)<<16 \
                                    | (u_int32_t)getub((buf), (off)+2)<<8)>>8)

#define NAVCOM_CHANNELS	12

static u_int8_t checksum(unsigned char *buf, size_t len)
{
    size_t n;
    u_int8_t csum = (u_int8_t)0x00;
    for(n = 0; n < len; n++)
      csum ^= buf[n];
    return csum;
}

static bool navcom_send_cmd(struct gps_device_t *session, unsigned char *cmd, size_t len)
{
    gpsd_report(LOG_RAW, "Navcom: command dump: %s\n", gpsd_hexdump(cmd, len));
    return (gpsd_write(session, cmd, len) == (ssize_t)len);
}

/* Data Request */
static void navcom_cmd_0x20(struct gps_device_t *session, u_int8_t block_id, u_int16_t rate)
{
    unsigned char msg[18];
    putbyte(msg, 0, 0x02);
    putbyte(msg, 1, 0x99);
    putbyte(msg, 2, 0x66);
    putbyte(msg, 3, 0x20);	/* Cmd ID */
    putword(msg, 4, 0x000e);	/* Length */
    putbyte(msg, 6, 0x00);	/* Action */
    putbyte(msg, 7, 0x01);      /* Count of blocks */
    putbyte(msg, 8, block_id);	/* Data Block ID */
    putbyte(msg, 9, 0x02);	/* Logical Ports */
    putword(msg, 10, rate);	/* Data rate */
    putbyte(msg, 12, 0x71);
    putbyte(msg, 13, 0x00);
    putword(msg, 14, 0x0000);
    putbyte(msg, 16, checksum(msg+3, 13));
    putbyte(msg, 17, 0x03);
    (void)navcom_send_cmd(session, msg, 18);
    gpsd_report(LOG_PROG,
                "Navcom: sent command 0x20 (Data Request) "
                "- data block id = %02x at rate %02x\n",
                block_id, rate);
}

/*@ unused @*/
/* Changes the LED settings in the receiver */
static void UNUSED navcom_cmd_0x3f(struct gps_device_t *session)
{
    unsigned char msg[12];
    putbyte(msg, 0, 0x02);
    putbyte(msg, 1, 0x99);
    putbyte(msg, 2, 0x66);
    putbyte(msg, 3, 0x3f);	/* Cmd ID */
    putword(msg, 4, 0x0008);
    putbyte(msg, 6, 0x01);	/* Action */
    putbyte(msg, 7, 0x00);	/* Reserved */
    putbyte(msg, 8, 0x02);	/* Link LED setting */
    putbyte(msg, 9, 0x0a);	/* Battery LED setting */
    putbyte(msg, 10, checksum(msg+3, 7));
    putbyte(msg, 11, 0x03);
    (void)navcom_send_cmd(session, msg, 12);
    gpsd_report(LOG_PROG,
                "Navcom: sent command 0x3f (LED Configuration Block)\n");
}

/* Test Support Block - Blinks the LEDs */
static void navcom_cmd_0x1c(struct gps_device_t *session, u_int8_t mode, u_int8_t length)
{
    unsigned char msg[12];
    putbyte(msg, 0, 0x02);
    putbyte(msg, 1, 0x99);
    putbyte(msg, 2, 0x66);
    putbyte(msg, 3, 0x1c);	/* Cmd ID */
    putword(msg, 4, 0x0008);
    putbyte(msg, 6, 0x04);      /* Use ACK/NAK */
    putbyte(msg, 7, mode);	/* 0x01 or 0x02 */
    putbyte(msg, 8, length);    /* Only if mode == 0x01 */
    putbyte(msg, 9, 0x00);
    putbyte(msg, 10, checksum(msg+3, 7));
    putbyte(msg, 11, 0x03);
    (void)navcom_send_cmd(session, msg, 12);
    gpsd_report(LOG_PROG,
                "Navcom: sent command 0x1c (Test Support Block)\n");
    gpsd_report(LOG_IO,
                "Navcom: command 0x1c mode = %02x, length = %u\n",
                mode, length);
}

/* Serial Port Configuration */
static void navcom_cmd_0x11(struct gps_device_t *session, u_int8_t port_selection)
{
    /* NOTE - We only allow changing one port at a time,
       although the message supports doing both at once. */
    unsigned char msg[12];
    putbyte(msg, 0, 0x02);
    putbyte(msg, 1, 0x99);
    putbyte(msg, 2, 0x66);
    putbyte(msg, 3, 0x11);	/* Cmd ID */
    putword(msg, 4, 0x0008);    /* Length */
    putbyte(msg, 6, 0x04);      /* Action - Use ACK/NAK) */
    putbyte(msg, 7, port_selection);
    putbyte(msg, 8, 0x00);      /* Reserved */
    putbyte(msg, 9, 0x00);      /* Reserved */
    putbyte(msg, 10, checksum(msg+3, 7));
    putbyte(msg, 11, 0x03);
    (void)navcom_send_cmd(session, msg, 12);
    gpsd_report(LOG_PROG,
                "Navcom: sent command 0x11 (Serial Port Configuration)\n");
    gpsd_report(LOG_IO,
                "Navcom: serial port selection: 0x%02x\n", port_selection);
}

static void navcom_probe_subtype(struct gps_device_t *session, unsigned int seq)
{
    /* Request the following messages: */
    if (seq==0) {
	/*@ +charint @*/
        navcom_cmd_0x1c(session, 0x01, 5);      /* Blink LEDs on receiver */
        navcom_cmd_0x20(session, 0xae, 0x1770); /* Identification Block - send every 10 min*/
        navcom_cmd_0x20(session, 0xb1, 0x4000); /* PVT Block */
        navcom_cmd_0x20(session, 0xb5, 0x00c8); /* Pseudorange Noise Statistics - send every 20s */
        navcom_cmd_0x20(session, 0xb0, 0x4000); /* Raw Meas Data Block */
        navcom_cmd_0x20(session, 0x81, 0x0000); /* Packed Ephemeris Data - send once */
        navcom_cmd_0x20(session, 0x81, 0x4000); /* Packed Ephemeris Data */
        navcom_cmd_0x20(session, 0x86, 0x4000); /* Channel Status */
        navcom_cmd_0x20(session, 0x83, 0x4000); /* Ionosphere and UTC Data */
        navcom_cmd_0x20(session, 0xef, 0x0bb8); /* Clock Drift - send every 5 min */
	/*@ -charint @*/
    }
}

static void navcom_ping(struct gps_device_t *session)
{
/* NOTE - This allows us to know into which of the unit's various
          serial ports we are connected.
          Its value gets updated every time we receive a 0x06 (Ack)
          message.  Note that if commands are being fed into the
          unit from more than one port (which is entirely possible
          although not necessarily a bright idea), there is a good
          chance that we might misidentify our port */
    /*@ -type @*/
    session->driver.navcom.physical_port = 0xFF;

    navcom_cmd_0x1c(session, 0x02, 0);      /* Test Support Block */
    navcom_cmd_0x20(session, 0xae, 0x0000); /* Identification Block */
    navcom_cmd_0x20(session, 0x86, 0x000a); /* Channel Status */
    /*@ +type @*/
}

static bool navcom_speed(struct gps_device_t *session, unsigned int speed)
{
#ifdef ALLOW_RECONFIGURE
    u_int8_t port_selection;
    u_int8_t baud;
    if (session->driver.navcom.physical_port == (unsigned char)0xFF) {
        /* We still don't know which port we're connected to */
        return false;
    }
    /*@ +charint @*/
    switch (speed) {
        /* NOTE - The spec says that certain baud combinations
                  on ports A and B are not allowed, those are
                  1200/115200, 2400/57600, and 2400/115200.
                  To try and minimise the possibility of those
                  occurring, we do not allow baud rates below
                  4800.  We could also disallow 57600 and 115200
                  to totally prevent this, but I do not consider
                  that reasonable.  Finding which baud speed the
                  other port is set at would also be too much
                  trouble, so we do not do it. */
        case   4800:
            baud = 0x04;
            break;
        case   9600:
            baud = 0x06;
            break;
        case  19200:
            baud = 0x08;
            break;
        case  38400:
            baud = 0x0a;
            break;
        case  57600:
            baud = 0x0c;
            break;
        case 115200:
            baud = 0x0e;
            break;
        default:
            /* Unsupported speed */
            return false;
    }
    /*@ -charint @*/
    
    /* Proceed to construct our message */
    port_selection = session->driver.navcom.physical_port | baud;
    
    /* Send it off */
    navcom_cmd_0x11(session, port_selection);
    
    /* And cheekily return true, even though we have
       no way to know if the speed change succeeded
       until and if we receive an ACK (message 0x06),
       which will be at the new baud speed if the
       command was successful.  Bottom line, the client
       should requery gpsd to see if the new speed is
       different than the old one */
    return true;
#else
    return false;
#endif /* ALLOW_RECONFIGURE */
}

/* Ionosphere and UTC Data */
static gps_mask_t handle_0x83(struct gps_device_t *session)
{
    /* NOTE - At the present moment this is only being used
              for determining the GPS-UTC time difference,
              for which the iono data is not needed as far
              as we are concerned.  However, I am still 
              reporting it (if debuglevel >= LOG_IO) as a
              matter of interest */
/* 2^-30 */
#define SF_A0 (0.000000000931322574615478515625)
/* 2^-50 */
#define SF_A1 (0.000000000000000888178419700125)
/* 2^12 */
#define SF_TOT (4096)
/* 2^-30 */
#define SF_ALPHA0 (0.000000000931322574615478515625)
/* 2^-27 */
#define SF_ALPHA1 (0.000000007450580596923828125)
/* 2^-24 */
#define SF_ALPHA2 (0.000000059604644775390625)
/* 2^-24 */
#define SF_ALPHA3 (0.000000059604644775390625)
/* 2^11 */
#define SF_BETA0 (2048)
/* 2^14 */
#define SF_BETA1 (16384)
/* 2^16 */