garmin.c

gpsd, a popular GPS daemon.

/* $Id: garmin.c 4661 2008-01-19 22:54:23Z garyemiller $ */
/*
 * Handle the Garmin binary packet format supported by the USB Garmins
 * tested with the Garmin 18 and other models.  This driver is NOT for
 * serial port connected Garmins, they provide adequate NMEA support.
 *
 * This code is partly from the Garmin IOSDK and partly from the
 * sample code in the Linux garmin_gps driver.
 *
 * This code supports both Garmin on a serial port and USB Garmins.
 *
 * USB Garmins need the Linux garmin_gps driver and will not function
 * without it.  This code has been tested and at least at one time is
 * known to work on big- and little-endian CPUs and 32 and 64 bit cpu
 * modes.
 *
 * Protocol info from:
 *	 425_TechnicalSpecification.pdf
 *	 ( formerly GPS18_TechnicalSpecification.pdf )
 *	 iop_spec.pdf
 * http://www.garmin.com/support/commProtocol.html
 *
 * bad code by: Gary E. Miller <[e-mail]>
 * all rights abandoned, a thank would be nice if you use this code.
 *
 * -D 3 = packet trace
 * -D 4 = packet details
 * -D 5 = more packet details
 * -D 6 = very excessive details
 *
 * limitations:
 *
 * do not have from garmin:
 *      pdop
 *      hdop
 *      vdop
 *	magnetic variation
 *
 * known bugs:
 *      hangs in the fread loop instead of keeping state and returning.
 *      may or may not work on a little-endian machine
 */

#define __USE_POSIX199309 1
#include <sys/types.h>
#include <time.h> // for nanosleep()

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <inttypes.h>

#include "gpsd_config.h"
#if defined (HAVE_SYS_SELECT_H)
#include <sys/select.h>
#endif

#if defined(HAVE_STRINGS_H)
#include <strings.h>
#endif

#include "gpsd.h"
#include "gps.h"

#ifdef GARMIN_ENABLE

#define USE_RMD 0

#define ETX 0x03
#define ACK 0x06
#define DLE 0x10
#define NAK 0x15

#define GARMIN_LAYERID_TRANSPORT (uint8_t)  0
#define GARMIN_LAYERID_APPL      (uint32_t) 20
// Linux Garmin USB driver layer-id to use for some control mechanisms
#define GARMIN_LAYERID_PRIVATE  0x01106E4B

// packet ids used in private layer
#define PRIV_PKTID_SET_DEBUG    1
#define PRIV_PKTID_SET_MODE     2
#define PRIV_PKTID_INFO_REQ     3
#define PRIV_PKTID_INFO_RESP    4
#define PRIV_PKTID_RESET_REQ    5
#define PRIV_PKTID_SET_DEF_MODE 6

#define MODE_NATIVE          0
#define MODE_GARMIN_SERIAL   1

#define GARMIN_PKTID_TRANSPORT_START_SESSION_REQ 5
#define GARMIN_PKTID_TRANSPORT_START_SESSION_RESP 6

#define GARMIN_PKTID_PROTOCOL_ARRAY     253
#define GARMIN_PKTID_PRODUCT_RQST       254
#define GARMIN_PKTID_PRODUCT_DATA       255
/* 0x29 ')' */
#define GARMIN_PKTID_RMD41_DATA         41
/* 0x33 '3' */
#define GARMIN_PKTID_PVT_DATA           51
/* 0x33 '4' */
#define GARMIN_PKTID_RMD_DATA           52
/* 0x72 'r' */
#define GARMIN_PKTID_SAT_DATA           114

#define GARMIN_PKTID_L001_XFER_CMPLT     12
#define GARMIN_PKTID_L001_COMMAND_DATA   10
#define GARMIN_PKTID_L001_DATE_TIME_DATA 14
#define GARMIN_PKTID_L001_RECORDS        27
#define GARMIN_PKTID_L001_WPT_DATA       35

#define	CMND_ABORT			 0
#define	CMND_START_PVT_DATA		 49
#define	CMND_STOP_PVT_DATA		 50
#define	CMND_START_RM_DATA		 110

#define MAX_BUFFER_SIZE 4096

#define GARMIN_CHANNELS	12

// something magic about 64, garmin driver will not return more than
// 64 at a time.  If you read less than 64 bytes the next read will
// just get the last of the 64 byte buffer.
#define ASYNC_DATA_SIZE 64


#pragma pack(1)
// This is the data format of the satellite data from the garmin USB
typedef struct {
	uint8_t  svid;
	int16_t snr; // 0 - 0xffff
	uint8_t  elev;
	uint16_t azmth;
	uint8_t  status; // bit 0, has ephemeris, 1, has diff correction
                               // bit 2 used in solution
			       // bit 3??
} cpo_sat_data;

/* Garmin D800_Pvt_Datetype_Type */
/* packet type:  GARMIN_PKTID_PVT_DATA   52 */
/* This is the data format of the position data from the garmin USB */
typedef struct {
	float alt;  /* altitude above WGS 84 (meters) */
	float epe;  /* estimated position error, 2 sigma (meters)  */
	float eph;  /* epe, but horizontal only (meters) */
	float epv;  /* epe but vertical only (meters ) */
	int16_t	fix; /* 0 - failed integrity check
                      * 1 - invalid or unavailable fix
                      * 2 - 2D
                      * 3 - 3D
		      * 4 - 2D Diff
                      * 5 - 3D Diff
                      */
	double	gps_tow; /* gps time  os week (seconds) */
	double	lat;     /* ->latitude (radians) */
	double	lon;     /* ->longitude (radians) */
	float	lon_vel; /* velocity east (meters/second) */
	float	lat_vel; /* velocity north (meters/second) */
	float	alt_vel; /* velocity up (meters/sec) */
        // Garmin GPS25 uses pkt_id 0x28 and does not output the 
        // next 3 items
	float	msl_hght; /* height of WGS 84 above MSL (meters) */
	int16_t	leap_sec; /* diff between GPS and UTC (seconds) */
	int32_t	grmn_days;
} cpo_pvt_data;

typedef struct {
	uint32_t cycles;
	double	 pr;
	uint16_t phase;
	int8_t slp_dtct;
	uint8_t snr_dbhz;
	uint8_t  svid;
	int8_t valid;
} cpo_rcv_sv_data;

/* packet type:  GARMIN_PKTID_RMD_DATA   53 */
/* seems identical to the packet id 0x29 from the Garmin GPS 25 */
typedef struct {
	double rcvr_tow;
	int16_t	rcvr_wn;
	cpo_rcv_sv_data sv[GARMIN_CHANNELS];
} cpo_rcv_data;

// This is the packet format to/from the Garmin USB
typedef struct {
    uint8_t  mPacketType;
    uint8_t  mReserved1;
    uint16_t mReserved2;
    uint16_t mPacketId;
    uint16_t mReserved3;
    uint32_t  mDataSize;
    union {
	    int8_t chars[MAX_BUFFER_SIZE];
	    uint8_t uchars[MAX_BUFFER_SIZE];
            cpo_pvt_data pvt;
            cpo_sat_data sats;
    } mData;
} Packet_t;

// useful funcs to read/write ints
//  floats and doubles are Intel order only...
static inline void set_int16(uint8_t *buf, uint32_t value)
{
        buf[0] = (uint8_t)(0x0FF & value);
        buf[1] = (uint8_t)(0x0FF & (value >> 8));
}

static inline void set_int32(uint8_t *buf, uint32_t value)
{
        buf[0] = (uint8_t)(0x0FF & value);
        buf[1] = (uint8_t)(0x0FF & (value >> 8));
        buf[2] = (uint8_t)(0x0FF & (value >> 16));
        buf[3] = (uint8_t)(0x0FF & (value >> 24));
}

static inline uint16_t get_uint16(const uint8_t *buf)
{
        return  (uint16_t)(0xFF & buf[0]) 
		| ((uint16_t)(0xFF & buf[1]) << 8);
}

static inline uint32_t get_int32(const uint8_t *buf)
{
        return  (uint32_t)(0xFF & buf[0]) 
		| ((uint32_t)(0xFF & buf[1]) << 8) 
		| ((uint32_t)(0xFF & buf[2]) << 16) 
		| ((uint32_t)(0xFF & buf[3]) << 24);
}

// convert radians to degrees
static inline double  radtodeg( double rad) {
	return (double)(rad * RAD_2_DEG );
}

static gps_mask_t PrintSERPacket(struct gps_device_t *session, unsigned char pkt_id, int pkt_len, unsigned char *buf );
static gps_mask_t PrintUSBPacket(struct gps_device_t *session, Packet_t *pkt );

gps_mask_t PrintSERPacket(struct gps_device_t *session, unsigned char pkt_id
	, int pkt_len, unsigned char *buf ) 
{

    gps_mask_t mask = 0;
    int i = 0, j = 0;
    uint16_t prod_id = 0;
    uint16_t ver = 0;
    int maj_ver;
    int min_ver;
    time_t time_l = 0;
    double track;
    char msg_buf[512] = "";
    char *msg = NULL;
    cpo_sat_data *sats = NULL;
    cpo_pvt_data *pvt = NULL;
    cpo_rcv_data *rmd = NULL;

    gpsd_report(LOG_IO, "PrintSERPacket(, %#02x, %#02x, )\n", pkt_id, pkt_len);

    switch( pkt_id ) {
    case ACK:
	gpsd_report(LOG_PROG, "ACK\n");
	break;
    case NAK:
	gpsd_report(LOG_PROG, "NAK\n");
	break;
    case GARMIN_PKTID_L001_COMMAND_DATA:
	prod_id = get_uint16((uint8_t *)buf);
	/*@ -branchstate @*/
	switch ( prod_id ) {
	case CMND_ABORT:
	    msg = "Abort current xfer";
	    break;
	case CMND_START_PVT_DATA:
	    msg = "Start Xmit PVT data";
	    break;
	case CMND_STOP_PVT_DATA:
	    msg = "Stop Xmit PVT data";
	    break;
	case CMND_START_RM_DATA:
	    msg = "Start RMD data";
	    break;
	default:
	    (void)snprintf(msg_buf, sizeof(msg_buf), "Unknown: %u", 
			(unsigned int)prod_id);
	    msg = msg_buf;
	    break;
	}
	/*@ +branchstate @*/
	gpsd_report(LOG_PROG, "Appl, Command Data: %s\n", msg);
	break;
    case GARMIN_PKTID_PRODUCT_RQST:
	gpsd_report(LOG_PROG, "Appl, Product Data req\n");
	break;
    case GARMIN_PKTID_PRODUCT_DATA:
	prod_id = get_uint16((uint8_t *)buf);
	ver = get_uint16((uint8_t *)&buf[2]);
	maj_ver = (int)(ver / 100);
	min_ver = (int)(ver - (maj_ver * 100));
	gpsd_report(LOG_PROG, "Appl, Product Data, sz: %d\n", pkt_len);
	(void)snprintf(session->subtype, sizeof(session->subtype),
		       "%d: %d.%02d"
		       , (int)prod_id, maj_ver, min_ver);
	gpsd_report(LOG_INF, "Garmin Product ID: %d, SoftVer: %d.%02d\n"
		, prod_id, maj_ver, min_ver);
	gpsd_report(LOG_INF, "Garmin Product Desc: %s\n"
		, &buf[4]);
	mask |= DEVICEID_SET;
	break;
    case GARMIN_PKTID_PVT_DATA:
	gpsd_report(LOG_PROG, "Appl, PVT Data Sz: %d\n", pkt_len);

	pvt = (cpo_pvt_data*) buf;

	// 631065600, unix seconds for 31 Dec 1989 Zulu 
	time_l = (time_t)(631065600 + (pvt->grmn_days * 86400));
	time_l -= pvt->leap_sec;
	session->context->leap_seconds = pvt->leap_sec;
	session->context->valid = LEAP_SECOND_VALID;
	// gps_tow is always like x.999 or x.998 so just round it
	time_l += (time_t) round(pvt->gps_tow);
	session->gpsdata.fix.time 
	  = session->gpsdata.sentence_time 
	  = (double)time_l;
	gpsd_report(LOG_PROG, "time_l: %ld\n", (long int)time_l);

	session->gpsdata.fix.latitude = radtodeg(pvt->lat);
	/* sanity check the lat */
	if ( 90.0 < session->gpsdata.fix.latitude ) {
		session->gpsdata.fix.latitude = 90.0;
		gpsd_report(LOG_INF, "ERROR: Latitude overrange\n");
	} else if ( -90.0 > session->gpsdata.fix.latitude ) {
		session->gpsdata.fix.latitude = -90.0;
		gpsd_report(LOG_INF, "ERROR: Latitude negative overrange\n");
	}
	session->gpsdata.fix.longitude = radtodeg(pvt->lon);
	/* sanity check the lon */
	if ( 180.0 < session->gpsdata.fix.longitude ) {
		session->gpsdata.fix.longitude = 180.0;
		gpsd_report(LOG_INF, "ERROR: Longitude overrange\n");
	} else if ( -180.0 > session->gpsdata.fix.longitude ) {
		session->gpsdata.fix.longitude = -180.0;
		gpsd_report(LOG_INF, "ERROR: Longitude negative overrange\n");
	}

	// altitude over WGS84 converted to MSL
	session->gpsdata.fix.altitude = pvt->alt + pvt->msl_hght;

	// geoid separation from WGS 84
	// gpsd sign is opposite of garmin sign
	session->gpsdata.separation = -pvt->msl_hght;

	// Estimated position error in meters.
	// We follow the advice at <http://gpsinformation.net/main/errors.htm>.
	// If this assumption changes here, it should also change in 
	// nmea_parse.c where we analyze PGRME.
	session->gpsdata.epe = pvt->epe * (GPSD_CONFIDENCE/CEP50_SIGMA);
	session->gpsdata.fix.eph = pvt->eph * (GPSD_CONFIDENCE/CEP50_SIGMA);
	session->gpsdata.fix.epv = pvt->epv * (GPSD_CONFIDENCE/CEP50_SIGMA);

	// convert lat/lon to directionless speed
	session->gpsdata.fix.speed = hypot(pvt->lon_vel, pvt->lat_vel);

	// keep climb in meters/sec
	session->gpsdata.fix.climb = pvt->alt_vel;

	track = atan2(pvt->lon_vel, pvt->lat_vel);
	if (track < 0) {
	    track += 2 * PI;
	}
	session->gpsdata.fix.track = radtodeg(track);

	switch ( pvt->fix) {
	case 0:
	case 1:
	default:
	    // no fix
	    session->gpsdata.status = STATUS_NO_FIX;
	    session->gpsdata.fix.mode = MODE_NO_FIX;
	    break;
	case 2:
	    // 2D fix
	    session->gpsdata.status = STATUS_FIX;
	    session->gpsdata.fix.mode = MODE_2D;
	    break;
	case 3:
	    // 3D fix
	    session->gpsdata.status = STATUS_FIX;
	    session->gpsdata.fix.mode = MODE_3D;
	    break;
	case 4:
	    // 2D Differential fix
	    session->gpsdata.status = STATUS_DGPS_FIX;
	    session->gpsdata.fix.mode = MODE_2D;
	    break;
	case 5:
	    // 3D differential fix
	    session->gpsdata.status = STATUS_DGPS_FIX;
	    session->gpsdata.fix.mode = MODE_3D;
	    break;
	}
#ifdef NTPSHM_ENABLE
	if (session->context->enable_ntpshm && session->gpsdata.fix.mode > MODE_NO_FIX)
	    (void) ntpshm_put(session, session->gpsdata.fix.time);