italk.c

gpsd, a popular GPS daemon.

/* $Id: italk.c 4661 2008-01-19 22:54:23Z garyemiller $ */
/*
 * Driver for the iTalk binary protocol used by FasTrax
 */
#include <sys/types.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <unistd.h>
#include <time.h>
#include <stdio.h>

#include "gpsd_config.h"
#include "gpsd.h"
#if defined(ITRAX_ENABLE) && defined(BINARY_ENABLE)

#define LITTLE_ENDIAN_PROTOCOL
#include "bits.h"
#include "italk.h"

static gps_mask_t italk_parse(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t decode_itk_navfix(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t decode_itk_prnstatus(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *, unsigned char *, size_t);

static gps_mask_t decode_itk_navfix(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    unsigned int tow;
    unsigned short gps_week, flags, cflags, pflags;
    gps_mask_t mask = 0;
    double epx, epy, epz, evx, evy, evz;
    double t;

    if (len != 296){
	gpsd_report(LOG_PROG, "ITALK: bad NAV_FIX (len %d, should be 296)\n", len);
	return -1;
    }

    flags = getuw(buf, 7 + 4);
    cflags = getuw(buf, 7 + 6);
    pflags = getuw(buf, 7 + 8);

    session->gpsdata.status = STATUS_NO_FIX;
    session->gpsdata.fix.mode = MODE_NO_FIX;
    mask =  ONLINE_SET | MODE_SET | STATUS_SET | CYCLE_START_SET;

    /* just bail out if this fix is not marked valid */
    if (0 != (pflags & FIX_FLAG_MASK_INVALID) || 0 == (flags & FIXINFO_FLAG_VALID))
	return mask;

    gps_week = (ushort)getsw(buf, 7 + 82);
    tow = getul(buf, 7 + 84);
    t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds;
    session->gpsdata.sentence_time = t;
    session->gpsdata.fix.time = t;
    mask |= TIME_SET;

    epx = (double)(getsl(buf, 7 + 96)/100.0);
    epy = (double)(getsl(buf, 7 + 100)/100.0);
    epz = (double)(getsl(buf, 7 + 104)/100.0);
    evx = (double)(getsl(buf, 7 + 186)/1000.0);
    evy = (double)(getsl(buf, 7 + 190)/1000.0);
    evz = (double)(getsl(buf, 7 + 194)/1000.0);
    ecef_to_wgs84fix(&session->gpsdata, epx, epy, epz, evx, evy, evz);
    mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET  ;
    session->gpsdata.fix.eph = (double)(getsl(buf, 7 + 252)/100.0);
    session->gpsdata.fix.eps = (double)(getsl(buf, 7 + 254)/100.0);

    session->gpsdata.satellites_used = 0xffff ^ getub(buf, 7 + 16);
    mask |= USED_SET ;

    if (flags & FIX_CONV_DOP_VALID){
	session->gpsdata.hdop = (double)(getuw(buf, 7 + 56)/100.0);
	session->gpsdata.gdop = (double)(getuw(buf, 7 + 58)/100.0);
	session->gpsdata.pdop = (double)(getuw(buf, 7 + 60)/100.0);
	session->gpsdata.vdop = (double)(getuw(buf, 7 + 62)/100.0);
	session->gpsdata.tdop = (double)(getuw(buf, 7 + 64)/100.0);
	mask |= HDOP_SET | GDOP_SET | PDOP_SET | VDOP_SET | TDOP_SET;
    }

    if ((pflags & FIX_FLAG_MASK_INVALID) == 0 && (flags & FIXINFO_FLAG_VALID) != 0){
	if (pflags & FIX_FLAG_3DFIX)
	    session->gpsdata.fix.mode = MODE_3D;
	else
	    session->gpsdata.fix.mode = MODE_2D;

	if (pflags & FIX_FLAG_DGPS_CORRECTION)
	    session->gpsdata.status = STATUS_DGPS_FIX;
	else
	    session->gpsdata.status = STATUS_FIX;
    }

    return mask;
}

static gps_mask_t decode_itk_prnstatus(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    unsigned int i, tow, nsv, nchan, st;
    unsigned short gps_week;
    double t;

    if (len < 62){
	gpsd_report(LOG_PROG, "ITALK: runt PRN_STATUS (len=%d)\n", len);
	return ERROR_SET;
    }

    gps_week = getuw(buf, 7 + 4);
    tow = getul(buf, 7 + 6);
    t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds;
    session->gpsdata.sentence_time = session->gpsdata.fix.time = t;

    gpsd_zero_satellites(&session->gpsdata);
    nchan = (unsigned int)((len - 10 - 52) / 20);
    nsv = 0;
    for (i = st = 0; i < nchan; i++) {
	int off = 7+ 52 + 20 * i;
	unsigned short flags;

	flags = getuw(buf, off);
	session->gpsdata.ss[i]		= (int)getuw(buf, off+2)&0xff;
	session->gpsdata.PRN[i]		= (int)getuw(buf, off+4)&0xff;
	session->gpsdata.elevation[i]	= (int)getsw(buf, off+6)&0xff;
	session->gpsdata.azimuth[i]	= (int)getsw(buf, off+8)&0xff;
	if (session->gpsdata.PRN[i]){
	    st++;
	    if (flags & PRN_FLAG_USE_IN_NAV)
		session->gpsdata.used[nsv++] = session->gpsdata.PRN[i];
	}
    }
    session->gpsdata.satellites = (int)st;
    session->gpsdata.satellites_used = (int)nsv;

    return USED_SET | SATELLITE_SET | TIME_SET;
}

static gps_mask_t decode_itk_utcionomodel(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    unsigned int tow;
    int leap;
    unsigned short gps_week, flags;
    double t;

    if (len != 64){
	gpsd_report(LOG_PROG, "ITALK: bad UTC_IONO_MODEL (len %d, should be 64)\n", len);
	return ERROR_SET;
    }

    flags = getuw(buf, 7);
    if (0 == (flags & UTC_IONO_MODEL_UTCVALID))
	return 0;

    leap = (int)getuw(buf, 7 + 24);
    if (session->context->leap_seconds < leap)
	session->context->leap_seconds = leap;

    gps_week = getuw(buf, 7 + 36);
    tow = getul(buf, 7 + 38);
    t = gpstime_to_unix((int)gps_week, tow/1000.0) - session->context->leap_seconds;
    session->gpsdata.sentence_time = session->gpsdata.fix.time = t;

    return TIME_SET;
}

/*@ +charint -usedef -compdef @*/
static bool italk_write(int fd, unsigned char *msg, size_t msglen) {
   bool      ok;

   /* CONSTRUCT THE MESSAGE */

   /* we may need to dump the message */
   gpsd_report(LOG_IO, "writing italk control type %02x:%s\n",
	       msg[0], gpsd_hexdump(msg, msglen));
#ifdef ALLOW_RECONFIGURE
   ok = (write(fd, msg, msglen) == (ssize_t)msglen);
   (void)tcdrain(fd);
#else
   ok = 0;
#endif /* ALLOW_RECONFIGURE */
   return(ok);
}
/*@ -charint +usedef +compdef @*/

/*@ +charint @*/
static gps_mask_t italk_parse(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    unsigned int type;
    gps_mask_t mask = 0;

    if (len == 0)
	return 0;

    type = (uint)getub(buf, 4);
    /* we may need to dump the raw packet */
    gpsd_report(LOG_RAW, "raw italk packet type 0x%02x length %d: %s\n", type, len, gpsd_hexdump(buf, len));

    switch (type)
    {
    case ITALK_NAV_FIX:
	gpsd_report(LOG_IO, "iTalk NAV_FIX len %d\n", len);
	mask = decode_itk_navfix(session, buf, len);
	break;
    case ITALK_PRN_STATUS:
	gpsd_report(LOG_IO, "iTalk PRN_STATUS len %d\n", len);
	mask = decode_itk_prnstatus(session, buf, len);
	break;
    case ITALK_UTC_IONO_MODEL:
	gpsd_report(LOG_IO, "iTalk UTC_IONO_MODEL len %d\n", len);
	mask = decode_itk_utcionomodel(session, buf, len);
	break;

    case ITALK_ACQ_DATA:
	gpsd_report(LOG_IO, "iTalk ACQ_DATA len %d\n", len);
	break;
    case ITALK_TRACK:
	gpsd_report(LOG_IO, "iTalk TRACK len %d\n", len);
	break;
    case ITALK_PSEUDO:
	gpsd_report(LOG_IO, "iTalk PSEUDO len %d\n", len);
	break;
    case ITALK_RAW_ALMANAC:
	gpsd_report(LOG_IO, "iTalk RAW_ALMANAC len %d\n", len);
	break;
    case ITALK_RAW_EPHEMERIS:
	gpsd_report(LOG_IO, "iTalk RAW_EPHEMERIS len %d\n", len);
	break;
    case ITALK_SUBFRAME:
	gpsd_report(LOG_IO, "iTalk SUBFRAME len %d\n", len);
	break;
    case ITALK_BIT_STREAM:
	gpsd_report(LOG_IO, "iTalk BIT_STREAM len %d\n", len);
	break;

    case ITALK_AGC:
    case ITALK_SV_HEALTH:
    case ITALK_PRN_PRED:
    case ITALK_FREQ_PRED:
    case ITALK_DBGTRACE:
    case ITALK_START:
    case ITALK_STOP:
    case ITALK_SLEEP:
    case ITALK_STATUS:
    case ITALK_ITALK_CONF:
    case ITALK_SYSINFO:
    case ITALK_ITALK_TASK_ROUTE:
    case ITALK_PARAM_CTRL:
    case ITALK_PARAMS_CHANGED:
    case ITALK_START_COMPLETED:
    case ITALK_STOP_COMPLETED:
    case ITALK_LOG_CMD:
    case ITALK_SYSTEM_START:
    case ITALK_STOP_SEARCH:
    case ITALK_SEARCH:
    case ITALK_PRED_SEARCH: