driver_skytraq.c

/*
 * This is the gpsd driver for Skytraq GPSes operating in binary mode.
 *
 * This file is Copyright (c) 2016-2018 by the GPSD project
 * SPDX-License-Identifier: BSD-2-clause
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <strings.h>
#include <math.h>
#include <ctype.h>
#include <unistd.h>

#include "gpsd.h"
#include "bits.h"
#include "strfuncs.h"
#if defined(SKYTRAQ_ENABLE)

#define HI(n)		((n) >> 8)
#define LO(n)		((n) & 0xff)

/*
 * No ACK/NAK?  Just rety after 6 seconds
 */
#define SKY_RETRY_TIME	6
#define SKY_CHANNELS	48	/* max channels allowed in format */

static gps_mask_t sky_parse(struct gps_device_t *, unsigned char *, size_t);

static gps_mask_t sky_msg_80(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t sky_msg_DC(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t sky_msg_DD(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t sky_msg_DE(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t sky_msg_DF(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t sky_msg_E0(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t sky_msg_E2(struct gps_device_t *, unsigned char *, size_t);
static gps_mask_t sky_msg_E3(struct gps_device_t *, unsigned char *, size_t);

#ifdef __UNUSED
/* Poll Software Version MID 2 */
static unsigned char versionprobe[] = {
    0xa0, 0xa1, 0x00, 0x02,
    0x02,		/* MID 2 */
    0x01,		/* System */
    0x00, 0x0d, 0x0a
};

// cppcheck-suppress unusedFunction
static bool sky_write(struct gps_device_t *session, unsigned char *msg)
{
    unsigned int crc;
    size_t i, len;
    bool ok;
    unsigned int type = (unsigned int)msg[4];

    len = (size_t) ((msg[2] << 8) | msg[3]);

    /* calculate Checksum */
    crc = 0;
    /* coverity_submit[tainted_data] */
    for (i = 0; i < len; i++)
	crc ^= (int)msg[4 + i];

    /* enter CRC after payload */
    msg[len + 4] = (unsigned char)(crc & 0x00ff);

    gpsd_log(&session->context->errout, LOG_PROG,
	     "Skytraq: Writing control type %02x:\n", type);
    ok = (gpsd_write(session, (const char *)msg, len+7) == (ssize_t) (len+7));

    return (ok);
}
#endif /* __UNUSED */

/*
 * decode MID 0x80, Software Version
 *
 * 10 bytes
 */
static gps_mask_t sky_msg_80(struct gps_device_t *session,
				  unsigned char *buf, size_t len)
{
    unsigned int kver_x;  /* kernel version */
    unsigned int kver_y;  /* kernel version */
    unsigned int kver_z;  /* kernel version */
    unsigned int over_x;  /* ODM version */
    unsigned int over_y;  /* ODM version */
    unsigned int over_z;  /* ODM version */
    unsigned int rev_yy;   /* revision */
    unsigned int rev_mm;   /* revision */
    unsigned int rev_dd;   /* revision */

    if ( 14 != len)
	return 0;

    kver_x  = getbeu16(buf, 2);
    kver_y  = getub(buf, 4);
    kver_z  = getub(buf, 5);
    over_x  = getbeu16(buf, 6);
    over_y  = getub(buf, 8);
    over_z  = getub(buf, 9);
    rev_yy  = getbeu16(buf, 10);
    rev_mm  = getub(buf, 12);
    rev_dd  = getub(buf, 13);

    (void)snprintf(session->subtype, sizeof(session->subtype) - 1,
	     "kver=%3u.%2u,%2u, over=%3u.%2u,%2u, rev=%02u.%02u.%02u",
	    kver_x, kver_y, kver_z,
	    over_x, over_y, over_z,
	    rev_yy, rev_mm, rev_dd);

    gpsd_log(&session->context->errout, LOG_DATA,
	     "Skytraq: MID 0x80: kver=%u.%u,%u, over=%u.%u,%u, rev=%u.%u.%u\n",
	    kver_x, kver_y, kver_z,
	    over_x, over_y, over_z,
	    rev_yy, rev_mm, rev_dd);
    return 0;
}

/*
 * decode MID 0xDC, Measurement Time
 *
 * 10 bytes
 */
static gps_mask_t sky_msg_DC(struct gps_device_t *session,
				  unsigned char *buf, size_t len)
{
    unsigned int iod;   /* Issue of data 0 - 255 */
    unsigned int wn;    /* week number 0 - 65535 */
    unsigned int tow;   /* receiver tow 0 - 604799999 in mS */
    unsigned int mp;    /* measurement period 1 - 1000 ms */
    /* calculated */
    double	f_tow;  /* tow in seconds */
    unsigned int msec;  /* mSec part of tow */

    if ( 10 != len)
	return 0;

    iod = (unsigned int)getub(buf, 1);
    wn = getbeu16(buf, 2);
    tow = getbeu32(buf, 4);
    f_tow = (double)(tow / 1000);
    msec = tow % 1000;
    mp = getbeu16(buf, 8);

    /* should this be newdata.skyview_time? */
    session->gpsdata.skyview_time = gpsd_gpstime_resolve(session, wn, f_tow );

    gpsd_log(&session->context->errout, LOG_DATA,
	     "Skytraq: MID 0xDC: iod=%u, wn=%u, tow=%u, mp=%u, t=%lld.%03u\n",
	     iod, wn, tow, mp,
	     (long long)session->gpsdata.skyview_time, msec);
    return 0;
}

/*
 * decode MID 0xDD, Raw Measurements
 *
 */
static gps_mask_t sky_msg_DD(struct gps_device_t *session,
				  unsigned char *buf, size_t len UNUSED)
{
    unsigned int iod;   /* Issue of data 0 - 255 */
    unsigned int nmeas; /* number of measurements */

    iod = (unsigned int)getub(buf, 1);
    nmeas = (unsigned int)getub(buf, 2);

    gpsd_log(&session->context->errout, LOG_DATA,
	     "Skytraq: MID 0xDD: iod=%u, nmeas=%u\n",
	     iod, nmeas);
    return 0;
}

/*
 * decode MID 0xDE, SV and channel status
 *
 * max payload: 3 + (Num_sats * 10) = 483 bytes
 */
static gps_mask_t sky_msg_DE(struct gps_device_t *session,
				  unsigned char *buf, size_t len UNUSED)
{
    int st, nsv;
    unsigned int i;
    unsigned int iod;   /* Issue of data 0 - 255 */
    unsigned int nsvs;  /* number of SVs in this packet */

    iod = (unsigned int)getub(buf, 1);
    nsvs = (unsigned int)getub(buf, 2);
    /* too many sats? */
    if ( SKY_CHANNELS < nsvs )
	return 0;

    gpsd_zero_satellites(&session->gpsdata);
    for (i = st = nsv =  0; i < nsvs; i++) {
	int off = 3 + (10 * i); /* offset into buffer of start of this sat */
	bool good;	      /* do we have a good record ? */
	unsigned short sv_stat;
	unsigned short chan_stat;
	unsigned short ura;

	session->gpsdata.skyview[st].PRN = (short)getub(buf, off + 1);
	sv_stat = (unsigned short)getub(buf, off + 2);
	ura = (unsigned short)getub(buf, off + 3);
	session->gpsdata.skyview[st].ss = (float)getub(buf, off + 4);
	session->gpsdata.skyview[st].elevation =
	    (short)getbes16(buf, off + 5);
	session->gpsdata.skyview[st].azimuth =
	    (short)getbes16(buf, off + 7);
	chan_stat = (unsigned short)getub(buf, off + 9);

	session->gpsdata.skyview[st].used = (bool)(chan_stat & 0x30);
	good = session->gpsdata.skyview[st].PRN != 0 &&
	    session->gpsdata.skyview[st].azimuth != 0 &&
	    session->gpsdata.skyview[st].elevation != 0;

	gpsd_log(&session->context->errout, LOG_DATA,
		 "Skytraq: PRN=%2d El=%d Az=%d ss=%3.2f stat=%02x,%02x ura=%d %c\n",
		session->gpsdata.skyview[st].PRN,
		session->gpsdata.skyview[st].elevation,
		session->gpsdata.skyview[st].azimuth,
		session->gpsdata.skyview[st].ss,
		chan_stat, sv_stat, ura,
		good ? '*' : ' ');

	if ( good ) {
	    st += 1;
	    if (session->gpsdata.skyview[st].used)
		nsv++;
	}
    }

    session->gpsdata.satellites_visible = st;
    session->gpsdata.satellites_used = nsv;

    gpsd_log(&session->context->errout, LOG_DATA,
	     "Skytraq: MID 0xDE: nsvs=%u visible=%u iod=%u\n", nsvs,
	     session->gpsdata.satellites_visible, iod);
    return SATELLITE_SET | USED_IS;
}

/*
 * decode MID 0xDF, Nav status (PVT)
 *
 * 81 bytes
 */
static gps_mask_t sky_msg_DF(struct gps_device_t *session,
				  unsigned char *buf, size_t len)
{
    unsigned int iod;   /* Issue of data 0 - 255 */
    unsigned short navstat;
    unsigned int wn;    /* week number 0 - 65535 */
    double f_tow;         /* receiver tow Sec */
    double clock_bias;
    double clock_drift;
    gps_mask_t mask = 0;

    if ( 81 != len)
	return 0;

    iod = (unsigned int)getub(buf, 1);

    /* fix status is byte 2 */
    navstat = (unsigned short)getub(buf, 2);
    session->gpsdata.status = STATUS_NO_FIX;
    session->newdata.mode = MODE_NO_FIX;
    switch ( navstat ) {
    case 1:
	/* fix prediction, ignore */
	break;
    case 2:
	session->gpsdata.status = STATUS_FIX;
	session->newdata.mode = MODE_2D;
	break;
    case 3:
	session->gpsdata.status = STATUS_FIX;
	session->newdata.mode = MODE_3D;
	mask |= ALTITUDE_SET | CLIMB_SET;
	break;
    case 4:
	session->gpsdata.status = STATUS_DGPS_FIX;
	session->newdata.mode = MODE_3D;
	mask |= ALTITUDE_SET | CLIMB_SET;
	break;
    default:
	break;
    }

    wn = getbeu16(buf, 3);
    f_tow = getbed64((const char *)buf, 5);

    /* position/velocity is bytes 13-48, meters and m/s */
    session->newdata.ecef.x = (double)getbed64((const char *)buf, 13),
    session->newdata.ecef.y = (double)getbed64((const char *)buf, 21),
    session->newdata.ecef.z = (double)getbed64((const char *)buf, 29),
    session->newdata.ecef.vx = (double)getbef32((const char *)buf, 37),
    session->newdata.ecef.vy = (double)getbef32((const char *)buf, 41),
    session->newdata.ecef.vz = (double)getbef32((const char *)buf, 46);

    ecef_to_wgs84fix(&session->newdata, &session->gpsdata.separation,
	session->newdata.ecef.x, session->newdata.ecef.y,
	session->newdata.ecef.z, session->newdata.ecef.vx,
	session->newdata.ecef.vy, session->newdata.ecef.vz);

    clock_bias = getbed64((const char *)buf, 49);
    clock_drift = getbes32(buf, 57);

    session->gpsdata.dop.gdop = getbef32((const char *)buf, 61);
    session->gpsdata.dop.pdop = getbef32((const char *)buf, 65);
    session->gpsdata.dop.hdop = getbef32((const char *)buf, 69);
    session->gpsdata.dop.vdop = getbef32((const char *)buf, 73);
    session->gpsdata.dop.tdop = getbef32((const char *)buf, 77);

    session->newdata.time = gpsd_gpstime_resolve(session, wn, f_tow );

    gpsd_log(&session->context->errout, LOG_DATA,
	    "Skytraq: MID 0xDF: iod=%u, stat=%u, wn=%u, tow=%f, t=%.6f "
	    "cb: %f, cd: %f "
	    "gdop: %.2f, pdop: %.2f, hdop: %.2f, vdop: %.2f, tdop: %.2f\n",
	    iod, navstat, wn, f_tow,
	    session->newdata.time,
	    clock_bias, clock_drift,
	    session->gpsdata.dop.gdop,
	    session->gpsdata.dop.pdop,
	    session->gpsdata.dop.hdop,
	    session->gpsdata.dop.vdop,
	    session->gpsdata.dop.tdop);

    mask |= TIME_SET | LATLON_SET | TRACK_SET | ECEF_SET | VECEF_SET
            | SPEED_SET | STATUS_SET | MODE_SET | DOP_SET | CLEAR_IS
            | REPORT_IS;
    return mask;
}

/*
 * decode MID 0xE0, GPS Subframe data
 *
 * len 33 bytes
 *
 */
static gps_mask_t sky_msg_E0(struct gps_device_t *session,
				  unsigned char *buf, size_t len UNUSED)
{
    int i;
    unsigned int prn;   /* GPS sat PRN */
    unsigned int subf;  /* subframe 1-5 */
    /* the words are preprocessed, not raw, just the 24bits of data */
    uint32_t words[10];  /* subframe 1-5 */

    if ( 33 != len)
	return 0;

    prn = (unsigned int)getub(buf, 1);
    subf = (unsigned int)getub(buf, 2);
    for ( i = 0; i < 10; i++ ) {
	words[i] = (uint32_t)getbeu24(buf, 3 + (i * 3));
    }

    gpsd_log(&session->context->errout, LOG_DATA,
	     "Skytraq: 50B MID 0xE0: prn=%u, subf=%u\n",
	     prn, subf);

    return gpsd_interpret_subframe(session, prn, words);
}

/*
 * pretend to decode MID 0xE2, Beiduo D1 Subframe data
 *
 * from Beidou Standard BDS-SIS-ICD-2.0
 * D1, with the data rate of 50 bps, is broadcasted by the MEO/IGSO satellites
 *
 * len 31 bytes
 *
 */
static gps_mask_t sky_msg_E2(struct gps_device_t *session,
				  unsigned char *buf, size_t len)
{
    int i;
    unsigned int prn;   /* BeidouPS sat PRN 206-214 */
    unsigned int subf;  /* subframe 1-5 */
    /* the words are preprocessed, not raw, just the 28 bytes of data */
    uint8_t bytes[28];  /* raw data */

    if ( 31 != len)
	return 0;

    prn = (unsigned int)getub(buf, 1);
    subf = (unsigned int)getub(buf, 2);
    for ( i = 0; i < 28; i++ ) {
	bytes[i] = getub(buf, 3 + i);
    }

    /* extra guard prevents expensive hexdump calls */
    if (session->context->errout.debug >= LOG_PROG) {
	gpsd_log(&session->context->errout, LOG_PROG,
		 "Skytraq: Beidou D1 subframe PRN %d Subframe %d "
	         "length %zd byte:%s\n",
		 prn, subf,
		 len,
		 gpsd_hexdump(session->msgbuf, sizeof(session->msgbuf),
				 (char *)bytes, 28));
    }

    return ONLINE_SET;
}

/*
 * pretend to decode MID 0xE3, Beiduo D2 Subframe data
 *
 * from Beidou Standard BDS-SIS-ICD-2.0
 * D2, with the data rate of 500 bps, is broadcasted by the GEO satellites.
 *
 * len 31 bytes
 *
 */
static gps_mask_t sky_msg_E3(struct gps_device_t *session,
				  unsigned char *buf, size_t len)
{
    int i;
    unsigned int prn;   /* BeidouPS sat PRN 201-205 */
    unsigned int subf;  /* subframe 1-5 */
    /* the words are preprocessed, not raw, just the 28 bytes of data */
    uint8_t bytes[28];  /* raw data */

    if ( 31 != len)
	return 0;

    prn = (unsigned int)getub(buf, 1);
    subf = (unsigned int)getub(buf, 2);
    for ( i = 0; i < 28; i++ ) {
	bytes[i] = getub(buf, 3 + i);
    }

    /* extra guard prevents expensive hexdump calls */
    if (session->context->errout.debug >= LOG_PROG) {
	gpsd_log(&session->context->errout, LOG_PROG,
		 "Skytraq: Beidou D2 subframe PRN %d Subframe %d "
	         "length %zd byte:%s\n",
		 prn, subf,
		 len,
		 gpsd_hexdump(session->msgbuf, sizeof(session->msgbuf),
				 (char *)bytes, 28));
    }


    return ONLINE_SET;
}


static gps_mask_t sky_parse(struct gps_device_t * session, unsigned char *buf,
		      size_t len)
{
    gps_mask_t mask = 0;

    if (len == 0)
	return mask;

    buf += 4;   /* skip the leaders and length */
    len -= 7;   /* don't count the leaders, length, csum and terminators */
    // session->driver.sirf.lastid = buf[0];

    /* check the checksum?? */

    /* could change if the set of messages we enable does */
    /* session->cycle_end_reliable = true; */

    switch (buf[0]) {
    case 0x80:
	/* 128 */
	return sky_msg_80(session, buf, len);

    case 0x83:
	/* 131 - ACK */
	gpsd_log(&session->context->errout, LOG_PROG,
		 "Skytraq: ACK to MID %#02x\n", buf[1]);
	break;
    case 0x84:
	/* 132 - NACK */
	gpsd_log(&session->context->errout, LOG_INF,
		 "Skytraq: NACK to MID %#02x\n", buf[1]);
	break;
    case 0xDC:
	/* 220 */
	return sky_msg_DC(session, buf, len);

    case 0xDD:
	/* 221 */
	return sky_msg_DD(session, buf, len);

    case 0xDE:
	/* 222 */
	return sky_msg_DE(session, buf, len);

    case 0xDF:
	/* 223 - Nave status (PVT)  */
	return sky_msg_DF(session, buf, len);

    case 0xE0:
	/* 224 */
	return sky_msg_E0(session, buf, len);

    case 0xE2:
	/* 226 - Beidou2 D1 Subframe data */
	return sky_msg_E2(session, buf, len);

    case 0xE3:
	/* 227 - Beidou2 D2 Subframe data */
	return sky_msg_E3(session, buf, len);

    default:
	gpsd_log(&session->context->errout, LOG_PROG,
		 "Skytraq: Unknown packet id %#02x length %zd\n",
		 buf[0], len);
    }
    return mask;
}

static gps_mask_t skybin_parse_input(struct gps_device_t *session)
{
    if (session->lexer.type ==  SKY_PACKET) {
	return  sky_parse(session, session->lexer.outbuffer,
			session->lexer.outbuflen);
#ifdef NMEA0183_ENABLE
    } else if (session->lexer.type == NMEA_PACKET) {
	return nmea_parse((char *)session->lexer.outbuffer, session);
#endif /* NMEA0183_ENABLE */
    } else
	return 0;
}

/* this is everything we export */
/* *INDENT-OFF* */
const struct gps_type_t driver_skytraq =
{
    .type_name      = "Skytraq",		/* full name of type */
    .packet_type    = SKY_PACKET,	/* associated lexer packet type */
    .flags	    = DRIVER_STICKY,	/* remember this */
    .trigger	    = NULL,		/* no trigger */
    .channels       =  SKY_CHANNELS,	/* consumer-grade GPS */
    .probe_detect   = NULL,		/* no probe */
    .get_packet     = generic_get,	/* be prepared for Skytraq or NMEA */
    .parse_packet   = skybin_parse_input,/* parse message packets */
    .rtcm_writer    = gpsd_write,	/* send RTCM data straight */
    .init_query     = NULL,              /* non-perturbing initial qury */
    .event_hook     = NULL,              /* lifetime event handler */
};
/* *INDENT-ON* */
#endif /* defined( SKYTRAQ_ENABLE) && defined(BINARY_ENABLE) */