gpsd/drivers/driver_geostar.c

/*
 * This is the gpsd driver for GeoStar Navigation receivers
 * operating in binary mode.
 *
 * Tested with GeoS-1M GPS/GLONASS receiver.
 *
 * By Viktar Palstsiuk, viktar.palstsiuk@promwad.com
 *
 * This file is Copyright 2010 by the GPSD project
 * SPDX-License-Identifier: BSD-2-clause
 */

#include "../include/gpsd_config.h"  /* must be before all includes */

#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>

#include "../include/bits.h"
#include "../include/gpsd.h"
#include "../include/strfuncs.h"
#include "../include/timespec.h"

#ifdef GEOSTAR_ENABLE
#define GEOSTAR_CHANNELS   24

#define JAN_2008           0x47798280 /* 1199145600 = 2008 - 1970 in seconds */

#define OFFSET(n)          ((n)*4+4)

static int decode_channel_id (uint32_t ch_id) {
        int num = 0;
        num = (int)(ch_id & 0x1F);                              /* SV ID */
        if((ch_id & (1<<30)) == 0) num += GLONASS_PRN_OFFSET;   /* GLONASS SV */
        else if (num == 0 ) num = 32;                           /* GPS SV */
        return num;
}

static int geostar_write(struct gps_device_t *session,
                         unsigned int id, unsigned char *data, size_t len)
{
    int i;
    unsigned long cs = 0;

    putbyte(session->msgbuf, 0, 'P');
    putbyte(session->msgbuf, 1, 'S');
    putbyte(session->msgbuf, 2, 'G');
    putbyte(session->msgbuf, 3, 'G');

    putbe16(session->msgbuf, 4, id);
    putbe16(session->msgbuf, 6, len);

    /* Copy content */
    memcpy(session->msgbuf + 8, data, len * 4);

    len += 2; /* PSGG + id + len */

    /* Calculate checksum */
    for (i = 0; (size_t)i < len; i++) {
        cs ^= getleu32(session->msgbuf, i * 4);
    }

    putle32(session->msgbuf, len * 4, cs);

    len += 1; /* Checksum */

    session->msgbuflen = len * 4;

    GPSD_LOG(LOG_PROG, &session->context->errout,
             "Sent GeoStar packet id 0x%x\n", id);
    if (gpsd_write(session, session->msgbuf, session->msgbuflen) !=
        (ssize_t) session->msgbuflen)
        return -1;

    return 0;
}

/* geostar_detect()
 *
 * see if it looks like a GeoStar device is listening and
 * return 1 if found, 0 if not
 */
static bool geostar_detect(struct gps_device_t *session)
{
    unsigned char buf[1 * 4];
    bool ret = false;
    int myfd;

    myfd = session->gpsdata.gps_fd;

    /* request firmware revision and look for a valid response */
    putbe32(buf, 0, 0);
    if (geostar_write(session, 0xc1, buf, 1) == 0) {
        unsigned int n;
        struct timespec to;
        // FIXME: this holds the main loop from running...
        for (n = 0; n < 3; n++) {
            // wait one second
            to.tv_sec = 1;
            to.tv_nsec = 0;
            if (!nanowait(myfd, &to))
                break;
            if (generic_get(session) >= 0) {
                if (session->lexer.type == GEOSTAR_PACKET) {
                    GPSD_LOG(LOG_RAW, &session->context->errout,
                             "geostar_detect found\n");
                    ret = true;
                    break;
                }
            }
        }
    }

    return ret;
}

static gps_mask_t geostar_analyze(struct gps_device_t *session)
{
    int i, len;
    gps_mask_t mask = 0;
    unsigned int id;
    uint16_t uw1, uw2;
    uint32_t ul1, ul2, ul3, ul4, ul5;
    double d1, d2, d3, d4, d5;
    char buf[BUFSIZ];
    char buf2[BUFSIZ];

    if (session->lexer.type != GEOSTAR_PACKET) {
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "geostar_analyze packet type %d\n",
                 session->lexer.type);
        return 0;
    }

    if (session->lexer.outbuflen < 12 || session->lexer.outbuffer[0] != 'P')
        return 0;

    /* put data part of message in buf */

    memset(buf, 0, sizeof(buf));
    memcpy(buf, session->lexer.outbuffer, session->lexer.outbuflen);

    buf2[len = 0] = '\0';
    for (i = 0; i < (int)session->lexer.outbuflen; i++) {
        str_appendf(buf2, sizeof(buf2),
                       "%02x", buf[len++] = session->lexer.outbuffer[i]);
    }

    id = (unsigned int)getleu16(session->lexer.outbuffer, OFFSET(0));

    GPSD_LOG(LOG_DATA, &session->context->errout,
             "GeoStar packet id 0x%02x length %d: %s\n", id, len, buf2);

    session->cycle_end_reliable = true;

    switch (id) {
    case 0x10:
        GPSD_LOG(LOG_INF, &session->context->errout, "Raw measurements\n");
        break;
    case 0x11:
        GPSD_LOG(LOG_INF, &session->context->errout, "GPS sub-frame data\n");
        break;
    case 0x12:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "GLONASS sub-frame data\n");
        break;
    case 0x13:
        d1 = getled64(buf, OFFSET(1));
        d2 = getled64(buf, OFFSET(3));
        d3 = getled64(buf, OFFSET(5));
        d4 = getled64(buf, OFFSET(29)); /* GPS time */
        d5 = getled64(buf, OFFSET(31)); /* GLONASS time */
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "ECEF coordinates %g %g %g %f %f\n", d1, d2, d3, d4, d5);
        break;
    case 0x20:
        d1 = getled64(buf, OFFSET(1)); /* time */

        DTOTS(&session->newdata.time, d1);
        session->newdata.time.tv_sec += JAN_2008;

        session->newdata.latitude = getled64(buf, OFFSET(3)) * RAD_2_DEG;
        session->newdata.longitude = getled64(buf, OFFSET(5)) * RAD_2_DEG;
        /* altitude above ellipsoid */
        session->newdata.altHAE = getled64(buf, OFFSET(7));
        session->newdata.geoid_sep = getled64(buf, OFFSET(9));
        session->gpsdata.satellites_used = (int)getles32(buf, OFFSET(11));
        session->gpsdata.dop.gdop = getled64(buf, OFFSET(13));
        session->gpsdata.dop.pdop = getled64(buf, OFFSET(15));
        session->gpsdata.dop.tdop = getled64(buf, OFFSET(17));
        session->gpsdata.dop.hdop = getled64(buf, OFFSET(19));
        session->gpsdata.dop.vdop = getled64(buf, OFFSET(21));
        session->newdata.speed = getled64(buf, OFFSET(31));
        session->newdata.track = getled64(buf, OFFSET(33)) * RAD_2_DEG;

        ul1 = getleu32(buf, OFFSET(29)); /* status */

        if (ul1 != 0) {
            session->newdata.status = STATUS_UNK;
            mask |= STATUS_SET;
        } else if (session->newdata.status < STATUS_GPS) {
            // Don't step on previously set status
            session->newdata.status = STATUS_GPS;
            mask |= STATUS_SET;
        }
        mask |= TIME_SET | NTPTIME_IS | LATLON_SET | ALTITUDE_SET |
                SPEED_SET | STATUS_SET | TRACK_SET | DOP_SET | USED_IS |
                REPORT_IS;

        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Geographic coordinates %f %g %g %g %g %g\n",
                 d1,
                 session->newdata.latitude,
                 session->newdata.longitude,
                 session->newdata.altHAE,
                 session->newdata.speed,
                 session->newdata.track);
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Dilution of precision %g %g %g %g %g\n",
                 session->gpsdata.dop.gdop,
                 session->gpsdata.dop.pdop,
                 session->gpsdata.dop.tdop,
                 session->gpsdata.dop.hdop,
                 session->gpsdata.dop.vdop);
        break;
    case 0x21:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        uw1 = getleu16(buf, OFFSET(3));
        uw2 = getleu16(buf, OFFSET(3) + 2);
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Current receiver telemetry %x %d %d %d\n",
                 ul1, ul2, uw1, uw2);
        if(ul1 & (1<<3)) {
            session->newdata.mode = MODE_2D;
        }
        else {
            session->newdata.mode = MODE_3D;
        }
        if(ul1 & (1<<2)) {
            session->newdata.status = STATUS_GPS;
        }
        else {
            session->newdata.status = STATUS_UNK;
            session->newdata.mode = MODE_NO_FIX;
        }

        mask |= MODE_SET | STATUS_SET;
        break;
    case 0x22:
        ul1 = getleu32(buf, OFFSET(1));
        GPSD_LOG(LOG_INF, &session->context->errout, "SVs in view %d\n", ul1);
        session->gpsdata.satellites_visible = (int)ul1;
        if(ul1 > GEOSTAR_CHANNELS) ul1 = GEOSTAR_CHANNELS;
        for(i = 0; (uint32_t)i < ul1; i++) {
            int16_t s1, s2, s3;
            ul2 = getleu32(buf, OFFSET(2) + i * 3 * 4);
            s1 = getles16(buf, OFFSET(3) + i * 3 * 4);
            s2 = getles16(buf, OFFSET(3) + 2 + i * 3 * 4);
            s3 = getles16(buf, OFFSET(4) + 2 + i * 3 * 4);
            GPSD_LOG(LOG_INF, &session->context->errout,
                     "ID %d Az %g El %g SNR %g\n",
                     decode_channel_id(ul2), s1 * 0.001 * RAD_2_DEG,
                     s2 * 0.001 * RAD_2_DEG, s3 * 0.1);
            session->gpsdata.skyview[i].PRN = (short)decode_channel_id(ul2);
            session->gpsdata.skyview[i].azimuth =
                    (short)round((double)s1 * 0.001 * RAD_2_DEG);
            session->gpsdata.skyview[i].elevation =
                    (short)round((double)s2 * 0.001 * RAD_2_DEG);
            session->gpsdata.skyview[i].ss = (double)s3*0.1;
            session->gpsdata.skyview[i].used = (bool)(ul2 & (1<<27));
        }
        session->gpsdata.skyview_time.tv_sec = 0;
        session->gpsdata.skyview_time.tv_nsec = 0;
        mask |= SATELLITE_SET | USED_IS;
        break;
    case 0x3e:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        ul3 = getleu32(buf, OFFSET(3));
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Receiver power-up message %d %d %d\n", ul1, ul2, ul3);
        break;
    case 0x3f:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        GPSD_LOG(LOG_WARN, &session->context->errout,
                 "Negative acknowledge %x %d\n", ul1, ul2);
        break;
    case 0x40:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set initial parameters\n");
        break;
    case 0x41:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set serial ports parameters\n");
        break;
    case 0x42:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        ul3 = getleu32(buf, OFFSET(3));
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set receiver operation mode %d %d %d\n",
                 ul1, ul2, ul3);
        break;
    case 0x43:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set navigation task solution parameters\n");
        break;
    case 0x44:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set output data rate\n");
        break;
    case 0x46:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Assign data protocol to communication port\n");
        break;
    case 0x48:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set GPS almanac\n");
        break;
    case 0x49:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set GLONASS almanac\n");
        break;
    case 0x4a:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set GPS ephemeris\n");
        break;
    case 0x4b:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set GLONASS ephemeris\n");
        break;
    case 0x4c:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        ul3 = getleu32(buf, OFFSET(3));
        ul4 = getleu32(buf, OFFSET(4));
        ul5 = getleu32(buf, OFFSET(5));
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Set PPS parameters %d %d %d %d %d\n",
                 ul1, ul2, ul3, ul4, ul5);
        break;
    case 0x4d:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Enable/disable SV in position fix\n");
        break;
    case 0x4e:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Enable/disable NMEA messages\n");
        break;
    case 0x4f:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Enable/disable binary messages %x %x\n",
                 ul1, ul2);
        break;
    case 0x80:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query initial parameters\n");
        break;
    case 0x81:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query serial ports parameters\n");
        break;
    case 0x82:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        ul3 = getleu32(buf, OFFSET(3));
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query receiver operation mode %d %d %d\n",
                 ul1, ul2, ul3);
        break;
    case 0x83:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query navigation task solution parameters\n");
        break;
    case 0x84:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query output data rate\n");
        break;
    case 0x86:
        session->driver.geostar.physical_port =
            (unsigned int)getleu32(buf, OFFSET(1));

        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query data protocol assignment to "
                 "communication port\n");
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Connected to physical port %d\n",
                 session->driver.geostar.physical_port);
        break;
    case 0x88:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query GPS almanac\n");
        break;
    case 0x89:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query GLONASS almanac\n");
        break;
    case 0x8a:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query GPS ephemerides\n");
        break;
    case 0x8b:
        d1 = getled64(buf, OFFSET(23));
        d2 = getled64(buf, OFFSET(25));
        d3 = getled64(buf, OFFSET(27));
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query GLONASS ephemerides %g %g %g\n",
                 d1, d2, d3);
        break;
    case 0x8c:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        ul3 = getleu32(buf, OFFSET(3));
        ul4 = getleu32(buf, OFFSET(4));
        ul5 = getleu32(buf, OFFSET(5));
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query PPS parameters %d %d %d %d %d\n",
                 ul1, ul2, ul3, ul4, ul5);
        break;
    case 0x8d:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query enable/disable status of "
                 "the SV in position fix\n");
        break;
    case 0x8e:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query enable NMEA messages\n");
        break;
    case 0x8f:
        ul1 = getleu32(buf, OFFSET(1));
        ul2 = getleu32(buf, OFFSET(2));
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Query enable binary messages %x %x\n",
                 ul1, ul2);
        break;
    case 0xc0:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Change operation mode command\n");
        break;
    case 0xc1:
        ul4 = getleu32(buf, OFFSET(1));
        ul1 = getleu32(buf, OFFSET(2));
        ul2 = getleu32(buf, OFFSET(3));
        ul3 = getleu32(buf, OFFSET(4));
        (void)snprintf(session->subtype, sizeof(session->subtype),
                "%u.%u %u.%u.%u %x %c-%u\n",
                ul4>>16, ul4&0xFFFF, ul1>>9, (ul1>>5)&0xF, ul1&0x1F, ul2,
                ul3>>24, ul3&0x00FFFFFF);
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Request FW version command: %s\n",
                 session->subtype);
        mask |= DEVICEID_SET;
        break;
    case 0xc2:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Restart receiver command\n");
        break;
    case 0xc3:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Store parameters to Flash command\n");
        break;
    case 0xd0:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Erase Flash sector command\n");
        break;
    case 0xd1:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Write data to Flash command\n");
        break;
    case 0xd2:
        GPSD_LOG(LOG_INF, &session->context->errout,
                 "Response to Store Serial Number command\n");
        break;
    default:
        GPSD_LOG(LOG_WARN, &session->context->errout,
                 "Unhandled GeoStar packet type 0x%02x\n", id);
        break;
    }

    return mask;
}

static gps_mask_t geostar_parse_input(struct gps_device_t *session)
{
    if (session->lexer.type == GEOSTAR_PACKET) {
        return geostar_analyze(session);;
    } else
        return 0;
}

static ssize_t geostar_control_send(struct gps_device_t *session,
                                 char *buf, size_t buflen)
/* not used by the daemon, it's for gpsctl and friends */
{
    return (ssize_t) geostar_write(session,
                                (unsigned int)buf[0],
                                (unsigned char *)buf + 1, (buflen - 1)/4);
}


static void geostar_init_query(struct gps_device_t *session)
{
    unsigned char buf[2 * 4];
    /* Poll Software Version */
    (void)geostar_write(session, 0xc1, buf, 1);
}

static void geostar_event_hook(struct gps_device_t *session, event_t event)
{
    unsigned char buf[2 * 4];

    if (session->context->readonly ||
        session->context->passive) {
        return;
    }

    if (event == event_identified || event == event_reactivate) {
        /* Select binary packets */
        putbe32(buf, 0, 0xffff0000);
        putbe32(buf, 4, 0);
        (void)geostar_write(session, 0x4f, buf, 2);

        /* Poll Ports params */
        putbe32(buf, 0, 1);
        (void)geostar_write(session, 0x81, buf, 1);
        putbe32(buf, 0, 0);
        (void)geostar_write(session, 0x81, buf, 1);
        /* Poll Init params */
        (void)geostar_write(session, 0x80, buf, 1);
        /* Poll Mode */
        (void)geostar_write(session, 0x82, buf, 1);
        /* Poll Solution params */
        (void)geostar_write(session, 0x83, buf, 1);
        /* Poll Output rate */
        (void)geostar_write(session, 0x84, buf, 1);
        /* Poll Protocols assignment */
        (void)geostar_write(session, 0x86, buf, 1);
        /* Poll PPS params */
        (void)geostar_write(session, 0x8c, buf, 1);
        /* Poll NMEA packets selected */
        (void)geostar_write(session, 0x8e, buf, 1);
        /* Poll binary packets selected */
        (void)geostar_write(session, 0x8f, buf, 1);
    }

    if (event == event_deactivate) {
        /* Perform cold restart */
        putbe32(buf, 0, 3);
        (void)geostar_write(session, 0xc2, buf, 1);
    }
}

static bool geostar_speed_switch(struct gps_device_t *session,
                              speed_t speed, char parity, int stopbits)
{
    unsigned char buf[4 * 4];
    int iparity;

    switch (parity) {
    case 'E':
    case 2:
        parity = (char)2;
        break;
    case 'O':
    case 1:
        parity = (char)1;
        break;
    case 'N':
    case 0:
    default:
        parity = (char)0;
        break;
    }
    iparity = parity;

    putbe32(buf, 0, session->driver.geostar.physical_port);
    putbe32(buf, 4, speed);
    putbe32(buf, 8, stopbits);
    putbe32(buf, 12, iparity);
    (void)geostar_write(session, 0x41, buf, 4);

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

static void geostar_mode(struct gps_device_t *session, int mode)
{
    if (mode == MODE_NMEA) {
        unsigned char buf[1 * 4];
        /* Switch to NMEA mode */
        putbe32(buf, 0, 1);
        (void)geostar_write(session, 0x46, buf, 1);
    } else if (mode == MODE_BINARY) {
        /* Switch to binary mode */
        (void)nmea_send(session, "$GPSGG,SWPROT");
    } else {
        GPSD_LOG(LOG_ERROR, &session->context->errout,
                 "unknown mode %i requested\n", mode);
    }
}

static double geostar_time_offset(struct gps_device_t *session UNUSED)
{
    return 0.31;
}

/* this is everything we export */
/* *INDENT-OFF* */
const struct gps_type_t driver_geostar =
{
    .type_name      = "GeoStar",        /* full name of type */
    .packet_type    = GEOSTAR_PACKET,   /* associated lexer packet type */
    .flags          = DRIVER_STICKY,    /* remember this */
    .trigger        = NULL,             /* no trigger */
    .channels       = GEOSTAR_CHANNELS, /* consumer-grade GPS/GLONASS */
    .probe_detect   = geostar_detect,   /* probe for device */
    .get_packet     = generic_get,      /* use the generic packet getter */
    .parse_packet   = geostar_parse_input,      /* parse message packets */
    .rtcm_writer    = NULL,             /* doesn't accept DGPS corrections */
    .init_query     = geostar_init_query,    // non-perturbing initial query
    .event_hook     = geostar_event_hook,    // fire on various lifetime events
    .speed_switcher = geostar_speed_switch,/* change baud rate */
    .mode_switcher  = geostar_mode,     /* there is a mode switcher */
    .rate_switcher  = NULL,             /* no rate switcher */
    .min_cycle.tv_sec  = 1,             /* not relevant, no rate switch */
    .min_cycle.tv_nsec = 0,             /* not relevant, no rate switch */
    .control_send   = geostar_control_send,/* how to send commands */
    .time_offset     = geostar_time_offset,
};
/* *INDENT-ON* */

#endif /* GEOSTAR_ENABLE */

// vim: set expandtab shiftwidth=4