path: root/zodiac.c

/*
 * Handle the Rockwell binary packet format supported by the old Zodiac chipset
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include "gpsd.h"

#define LITTLE_ENDIAN_PROTOCOL
#include "bits.h"

#ifdef ZODIAC_ENABLE

struct header {
    unsigned short sync;
    unsigned short id;
    unsigned short ndata;
    unsigned short flags;
    unsigned short csum;
};

static unsigned short zodiac_checksum(unsigned short *w, int n)
{
    unsigned short csum = 0;

    while (n-- > 0)
	csum += *(w++);
    return -csum;
}

/* zodiac_spew - Takes a message type, an array of data words, and a length
   for the array, and prepends a 5 word header (including checksum).
   The data words are expected to be checksummed */
#if defined (WORDS_BIGENDIAN)
/* data is assumed to contain len/2 unsigned short words
 * we change the endianness to little, when needed.
 */
static int end_write(int fd, void *d, int len)
{
    char buf[BUFSIZ];
    char *p = buf;
    char *data = (char *)d;

    while (len>0) {
	*p++ = *(data+1); *p++ = *data;
	data += 2; len -= 2;
    }
    return write(fd, buf, len);
}
#else
#define end_write write
#endif

static void zodiac_spew(struct gps_device_t *session, int type, unsigned short *dat, int dlen)
{
    struct header h;
    int i;
    char buf[BUFSIZ];

    h.sync = 0x81ff;
    h.id = (unsigned short)type;
    h.ndata = (unsigned short)(dlen - 1);
    h.flags = 0;
    h.csum = zodiac_checksum((unsigned short *) &h, 4);

    if (session->gpsdata.gps_fd != -1) {
	(void)end_write(session->gpsdata.gps_fd, &h, sizeof(h));
	(void)end_write(session->gpsdata.gps_fd, dat, sizeof(unsigned short) * dlen);
    }

    (void)snprintf(buf, sizeof(buf),
		   "%04x %04x %04x %04x %04x",
		   h.sync,h.id,h.ndata,h.flags,h.csum);
    for (i = 0; i < dlen; i++)
	(void)snprintf(buf+strlen(buf), sizeof(buf)-strlen(buf),
		       " %04x", dat[i]);

    gpsd_report(5, "Sent Zodiac packet: %s\n",buf);
}

static bool zodiac_speed_switch(struct gps_device_t *session, speed_t speed)
{
    unsigned short data[15];

    if (session->zodiac.sn++ > 32767)
	session->zodiac.sn = 0;
      
    memset(data, 0, sizeof(data));
    /* data is the part of the message starting at word 6 */
    data[0] = session->zodiac.sn;	/* sequence number */
    data[1] = 1;			/* port 1 data valid */
    data[2] = 1;			/* port 1 character width (8 bits) */
    data[3] = 0;			/* port 1 stop bits (1 stopbit) */
    data[4] = 0;			/* port 1 parity (none) */
    data[5] = (unsigned short)(round(log((double)speed/300)/M_LN2)+1); /* port 1 speed */
    data[14] = zodiac_checksum(data, 14);

    zodiac_spew(session, 1330, data, 15);

    return true;	/* it would be nice to error-check this */
}

static void send_rtcm(struct gps_device_t *session, 
		      char *rtcmbuf, size_t rtcmbytes)
{
    unsigned short data[34];
    int n = 1 + (int)(rtcmbytes/2 + rtcmbytes%2);

    if (session->zodiac.sn++ > 32767)
	session->zodiac.sn = 0;

    memset(data, 0, sizeof(data));
    data[0] = session->zodiac.sn;		/* sequence number */
    memcpy(&data[1], rtcmbuf, rtcmbytes);
    data[n] = zodiac_checksum(data, n);

    zodiac_spew(session, 1351, data, n+1);
}

static ssize_t zodiac_send_rtcm(struct gps_device_t *session,
			char *rtcmbuf, size_t rtcmbytes)
{
    size_t len;

    while (rtcmbytes > 0) {
	len = (size_t)(rtcmbytes>64?64:rtcmbytes);
	send_rtcm(session, rtcmbuf, len);
	rtcmbytes -= len;
	rtcmbuf += len;
    }
    return 1;
}

static gps_mask_t handle1000(struct gps_device_t *session)
{
    double subseconds;
    struct tm unpacked_date;
    /* ticks                      = getlong(6); */
    /* sequence                   = getword(8); */
    /* measurement_sequence       = getword(9); */
    /*@ -boolops -predboolothers @*/
    session->gpsdata.status       = (getword(10) & 0x1c) ? 0 : 1;
    if (session->gpsdata.status != 0)
	session->gpsdata.newdata.mode = (getword(10) & 1) ? MODE_2D : MODE_3D;
    else
	session->gpsdata.newdata.mode = MODE_NO_FIX;
    /*@ +boolops -predboolothers @*/

    /* solution_type                 = getword(11); */
    session->gpsdata.satellites_used = (int)getword(12);
    /* polar_navigation              = getword(13); */
    /* gps_week                      = getword(14); */
    /* gps_seconds                   = getlong(15); */
    /* gps_nanoseconds               = getlong(17); */
    unpacked_date.tm_mday = (int)getword(19);
    unpacked_date.tm_mon = (int)getword(20) - 1;
    unpacked_date.tm_year = (int)getword(21) - 1900;
    unpacked_date.tm_hour = (int)getword(22);
    unpacked_date.tm_min = (int)getword(23);
    unpacked_date.tm_sec = (int)getword(24);
    subseconds = (int)getlong(25) / 1e9;
    /*@ -compdef */
    session->gpsdata.newdata.time = session->gpsdata.sentence_time =
	(double)mkgmtime(&unpacked_date) + subseconds;
    /*@ +compdef */
#ifdef NTPSHM_ENABLE
    if (session->gpsdata.newdata.mode > MODE_NO_FIX)
	(void)ntpshm_put(session, session->gpsdata.newdata.time + 1.1);
#endif
    /*@ -type @*/
    session->gpsdata.newdata.latitude  = ((long)getlong(27)) * RAD_2_DEG * 1e-8;
    session->gpsdata.newdata.longitude = ((long)getlong(29)) * RAD_2_DEG * 1e-8;
    /*
     * The Rockwell Jupiter TU30-D140 reports altitude as uncorrected height
     * above WGS84 geoid.  The Zodiac binary protocol manual does not 
     * specify whether word 31 is geodetic or WGS 84. 
     */
    session->gpsdata.newdata.altitude  = ((long)getlong(31)) * 1e-2;
    /*@ +type @*/
    session->gpsdata.separation    = ((short)getword(33)) * 1e-2;
    session->gpsdata.newdata.altitude -= session->gpsdata.separation;
    session->gpsdata.newdata.speed     = (int)getlong(34) * 1e-2;
    session->gpsdata.newdata.track     = (int)getword(36) * RAD_2_DEG * 1e-3;
    session->mag_var               = ((short)getword(37)) * RAD_2_DEG * 1e-4;
    session->gpsdata.newdata.climb     = ((short)getword(38)) * 1e-2;
    /* map_datum                   = getword(39); */
    /* manual says these are 1-sigma */
    session->gpsdata.newdata.eph       = (int)getlong(40) * 1e-2 * GPSD_CONFIDENCE;
    session->gpsdata.newdata.epv       = (int)getlong(42) * 1e-2 * GPSD_CONFIDENCE;
    session->gpsdata.newdata.ept       = (int)getlong(44) * 1e-2 * GPSD_CONFIDENCE;
    session->gpsdata.newdata.eps       = (int)getword(46) * 1e-2 * GPSD_CONFIDENCE;
    /* clock_bias                  = (int)getlong(47) * 1e-2; */
    /* clock_bias_sd               = (int)getlong(49) * 1e-2; */
    /* clock_drift                 = (int)getlong(51) * 1e-2; */
    /* clock_drift_sd              = (int)getlong(53) * 1e-2; */

#if 0
    gpsd_report(1, "date: %lf\n", session->gpsdata.newdata.time);
    gpsd_report(1, "  solution invalid:\n");
    gpsd_report(1, "    altitude: %d\n", (getword(10) & 1) ? 1 : 0);
    gpsd_report(1, "    no diff gps: %d\n", (getword(10) & 2) ? 1 : 0);
    gpsd_report(1, "    not enough satellites: %d\n", (getword(10) & 4) ? 1 : 0);
    gpsd_report(1, "    exceed max EHPE: %d\n", (getword(10) & 8) ? 1 : 0);
    gpsd_report(1, "    exceed max EVPE: %d\n", (getword(10) & 16) ? 1 : 0);
    gpsd_report(1, "  solution type:\n");
    gpsd_report(1, "    propagated: %d\n", (getword(11) & 1) ? 1 : 0);
    gpsd_report(1, "    altitude: %d\n", (getword(11) & 2) ? 1 : 0);
    gpsd_report(1, "    differential: %d\n", (getword(11) & 4) ? 1 : 0);
    gpsd_report(1, "Number of measurements in solution: %d\n", getword(12));
    gpsd_report(1, "Lat: %f\n", getlong(27) * RAD_2_DEG * 1e-8);
    gpsd_report(1, "Lon: %f\n", getlong(29) * RAD_2_DEG * 1e-8);
    gpsd_report(1, "Alt: %f\n", (double) getlong(31) * 1e-2);
    gpsd_report(1, "Speed: %f\n", (double) getlong(34) * 1e-2 * MPS_TO_KNOTS);
    gpsd_report(1, "Map datum: %d\n", getword(39));
    gpsd_report(1, "Magnetic variation: %f\n", getword(37) * RAD_2_DEG * 1e-4);
    gpsd_report(1, "Course: %f\n", getword(36) * RAD_2_DEG * 1e-4);
    gpsd_report(1, "Separation: %f\n", getword(33) * 1e-2);
#endif

    session->gpsdata.sentence_length = 55;
    return TIME_SET|LATLON_SET|ALTITUDE_SET|CLIMB_SET|SPEED_SET|TRACK_SET|STATUS_SET|MODE_SET|CYCLE_START_SET; /* |HERR_SET|VERR_SET|SPEEDERR_SET */
}

static gps_mask_t handle1002(struct gps_device_t *session)
{
    int i, j, status, prn;

    session->gpsdata.satellites_used = 0;
    memset(session->gpsdata.used,0,sizeof(session->gpsdata.used));
    /* ticks                      = getlong(6); */
    /* sequence                   = getword(8); */
    /* measurement_sequence       = getword(9); */
    /* gps_week                   = getword(10); */
    /* gps_seconds                = getlong(11); */
    /* gps_nanoseconds            = getlong(13); */
    for (i = 0; i < ZODIAC_CHANNELS; i++) {
	/*@ -type @*/ 
	session->zodiac.Zv[i] = status = (int)getword(15 + (3 * i));
	session->zodiac.Zs[i] = prn = (int)getword(16 + (3 * i));
	/*@ +type @*/ 
#if 0
	gpsd_report(1, "Sat%02d:\n", i);
	gpsd_report(1, " used:%d\n", (status & 1) ? 1 : 0);
	gpsd_report(1, " eph:%d\n", (status & 2) ? 1 : 0);
	gpsd_report(1, " val:%d\n", (status & 4) ? 1 : 0);
	gpsd_report(1, " dgps:%d\n", (status & 8) ? 1 : 0);
	gpsd_report(1, " PRN:%d\n", prn);
	gpsd_report(1, " C/No:%d\n", getword(17 + (3 * i)));
#endif
	if (status & 1)
	    session->gpsdata.used[session->gpsdata.satellites_used++] = prn;
	for (j = 0; j < ZODIAC_CHANNELS; j++) {
	    if (session->gpsdata.PRN[j] != prn)
		continue;
	    session->gpsdata.ss[j] = (int)getword(17 + (3 * i));
	    break;
	}
    }
    return SATELLITE_SET | USED_SET;
}

static gps_mask_t handle1003(struct gps_device_t *session)
{
    int i;

    /* ticks              = getlong(6); */
    /* sequence           = getword(8); */
    session->gpsdata.gdop = (unsigned int)getword(9) * 1e-2;
    session->gpsdata.pdop = (unsigned int)getword(10) * 1e-2;
    session->gpsdata.hdop = (unsigned int)getword(11) * 1e-2;
    session->gpsdata.vdop = (unsigned int)getword(12) * 1e-2;
    session->gpsdata.tdop = (unsigned int)getword(13) * 1e-2;
    session->gpsdata.satellites = (int)getword(14);

    for (i = 0; i < ZODIAC_CHANNELS; i++) {
	if (i < session->gpsdata.satellites) {
	    session->gpsdata.PRN[i] = (int)getword(15 + (3 * i));
	    session->gpsdata.azimuth[i] = (int)(((short)getword(16 + (3 * i))) * RAD_2_DEG * 1e-4);
	    if (session->gpsdata.azimuth[i] < 0)
		session->gpsdata.azimuth[i] += 360;
	    session->gpsdata.elevation[i] = (int)(((short)getword(17 + (3 * i))) * RAD_2_DEG * 1e-4);
#if 0
	    gpsd_report(1, "Sat%02d:  PRN:%d az:%d el:%d\n", 
			i, getword(15+(3 * i)),getword(16+(3 * i)),getword(17+(3 * i)));
#endif
	} else {
	    session->gpsdata.PRN[i] = 0;
	    session->gpsdata.azimuth[i] = 0;
	    session->gpsdata.elevation[i] = 0;
	}
    }
    return SATELLITE_SET | HDOP_SET | VDOP_SET | PDOP_SET;
}

static void handle1005(struct gps_device_t *session UNUSED)
{
    /* ticks              = getlong(6); */
    /* sequence           = getword(8); */
    int numcorrections = (int)getword(12);
#if 0
    int i;

    gpsd_report(1, "Packet: %d\n", session->zodiac.sn);
    gpsd_report(1, "Station bad: %d\n", (getword(9) & 1) ? 1 : 0);
    gpsd_report(1, "User disabled: %d\n", (getword(9) & 2) ? 1 : 0);
    gpsd_report(1, "Station ID: %d\n", getword(10));
    gpsd_report(1, "Age of last correction in seconds: %d\n", getword(11));
    gpsd_report(1, "Number of corrections: %d\n", getword(12));
    for (i = 0; i < numcorrections; i++) {
	gpsd_report(1, "Sat%02d:\n", getword(13+i) & 0x3f);
	gpsd_report(1, "ephemeris:%d\n", (getword(13+i) & 64) ? 1 : 0);
	gpsd_report(1, "rtcm corrections:%d\n", (getword(13+i) & 128) ? 1 : 0);
	gpsd_report(1, "rtcm udre:%d\n", (getword(13+i) & 256) ? 1 : 0);
	gpsd_report(1, "sat health:%d\n", (getword(13+i) & 512) ? 1 : 0);
	gpsd_report(1, "rtcm sat health:%d\n", (getword(13+i) & 1024) ? 1 : 0);
	gpsd_report(1, "corrections state:%d\n", (getword(13+i) & 2048) ? 1 : 0);
	gpsd_report(1, "iode mismatch:%d\n", (getword(13+i) & 4096) ? 1 : 0);
    }
#endif
    if (session->gpsdata.fix.mode == MODE_NO_FIX)
	session->gpsdata.status = STATUS_NO_FIX;
    else if (numcorrections == 0)
	session->gpsdata.status = STATUS_FIX;
    else
	session->gpsdata.status = STATUS_DGPS_FIX;
}

static void handle1108(struct gps_device_t *session)
{
    /* ticks              = getlong(6); */
    /* sequence           = getword(8); */
    /* utc_week_seconds   = getlong(14); */
    /* leap_nanoseconds   = getlong(17); */
    if ((int)(getword(19) & 3) == 3)
	session->context->leap_seconds = (int)getword(16);
#if 0
    gpsd_report(1, "Leap seconds: %d.%09d\n", getword(16), getlong(17));
    gpsd_report(1, "UTC validity: %d\n", getword(19) & 3);
#endif
}

static gps_mask_t zodiac_analyze(struct gps_device_t *session)
{
    char buf[BUFSIZ];
    int i;
    unsigned int id = (unsigned int)((session->outbuffer[3]<<8) | session->outbuffer[2]);

    if (session->packet_type != ZODIAC_PACKET) {
	gpsd_report(2, "zodiac_analyze packet type %d\n",session->packet_type);
	return 0;
    }

    buf[0] = '\0';
    for (i = 0; i < (int)session->outbuflen; i++)
	(void)snprintf(buf+strlen(buf), sizeof(buf)-strlen(buf),
		       "%02x", (unsigned int)session->outbuffer[i]);
    (void)strcat(buf, "\n");
    gpsd_report(5, "Raw Zodiac packet type %d length %d: %s\n",id,session->outbuflen,buf);

    if (session->outbuflen < 10)
	return 0;

    (void)snprintf(session->gpsdata.tag,sizeof(session->gpsdata.tag),"%u",id);

    switch (id) {
    case 1000:
	return handle1000(session);
    case 1002:
	return handle1002(session);
    case 1003:
	return handle1003(session);
    case 1005:
	handle1005(session);
	return 0;	
    case 1108:
	handle1108(session);
	return 0;
    default:
	return 0;
    }
}

/* caller needs to specify a wrapup function */

/* this is everything we export */
struct gps_type_t zodiac_binary =
{
    .typename       = "Zodiac binary",	/* full name of type */
    .trigger        = NULL,		/* no trigger */
    .probe          = NULL,		/* no probe */
    .initializer    = NULL,		/* no initialization */
    .get_packet     = packet_get,	/* use the generic packet getter */
    .parse_packet   = zodiac_analyze,	/* parse message packets */
    .rtcm_writer    = zodiac_send_rtcm,	/* send DGPS correction */
    .speed_switcher = zodiac_speed_switch,/* we can change baud rate */
    .mode_switcher  = NULL,		/* no mode switcher */
    .rate_switcher  = NULL,		/* no sample-rate switcher */
    .cycle_chars    = -1,		/* not relevant, no rate switch */
    .wrapup         = NULL,		/* caller might supply a close hook */
    .cycle          = 1,		/* updates every second */
};

#endif /* ZODIAC_ENABLE */