garmin.c

gpsd, a popular GPS daemon.

	gpsd_report(LOG_PROG, "Set Garmin to send reports every 1 second\n");

        Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL
	    , GARMIN_PKTID_L001_COMMAND_DATA, 2, CMND_START_PVT_DATA);

#if USE_RMD
	// turn on RMD data 110
	gpsd_report(LOG_PROG, "Set Garmin to send Raw sat data\n");
        Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL
	      , GARMIN_PKTID_L001_COMMAND_DATA, 2, CMND_START_RM_DATA);
#endif
    }
}

static void garmin_close(struct gps_device_t *session UNUSED) 
{
    /* FIXME -- do we need to put the garmin to sleep?  or is closing the port
       sufficient? */
    gpsd_report(LOG_PROG, "garmin_close()\n");
    return;
}

#define Send_ACK()    Build_Send_SER_Packet(session, 0, ACK, 0, 0)
#define Send_NAK()    Build_Send_SER_Packet(session, 0, NAK, 0, 0)

/*@ +charint @*/
gps_mask_t garmin_ser_parse(struct gps_device_t *session)
{
    unsigned char *buf = session->packet.outbuffer;
    size_t len = session->packet.outbuflen;
    unsigned char data_buf[MAX_BUFFER_SIZE];
    unsigned char c;
    int i = 0;
    size_t n = 0;
    int data_index = 0;
    int got_dle = 0;
    unsigned char pkt_id = 0;
    unsigned char pkt_len = 0;
    unsigned char chksum = 0;
    gps_mask_t mask = 0;

    gpsd_report(LOG_RAW, "garmin_ser_parse()\n");
    if (  6 > len ) {
	/* WTF? */
        /* minimum packet; <DLE> [pkt id] [length=0] [chksum] <DLE> <STX> */
	Send_NAK();
	gpsd_report(LOG_RAW+1, "Garmin serial too short: %#2x\n", len);
	return 0;
    }
    /* debug */
    for ( i = 0 ; i < (int)len ; i++ ) {
	gpsd_report(LOG_RAW+1, "Char: %#02x\n", buf[i]);
    }
    
    if ( '\x10' != buf[0] ) {
	Send_NAK();
	gpsd_report(LOG_RAW+1, "buf[0] not DLE\n", buf[0]);
        return 0;
    }
    n = 1;
    pkt_id = buf[n++];
    chksum = pkt_id;
    if ( '\x10' == pkt_id ) {
        if ( '\x10' != buf[n++] ) {
	    Send_NAK();
	    gpsd_report(LOG_RAW+1, "Bad pkt_id %#02x\n", pkt_id);
	    return 0;
        }
    }

    pkt_len = buf[n++];
    chksum += pkt_len;
    if ( '\x10' == pkt_len ) {
        if ( '\x10' != buf[n++] ) {
	    gpsd_report(LOG_RAW+1, "Bad pkt_len %#02x\n", pkt_len);
	    Send_NAK();
	    return 0;
        }
    }
    data_index = 0;
    for ( i = 0; i < 256 ; i++ ) {

	if ( (int)pkt_len == data_index )  {
		// got it all
		break;
	}
        if ( len < n + i ) {
	    gpsd_report(LOG_RAW+1, "Packet too short %#02x < %#0x\n", len, n + i);
	    Send_NAK();
	    return 0;
        }
	c = buf[n + i];
        if ( got_dle ) {
	    got_dle = 0;
            if ( '\x10' != c ) {
		Send_NAK();
	        gpsd_report(LOG_RAW+1, "Bad DLE %#02x\n", c);
	        return 0;
            }
	} else {
            chksum += c;
	    data_buf[ data_index++ ] = c;
            if ( '\x10' == c ) {
		got_dle = 1;
	    }
	}
    }
    /* get checksum */
    if ( len < n + i ) {
	Send_NAK();
        gpsd_report(LOG_RAW+1, "No checksum, Packet too short %#02x < %#0x\n"
	    , len, n + i);
        return 0;
    }
    c = buf[n + i++];
    chksum += c;
    /* get final DLE */
    if ( len < n + i ) {
	Send_NAK();
        gpsd_report(LOG_RAW+1, "No final DLE, Packet too short %#02x < %#0x\n"
	    , len, n + i);
        return 0;
    }
    c = buf[n + i++];
    if ( '\x10' != c ) {
	Send_NAK();
	gpsd_report(LOG_RAW+1, "Final DLE not DLE\n", c);
        return 0;
    }
    /* get final ETX */
    if ( len < n + i ) {
	Send_NAK();
        gpsd_report(LOG_RAW+1, "No final ETX, Packet too short %#02x < %#0x\n"
	    , len, n + i);
        return 0;
    }
    c = buf[n + i++];
    if ( '\x03' != c ) {
	Send_NAK();
	gpsd_report(LOG_RAW+1, "Final ETX not ETX\n", c);
        return 0;
    }

    /* debug */
    /*@ -usedef -compdef @*/
    for ( i = 0 ; i < data_index ; i++ ) {
	gpsd_report(LOG_RAW+1, "Char: %#02x\n", data_buf[i]);
    }


    gpsd_report(LOG_IO
	, "garmin_ser_parse() Type: %#02x, Len: %#02x, chksum: %#02x\n"
        , pkt_id, pkt_len, chksum);

    mask = PrintSERPacket(session, pkt_id, pkt_len, data_buf);

    // sending ACK too soon might hang the session
    // so send ACK last, after a pause
    (void)usleep(300);
    Send_ACK();
    /*@ +usedef +compdef @*/
    return mask;
}
/*@ -charint @*/

#ifdef ALLOW_RECONFIGURE
static void settle(void)
{
    struct timespec delay, rem;
    /*@ -type -unrecog @*/
    memset( &delay, 0, sizeof(delay));
    delay.tv_sec = 0;
    delay.tv_nsec = 333000000L;
    nanosleep(&delay, &rem);
    /*@ +type +unrecog @*/
}
#endif /* ALLOW_RECONFIGURE */

static void garmin_switcher(struct gps_device_t *session, int mode)
{
#ifdef ALLOW_RECONFIGURE
    if (mode == 0) {
	const char *switcher = "\x10\x0A\x02\x26\x00\xCE\x10\x03";
	int status = (int)gpsd_write(session, switcher, strlen(switcher));
	if (status == (int)strlen(switcher)) {
	    gpsd_report(LOG_IO, "=> GPS: turn off binary %02x %02x %02x... \n"
			, switcher[0], switcher[1], switcher[2]);
	} else {
	    gpsd_report(LOG_ERROR, "=> GPS: FAILED\n");
	}
	settle(); // wait 333mS, essential!

	/* once a sec, no binary, no averaging, NMEA 2.3, WAAS */
	(void)nmea_send(session->gpsdata.gps_fd, "$PGRMC1,1,1");
	//(void)nmea_send(fd, "$PGRMC1,1,1,1,,,,2,W,N");
	(void)nmea_send(session->gpsdata.gps_fd, "$PGRMI,,,,,,,R");
	settle();    // wait 333mS, essential!
    } else {
	(void)nmea_send(session->gpsdata.gps_fd, "$PGRMC1,1,2,1,,,,2,W,N");
	(void)nmea_send(session->gpsdata.gps_fd, "$PGRMI,,,,,,,R");
	// garmin serial binary is 9600 only!
	gpsd_report(LOG_ERROR, "NOTE: Garmin binary is 9600 baud only!\n");
	settle();	// wait 333mS, essential!
    }
#endif /* ALLOW_RECONFIGURE */
}

/* this is everything we export */
#ifdef __UNUSED__
static int GetPacket (struct gps_device_t *session );
//-----------------------------------------------------------------------------
// Gets a single packet.
// this is odd, the garmin usb driver will only return 64 bytes, or less
// at a time, no matter what you ask for.
//
// is you ask for less than 64 bytes then the next packet will include
// just the remaining bytes of the last 64 byte packet.
//
// Reading a packet of length Zero, or less than 64, signals the end of 
// the entire packet.
//
// The Garmin sample WinXX code also assumes the same behavior, so
// maybe it is something in the USB protocol.
//
// Return: 0 = got a good packet
//         -1 = error
//         1 = got partial packet
static int GetPacket (struct gps_device_t *session ) 
{
    struct timespec delay, rem;
    int cnt = 0;
    // int x = 0; // for debug dump

    memset( session->driver.garmin.Buffer, 0, sizeof(Packet_t));
    memset( &delay, 0, sizeof(delay));
    session->driver.garmin.BufferLen = 0;
    session->packet.outbuflen = 0;

    gpsd_report(LOG_IO, "GetPacket()\n");

    for( cnt = 0 ; cnt < 10 ; cnt++ ) {
	size_t pkt_size;
	// Read async data until the driver returns less than the
	// max async data size, which signifies the end of a packet

	// not optimal, but given the speed and packet nature of
	// the USB not too bad for a start
	ssize_t theBytesReturned = 0;
	uint8_t *buf = (uint8_t *)session->driver.garmin.Buffer;
	Packet_t *thePacket = (Packet_t*)buf;

	theBytesReturned = read(session->gpsdata.gps_fd
		, buf + session->driver.garmin.BufferLen
		, ASYNC_DATA_SIZE);
	// zero byte returned is a legal value and denotes the end of a 
        // binary packet.
        if ( 0 >  theBytesReturned ) {
	    // read error...
            // or EAGAIN, but O_NONBLOCK is never set
	    gpsd_report(LOG_ERROR, "GetPacket() read error=%d, errno=%d\n"
		, theBytesReturned, errno);
	    continue;
	}
	gpsd_report(LOG_RAW, "got %d bytes\n", theBytesReturned);
#if 1
        gpsd_report(LOG_IO, "getPacket(), got %d bytes: %s\n"
		, theBytesReturned, gpsd_hexdump(thePacket, theBytesReturned));
#endif

	session->driver.garmin.BufferLen += theBytesReturned;
	if ( 256 <=  session->driver.garmin.BufferLen ) {
	    // really bad read error...
	    gpsd_report(LOG_ERROR, "GetPacket() packet too long, %ld > 255 !\n"
		    , session->driver.garmin.BufferLen);
	    session->driver.garmin.BufferLen = 0;
	    break;
	}
	pkt_size = 12 + get_int32((uint8_t*)&thePacket->mDataSize);
	if ( 12 <= session->driver.garmin.BufferLen) {
	    // have enough data to check packet size
	    if ( session->driver.garmin.BufferLen > pkt_size) {
	        // wrong amount of data in buffer
	        gpsd_report(LOG_ERROR
		    , "GetPacket() packet size wrong! Packet: %ld, s/b %ld\n"
		    , session->driver.garmin.BufferLen
		    , pkt_size);
	        session->driver.garmin.BufferLen = 0;
	        break;
	    }
	}
	if ( 64 > theBytesReturned ) {
	    // zero length, or short, read is a flag for got the whole packet
            break;
	}
		

	/*@ ignore @*/
	delay.tv_sec = 0;
	delay.tv_nsec = 3330000L;
	while (nanosleep(&delay, &rem) < 0)
	    continue;
	/*@ end @*/
    }
    // dump the individual bytes, debug only
    // for ( x = 0; x < session->driver.garmin.BufferLen; x++ ) {
        // gpsd_report(LOG_RAW+1, "p[%d] = %x\n", x, session->driver.garmin.Buffer[x]);
    // }
    if ( 10 <= cnt ) {
	    gpsd_report(LOG_ERROR, "GetPacket() packet too long or too slow!\n");
	    return -1;
    }

    gpsd_report(LOG_RAW, "GotPacket() sz=%d \n", session->driver.garmin.BufferLen);
    session->packet.outbuflen = session->driver.garmin.BufferLen;
    return 0;
}
static gps_mask_t garmin_usb_parse(struct gps_device_t *session)
{
    gpsd_report(LOG_PROG, "garmin_usb_parse()\n");
    return PrintUSBPacket(session, (Packet_t*)session->driver.garmin.Buffer);
}

static ssize_t garmin_get_packet(struct gps_device_t *session) 
{
    return (ssize_t)( 0 == GetPacket( session ) ? 1 : 0);
}

struct gps_type_t garmin_usb_binary_old =
{
    .type_name      = "Garmin USB binary",	/* full name of type */
    .trigger        = NULL,		/* no trigger, it has a probe */
    .channels       = GARMIN_CHANNELS,	/* consumer-grade GPS */
    .probe_wakeup   = NULL,		/* no wakeup to be done before hunt */
    .probe_detect   = garmin_detect,	/* how to detect at startup time */
    .probe_subtype  = garmin_probe_subtype,	/* get subtype info */
#ifdef ALLOW_RECONFIGURE
    .configurator   = garmin_usb_configure,	/* eable what we need */
#endif /* ALLOW_RECONFIGURE */
    .get_packet     = garmin_get_packet,/* how to grab a packet */
    .parse_packet   = garmin_usb_parse,	/* parse message packets */
    .rtcm_writer    = NULL,		/* don't send DGPS corrections */
    .speed_switcher = NULL,		/* no speed switcher */
    .mode_switcher  = NULL,		/* no mode switcher */
    .rate_switcher  = NULL,		/* no sample-rate switcher */
    .cycle_chars    = -1,		/* not relevant, no rate switch */
#ifdef ALLOW_RECONFIGURE
    .revert         = NULL,		/* no setting-reversion method */
#endif /* ALLOW_RECONFIGURE */
    .wrapup         = garmin_close,	/* close hook */
    .cycle          = 1,		/* updates every second */
};
#endif /* __UNUSED__ */

struct gps_type_t garmin_usb_binary =
{
    .type_name      = "Garmin USB binary",	/* full name of type */
    .trigger        = NULL,		/* no trigger, it has a probe */
    .channels       = GARMIN_CHANNELS,	/* consumer-grade GPS */
    .probe_wakeup   = NULL,		/* no wakeup to be done before hunt */
    .probe_detect   = garmin_detect,	/* how to detect at startup time */
    .probe_subtype  = garmin_probe_subtype,	/* get subtype info */
#ifdef ALLOW_RECONFIGURE
    .configurator   = NULL,	        /* enable what we need */
#endif /* ALLOW_RECONFIGURE */
    .get_packet     = generic_get,      /* how to grab a packet */
    .parse_packet   = garmin_ser_parse,	/* parse message packets */
    .rtcm_writer    = NULL,		/* don't send DGPS corrections */
    .speed_switcher = NULL,		/* no speed switcher */
    .mode_switcher  = NULL,		/* no mode switcher */
    .rate_switcher  = NULL,		/* no sample-rate switcher */
    .cycle_chars    = -1,		/* not relevant, no rate switch */
#ifdef ALLOW_RECONFIGURE
    .revert         = NULL,		/* no setting-reversion method */
#endif /* ALLOW_RECONFIGURE */
    .wrapup         = garmin_close,	/* close hook */
    .cycle          = 1,		/* updates every second */
};

struct gps_type_t garmin_ser_binary =
{
    .type_name      = "Garmin Serial binary",	/* full name of type */
    .trigger        = NULL,		/* no trigger, it has a probe */
    .channels       = GARMIN_CHANNELS,	/* consumer-grade GPS */
    .probe_wakeup   = NULL,		/* no wakeup to be done before hunt */
    .probe_detect   = NULL,        	/* how to detect at startup time */
    .probe_subtype  = NULL,        	/* initialize the device */
#ifdef ALLOW_RECONFIGURE
    .configurator   = NULL,	        /* enable what we need */
#endif /* ALLOW_RECONFIGURE */
    .get_packet     = generic_get,       /* how to grab a packet */
    .parse_packet   = garmin_ser_parse,	/* parse message packets */
    .rtcm_writer    = NULL,		/* don't send DGPS corrections */
    .speed_switcher = NULL,		/* no speed switcher */
    .mode_switcher  = garmin_switcher,	/* how to change modes */
    .rate_switcher  = NULL,		/* no sample-rate switcher */
    .cycle_chars    = -1,		/* not relevant, no rate switch */
#ifdef ALLOW_RECONFIGURE
    .revert         = NULL,		/* no setting-reversion method */
#endif /* ALLOW_RECONFIGURE */
    .wrapup         = NULL,	        /* close hook */
    .cycle          = 1,		/* updates every second */
};

#endif /* GARMIN_ENABLE */