/* $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 <gem@rellim.com> * 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 USBtypedef 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 USBtypedef 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 degreesstatic 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);