/*

* Garmin binary object for the GPS packet monitor.

*

* This file is Copyright (c) 2011 by the GPSD project

* BSD terms apply: see the file COPYING in the distribution root for details.

*/

#include <assert.h>

#include <math.h>

#include "gpsd.h"

#include "bits.h"

#include "gpsmon.h"

#if defined(GARMIN_ENABLE) && defined(BINARY_ENABLE)

extern const struct gps_type_t driver_garmin_ser_binary;

static WINDOW *miscwin, *mid51win, *mid114win;

#define display (void)mvwprintw

#define GARMIN_CHANNELS 12

#define GPSD_LE16TOH(x) getles16((char *)(&(x)), 0)

#define GPSD_LE32TOH(x) getles32((char *)(&(x)), 0)

#pragma pack(1)

/* Satellite Data Record */

typedef struct __attribute__((__packed__))

{

uint8_t svid;

uint16_t snr;

uint8_t elev;

uint16_t azmth;

uint8_t status;

} cpo_sat_data;

/* Position Record */

typedef struct __attribute__((__packed__))

{

float alt;

float epe;

float eph;

float epv;

int16_t fix;

double gps_tow;

double lat;

double lon;

float lon_vel;

float lat_vel;

float alt_vel;

float msl_hght;

int16_t leap_sec;

int32_t grmn_days;

} cpo_pvt_data;

/* Receiver Measurement Record */

typedef struct __attribute__((__packed__))

{

// cppcheck-suppress unusedStructMember

uint32_t cycles;

// cppcheck-suppress unusedStructMember

double pr;

// cppcheck-suppress unusedStructMember

uint16_t phase;

// cppcheck-suppress unusedStructMember

int8_t slp_dtct;

// cppcheck-suppress unusedStructMember

uint8_t snr_dbhz;

uint8_t svid;

// cppcheck-suppress unusedStructMember

int8_t valid;

} cpo_rcv_sv_data;

typedef struct __attribute__((__packed__))

{

// cppcheck-suppress unusedStructMember

double rcvr_tow;

// cppcheck-suppress unusedStructMember

int16_t rcvr_wn;

// cppcheck-suppress unusedStructMember

cpo_rcv_sv_data sv[GARMIN_CHANNELS];

} cpo_rcv_data;

/* description of the status */

static char *fixdesc[] = {

"no fix",

"no fix",

"2D",

"3D",

"2D dif",

"3D dif",

};

/* check range of an unsigned quantity */

#define CHECK_RANGE(vec, i) ((i) < sizeof(vec)/sizeof(vec[0]))

static bool garmin_bin_initialize(void)

{

unsigned int i;

#ifndef CONTROLSEND_ENABLE

if(serial) {

monitor_complain("Direct mode doesn't supported.");

return false;

}

#endif

miscwin = subwin(devicewin, 1, 80, 1, 0);

mid51win = subwin(devicewin, 12, 18, 2, 0);

mid114win = subwin(devicewin, GARMIN_CHANNELS + 3, 23, 2, 18);

if (miscwin == NULL || mid51win == NULL || mid114win == NULL)

return false;

(void)syncok(miscwin, true);

(void)syncok(mid51win, true);

(void)syncok(mid114win, true);

(void)wattrset(miscwin, A_BOLD);

display(miscwin, 0, 0, "Time:");

(void)wattrset(miscwin, A_NORMAL);

(void)wborder(mid51win, 0, 0, 0, 0, 0, 0, 0, 0),

(void)wattrset(mid51win, A_BOLD);

display(mid51win, 0, 4, " Position ");

display(mid51win, 1, 2, "Fix:");

display(mid51win, 2, 2, "Lat:");

(void)mvwaddch(mid51win, 2, 16, ACS_DEGREE);

display(mid51win, 3, 2, "Lon:");

(void)mvwaddch(mid51win, 3, 16, ACS_DEGREE);

display(mid51win, 4, 2, "Alt: m");

display(mid51win, 5, 2, "Speed: m/s");

display(mid51win, 6, 2, "Climb: m/s");

display(mid51win, 7, 2, "Leap: sec");

display(mid51win, 8, 2, "epe: m");

display(mid51win, 9, 2, "eph: m");

display(mid51win, 10, 2, "epv: m");

display(mid51win, 11, 3, " ID 51 (0x33) ");

(void)wattrset(mid51win, A_NORMAL);

(void)wborder(mid114win, 0, 0, 0, 0, 0, 0, 0, 0),

(void)wattrset(mid114win, A_BOLD);

display(mid114win, 1, 1, "Ch PRN Az El SNR ST");

for (i = 0; i < GARMIN_CHANNELS; i++) {

display(mid114win, (int)i + 2, 1, "%2d", i);

}

display(mid114win, 0, 5, " Satellite ");

display(mid114win, 14, 4, " ID 114 (0x72) ");

(void)wattrset(mid114win, A_NORMAL);

/* initialize the GPS context's time fields */

gpsd_time_init(session.context, time(NULL));

return true;

}

static void garmin_bin_update(uint16_t pkt_id, uint32_t pkt_size UNUSED, unsigned char *pkt_data)

{

int i;

cpo_sat_data *sats = NULL;

cpo_pvt_data *pvt = NULL;

//cpo_rcv_data *rmd = NULL;

char tbuf[JSON_DATE_MAX+1];

switch (pkt_id) {

case 0x29: /* Receiver Measurement Record */

case 0x34:

/* for future use */

//rmd = (cpo_rcv_data *)pkt_data;

monitor_log("RMD 0x%02x=", pkt_id);

break;

case 0x33: /* Position Record */

/* coverity_submit[tainted_data] */

display(miscwin, 0, 6, "%-24s",

unix_to_iso8601(session.gpsdata.fix.time, tbuf, sizeof(tbuf)));

pvt = (cpo_pvt_data *)pkt_data;

display(mid51win, 1, 7, "%s",

(CHECK_RANGE(fixdesc, (uint8_t)GPSD_LE16TOH(pvt->fix)) ? \

fixdesc[GPSD_LE16TOH(pvt->fix)] : "unknown"));

display(mid51win, 2, 8, "%3.5f", pvt->lat * RAD_2_DEG);

display(mid51win, 3, 8, "%3.5f", pvt->lon * RAD_2_DEG);

display(mid51win, 4, 8, "%8.2f", pvt->alt + pvt->msl_hght);

display(mid51win, 5, 9, "%5.1f", hypot(pvt->lon_vel, pvt->lat_vel));

display(mid51win, 6, 9, "%5.1f", pvt->alt_vel);

display(mid51win, 7, 8, "%d", (int)GPSD_LE16TOH(pvt->leap_sec));

if (GPSD_LE16TOH(pvt->fix) < 2) /* error value is very large when status no fix */

pvt->epe = pvt->eph = pvt->epv = NAN;

display(mid51win, 8, 7, "%6.2f", pvt->epe);

display(mid51win, 9, 7, "%6.2f", pvt->eph);

display(mid51win, 10, 7, "%6.2f", pvt->epv);

monitor_log("PVT 0x%02x=", pkt_id);

break;

case 0x72: /* Satellite Data Record */

sats = (cpo_sat_data *)pkt_data;

for (i = 0; i < GARMIN_CHANNELS; i++, sats++) {

display(mid114win, i + 2, 3, " %3u %3u %2u %4.1f %2x",

sats->svid,

GPSD_LE16TOH(sats->azmth),

sats->elev,

(float)GPSD_LE16TOH(sats->snr) / 100.0,

sats->status);

}

monitor_log("SAT 0x%02x=", pkt_id);

break;

case 0xff: /* Product Data Record */

monitor_log("PDR 0x%02x=", pkt_id);

break;

default:

monitor_log("UNK 0x%02x=", pkt_id);

break;

}

}

static void garmin_bin_ser_update(void)

{

unsigned char *buf;

size_t len;

uint16_t pkt_id;

uint32_t pkt_size;

unsigned char pkt_data[255];

int i, j;

unsigned char c;

unsigned char chksum;

bool pkt_good = false, got_dle = false;

buf = session.lexer.outbuffer;

len = session.lexer.outbuflen;

if (!(buf[0] == (unsigned char)0x10 && /* DLE */

buf[len-2] == (unsigned char)0x10 && /* DLE */

buf[len-1] == (unsigned char)0x03)) /* ETX */

goto end; /* bad pkt */

if (buf[1] == (unsigned char)0x10 && buf[2] != (unsigned char)0x10)

goto end; /* bad pkt */

pkt_id = (uint16_t)buf[1];

if (buf[2] == (unsigned char)0x10 && buf[3] != (unsigned char)0x10)

goto end; /* bad pkt */

pkt_size = (uint32_t)buf[2];

chksum = buf[1] + buf[2]; /* pkt_id + pkt_size */

j = 0;

for (i = 0; i <= 255; i++) {

if (pkt_size == (uint32_t)j)

break;

c = buf[i+3];

if (got_dle) {

got_dle = false;

if (c != (unsigned char)0x10)

goto end; /* bad pkt */

} else {

pkt_data[j++] = c;

chksum += c;

if (c == (unsigned char)0x10)

got_dle = true;

}

}

/* check pkt chksum */

if ((unsigned char)(-chksum) == buf[len-3])

pkt_good = true;

end:

if (pkt_good) {

#ifdef CONTROLSEND_ENABLE

if(serial)

/* good packet, send ACK */

(void)monitor_control_send((unsigned char *)"\x10\x06\x00\xfa\x10\x03", 6);

#endif

garmin_bin_update(pkt_id, pkt_size, pkt_data);

} else {

#ifdef CONTROLSEND_ENABLE

if(serial)

/* bad packet, send NAK */

(void)monitor_control_send((unsigned char *)"\x10\x15\x00\xeb\x10\x03", 6);

#endif

monitor_log("BAD 0x%02x=", buf[1]);

}

}

static void garmin_bin_wrap(void)

{

(void)delwin(miscwin);

(void)delwin(mid51win);

(void)delwin(mid114win);

}

const struct monitor_object_t garmin_bin_ser_mmt = {

.initialize = garmin_bin_initialize,

.update = garmin_bin_ser_update,

.command = NULL,

.wrap = garmin_bin_wrap,

.min_y = 16,.min_x = 80,

.driver = &driver_garmin_ser_binary,

};

#endif /* defined(GARMIN_ENABLE) && defined(BINARY_ENABLE) */