garmin.c

gpsd, a popular GPS daemon.

#endif /* NTPSHM_ENABLE */

	gpsd_report(LOG_PROG, "Appl, mode %d, status %d\n"
	    , session->gpsdata.fix.mode
	    , session->gpsdata.status);

	gpsd_report(LOG_INF, "UTC Time: %lf\n", session->gpsdata.fix.time);
	gpsd_report(LOG_INF
	    , "Geoid Separation (MSL-WGS84): from garmin %lf, calculated %lf\n"
	    , -pvt->msl_hght
	    , wgs84_separation(session->gpsdata.fix.latitude
	    , session->gpsdata.fix.longitude));

	gpsd_report(LOG_INF, "Alt: %.3f, Epe: %.3f, Eph: %.3f, Epv: %.3f, Fix: %d, Gps_tow: %f, Lat: %.3f, Lon: %.3f, LonVel: %.3f, LatVel: %.3f, AltVel: %.3f, MslHgt: %.3f, Leap: %d, GarminDays: %ld\n"
	    , pvt->alt
	    , pvt->epe
	    , pvt->eph
	    , pvt->epv
	    , pvt->fix
	    , pvt->gps_tow
	    , session->gpsdata.fix.latitude
	    , session->gpsdata.fix.longitude
	    , pvt->lon_vel
	    , pvt->lat_vel
	    , pvt->alt_vel
	    , pvt->msl_hght
	    , pvt->leap_sec
	    , pvt->grmn_days);

	mask |= TIME_SET | LATLON_SET | ALTITUDE_SET | STATUS_SET | MODE_SET | SPEED_SET | TRACK_SET | CLIMB_SET | HERR_SET | VERR_SET | PERR_SET | CYCLE_START_SET;
	break;
    case GARMIN_PKTID_RMD_DATA:
    case GARMIN_PKTID_RMD41_DATA:
	rmd = (cpo_rcv_data *) buf;
	gpsd_report(LOG_IO, "PVT RMD Data Sz: %d\n", pkt_len);
	gpsd_report(LOG_PROG, "PVT RMD rcvr_tow: %f, rcvr_wn: %d\n"
		, rmd->rcvr_tow, rmd->rcvr_wn);
        for ( i = 0 ; i < GARMIN_CHANNELS ; i++ ) {
	    gpsd_report(LOG_INF, "PVT RMD Sat: %3u, cycles: %9lu, pr: %16.6f, phase: %7.3f, slp_dtct: %3s, snr: %3u, Valid: %3s\n"
		, rmd->sv[i].svid + 1, rmd->sv[i].cycles, rmd->sv[i].pr
                , (rmd->sv[i].phase * 360.0)/2048.0
                , rmd->sv[i].slp_dtct!='\0' ? "Yes" : "No"
                , rmd->sv[i].snr_dbhz, rmd->sv[i].valid!='\0' ? "Yes" : "No");
	}
	break;

    case GARMIN_PKTID_SAT_DATA:
	gpsd_report(LOG_PROG, "SAT Data Sz: %d\n", pkt_len);
	sats = (cpo_sat_data *)buf;

	session->gpsdata.satellites_used = 0;
	memset(session->gpsdata.used,0,sizeof(session->gpsdata.used));
	gpsd_zero_satellites(&session->gpsdata);
	for ( i = 0, j = 0 ; i < GARMIN_CHANNELS ; i++, sats++ ) {
	    gpsd_report(LOG_INF,"  Sat %3d, snr: %5d, elev: %2d, Azmth: %3d, Stat: %x\n"
		, sats->svid
		, sats->snr
		, sats->elev
		, sats->azmth
		, sats->status);

	    if ( 255 == (int)sats->svid ) {
		// Garmin uses 255 for empty
		// gpsd uses 0 for empty
		continue;
	    }

	    session->gpsdata.PRN[j]       = (int)sats->svid;
	    session->gpsdata.azimuth[j]   = (int)sats->azmth;
	    session->gpsdata.elevation[j] = (int)sats->elev;
            // Garmin does not document this.  snr is in dB*100
	    // Known, but not seen satellites have a dB value of -1*100
            session->gpsdata.ss[j] = (int)round((float)sats->snr / 100);
	    if (session->gpsdata.ss[j] < 0) {
		session->gpsdata.ss[j] = 0;
	    }
	    // FIXME: Garmin documents this, but Daniel Dorau 
	    // <[e-mail]> says the behavior on his GPSMap60CSX
	    // doesn't match it.
	    if ( (uint8_t)0 != (sats->status & 4 ) )  {
	        // used in solution?
	        session->gpsdata.used[session->gpsdata.satellites_used++]
		    = (int)sats->svid;
	    }
	    session->gpsdata.satellites++;
	    j++;

	}
	mask |= SATELLITE_SET | USED_SET;
	break;
    case GARMIN_PKTID_PROTOCOL_ARRAY:
	// this packet is never requested, it just comes, in some case
	// after a GARMIN_PKTID_PRODUCT_RQST 
	gpsd_report(LOG_INF, "Appl, Product Capability, sz: %d\n", pkt_len);
	for ( i = 0; i < pkt_len ; i += 3 ) {
	    gpsd_report(LOG_INF, "  %c%03d\n", buf[i], get_uint16((uint8_t *)&buf[i+1] ) );
	}
	break;
    default:
	gpsd_report(LOG_WARN, "Unknown packet id: %#02x, Sz: %#02x, pkt:%s\n"
		    , pkt_id, pkt_len, gpsd_hexdump(buf, (size_t)pkt_len));
	break;
    }
    gpsd_report(LOG_IO, "PrintSERPacket(, %#02x, %#02x, ) = %#02x\n"
	, pkt_id, pkt_len, mask);
    return mask;
}


/*@ -branchstate @*/
// For debugging, decodes and prints some known packets.
static gps_mask_t PrintUSBPacket(struct gps_device_t *session, Packet_t *pkt)
{
    gps_mask_t mask = 0;
    int maj_ver;
    int min_ver;
    uint32_t mode = 0;
    uint16_t prod_id = 0;
    uint32_t veri = 0;
    uint32_t serial;
    uint32_t mDataSize = get_int32( (uint8_t*)&pkt->mDataSize);

//
    uint8_t *buffer = (uint8_t*)pkt;

    gpsd_report(LOG_PROG, "PrintUSBPacket()\n");
// gem
    if ( DLE == pkt->mPacketType) {
	    gpsd_report(LOG_PROG, "really a SER packet!\n");
            return PrintSERPacket ( session,  
				    (unsigned char)buffer[1], 
				    (int)buffer[2], 
				    (unsigned char*)(buffer + 3));
    }

// gem
    if ( 4096 < mDataSize) {
	gpsd_report(LOG_WARN, "bogus packet, size too large=%d\n", mDataSize);
	return 0;
    }

    (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag), "%u"
	, (unsigned int)pkt->mPacketType);
    switch ( pkt->mPacketType ) {
    case GARMIN_LAYERID_TRANSPORT:
        /* Garmin USB layer specific */
	switch( pkt->mPacketId ) {
	case GARMIN_PKTID_TRANSPORT_START_SESSION_REQ:
	    gpsd_report(LOG_PROG, "Transport, Start Session req\n");
	    break;
	case GARMIN_PKTID_TRANSPORT_START_SESSION_RESP:
	    mode = get_int32(&pkt->mData.uchars[0]);
	    gpsd_report(LOG_PROG, "Transport, Start Session resp, unit: 0x%x\n"
		, mode);
	    break;
	default:
	    gpsd_report(LOG_PROG, "Transport, Packet: Type %d %d %d, ID: %d, Sz: %d\n"
			, pkt->mPacketType
			, pkt->mReserved1
			, pkt->mReserved2
			, pkt->mPacketId
			, mDataSize);
	    break;
	}
	break;
    case GARMIN_LAYERID_APPL:
        /* raw data transport, shared with Garmin Serial Driver */

        mask = PrintSERPacket(session, 
			      (unsigned char)pkt->mPacketId,  
			      (int)mDataSize, 
			      (unsigned char *)pkt->mData.uchars);
	break;
    case 75:
	// private, garmin USB kernel driver specific
	switch( pkt->mPacketId ) {
	case PRIV_PKTID_SET_MODE:
	    prod_id = get_uint16(&pkt->mData.uchars[0]);
	    gpsd_report(LOG_PROG, "Private, Set Mode: %d\n", prod_id);
	    break;
	case PRIV_PKTID_INFO_REQ:
	    gpsd_report(LOG_PROG, "Private, ID: Info Req\n");
	    break;
	case PRIV_PKTID_INFO_RESP:
	    veri = get_int32(pkt->mData.uchars);
	    maj_ver = (int)(veri >> 16);
	    min_ver = (int)(veri & 0xffff);
	    mode = get_int32(&pkt->mData.uchars[4]);
	    serial = get_int32(&pkt->mData.uchars[8]);
	    gpsd_report(LOG_PROG, "Private, ID: Info Resp\n");
	    gpsd_report(LOG_INF, "Garmin USB Driver found, Version %d.%d, Mode: %d, GPS Serial# %u\n"
			,  maj_ver, min_ver, mode, serial);
	    break;
	default:
	    gpsd_report(LOG_PROG, "Private, Packet: ID: %d, Sz: %d\n"
			, pkt->mPacketId
			, mDataSize);
	    break;
	}
	break;
    default:
	gpsd_report(LOG_PROG, "Packet: Type %d %d %d, ID: %d, Sz: %d\n"
		    , pkt->mPacketType
		    , pkt->mReserved1
		    , pkt->mReserved2
		    , pkt->mPacketId
		    , mDataSize);
	break;
    }

    return mask;
}
/*@ +branchstate @*/


/* build and send a packet w/ USB protocol */
static void Build_Send_USB_Packet( struct gps_device_t *session,
       uint32_t layer_id, uint32_t pkt_id, uint32_t length, uint32_t data ) 
{
        uint8_t *buffer = (uint8_t *)session->driver.garmin.Buffer;
	Packet_t *thePacket = (Packet_t*)buffer;
	ssize_t theBytesReturned = 0;
	ssize_t theBytesToWrite = 12 + (ssize_t)length;

	set_int32(buffer, layer_id);
	set_int32(buffer+4, pkt_id);
	set_int32(buffer+8, length); 
        if ( 2 == length ) {
		set_int16(buffer+12, data);
        } else if ( 4 == length ) {
		set_int32(buffer+12, data);
	}

#if 0
        gpsd_report(LOG_IO, "SendPacket(), writing %d bytes: %s\n"
		, theBytesToWrite, gpsd_hexdump(thePacket, theBytesToWrite));
#endif
        (void)PrintUSBPacket ( session,  thePacket);

	theBytesReturned = gpsd_write( session , thePacket,
	    (size_t)theBytesToWrite);
	gpsd_report(LOG_IO, "SendPacket(), wrote %d bytes\n", theBytesReturned);

	// Garmin says:
	// If the packet size was an exact multiple of the USB packet
	// size, we must make a final write call with no data

	// as a practical matter no known packets are 64 bytes long so
        // this is untested

	// So here goes just in case
	if( 0 == (theBytesToWrite % ASYNC_DATA_SIZE) ) {
		char *n = "";
		theBytesReturned = gpsd_write( session , &n, 0);
	}
}
/* build and send a packet in serial protocol */
/* layer_id unused */
static void Build_Send_SER_Packet( struct gps_device_t *session,
       uint32_t layer_id UNUSED, uint32_t pkt_id, uint32_t length, uint32_t data ) 
{
        uint8_t *buffer = (uint8_t *)session->driver.garmin.Buffer;
        uint8_t *buffer0 = buffer;
	Packet_t *thePacket = (Packet_t*)buffer;
	ssize_t theBytesReturned = 0;
	ssize_t theBytesToWrite = 6 + (ssize_t)length;
        uint8_t chksum = 0;

	*buffer++ = (uint8_t)DLE;
	*buffer++ = (uint8_t)pkt_id;
	chksum = pkt_id;
	*buffer++ = (uint8_t)length; 
	chksum += length;
        if ( 2 == length ) {
		/* carefull!  no DLE stuffing here! */
		set_int16(buffer, data);
		chksum += buffer[0];
		chksum += buffer[1];
        } else if ( 4 == length ) {
		/* carefull!  no DLE stuffing here! */
		set_int32(buffer, data);
		chksum += buffer[0];
		chksum += buffer[1];
		chksum += buffer[2];
		chksum += buffer[3];
	}
	buffer += length;

	// Add checksum
	*buffer++ = -chksum;
	if ( DLE == -chksum ) {
		/* stuff another DLE */
		*buffer++ = (uint8_t)DLE;
		theBytesToWrite++;
	}		
	
	// Add DLE, ETX
	*buffer++ = (uint8_t)DLE;
	*buffer++ = (uint8_t)ETX;

#if 1
        gpsd_report(LOG_IO, "SendPacket(), writing %d bytes: %s\n"
		    , theBytesToWrite, gpsd_hexdump(thePacket, (size_t)theBytesToWrite));
#endif
        (void)PrintSERPacket ( session,  
			       (unsigned char)buffer0[1], 
			       (int)buffer0[2], 
			       (unsigned char *)(buffer0 + 3));

	theBytesReturned = gpsd_write( session, thePacket,
	    (size_t)theBytesToWrite);
	gpsd_report(LOG_IO, "SendPacket(), wrote %d bytes\n", theBytesReturned);

}


/*
 * garmin_detect()
 *
 * check that the garmin_gps driver is installed in the kernel
 * and that an active USB device is using it.
 *
 * It does not yet check that the currect device is the one 
 * attached to the garmin.  So if you have a garmin and another 
 * gps this could be a problem.
 *
 * this is very linux specific.  
 *
 * return 1 if garmin_gps device found
 * return 0 if not
 *
 */
static bool garmin_detect(struct gps_device_t *session)
{

    FILE *fp = NULL;
    char buf[256];
    bool ok = false;

    /* check for garmin USB serial driver -- very Linux-specific */
    if (access("/sys/module/garmin_gps", R_OK) != 0) {
	gpsd_report(LOG_WARN, "garmin_gps not active.\n"); 
        return false;
    }
    // check for a garmin_gps device in /proc
    if ( !(fp = fopen( "/proc/bus/usb/devices", "r") ) ) {
	gpsd_report(LOG_ERROR, "Can't open /proc/bus/usb/devices\n");
        return false;
    }

    ok = false;
    while ( 0 != fgets( buf, (int)sizeof(buf), fp ) ) {
	if ( strstr( buf, "garmin_gps") ) {
		ok = true;
		break;
	}
    }
    (void)fclose(fp);
    if ( !ok ) {
        // no device using garmin now
	gpsd_report(LOG_WARN, "garmin_gps not in /proc/bus/usb/devices.\n"); 
	return false;
    }

    if (!gpsd_set_raw(session)) {
	gpsd_report(LOG_ERROR, "garmin_detect: error changing port attributes: %s\n",
             strerror(errno));
	return false;
    }

#ifdef __UNUSED
    Packet_t *thePacket = NULL;
    uint8_t *buffer = NULL;
    /* reset the buffer and buffer length */
    memset( session->driver.garmin.Buffer, 0, sizeof(session->driver.garmin.Buffer) );
    session->driver.garmin.BufferLen = 0;

    if (sizeof(session->driver.garmin.Buffer) < sizeof(Packet_t)) {
	gpsd_report(LOG_ERROR, "garmin_detect: Compile error, garmin.Buffer too small.\n",
             strerror(errno));
	return false;
    }

    buffer = (uint8_t *)session->driver.garmin.Buffer;
    thePacket = (Packet_t*)buffer;
#endif /* __UNUSED__ */

    // set Mode 1, mode 0 is broken somewhere past 2.6.14
    // but how?
    gpsd_report(LOG_PROG, "Set garmin_gps driver mode = 0\n");
    Build_Send_USB_Packet( session, GARMIN_LAYERID_PRIVATE
        , PRIV_PKTID_SET_MODE, 4, MODE_GARMIN_SERIAL);
    // expect no return packet !?

    return true;
}

static void garmin_probe_subtype(struct gps_device_t *session, unsigned int seq)
{
    if (seq == 0) {
        // Tell the device to send product data
        gpsd_report(LOG_PROG, "Get Garmin Product Data\n");
        Build_Send_SER_Packet(session, GARMIN_LAYERID_APPL
           , GARMIN_PKTID_PRODUCT_RQST, 0, 0);

	// turn on PVT data 49