sirf.c

如何得到屏幕的分辨率,但是删减了其他部分的代码,所以还需要小修下就才能通过

/*
 * This is the gpsd driver for SiRF-II GPSes operating in binary mode.
 * It also handles uBlox, a SiRF derivative.
 *
 * The advantage: Reports climb/sink rate (raw-mode clients won't see this).
 * The disadvantages: Doesn't return PDOP or VDOP, just HDOP.
 *
 * Chris Kuethe, our SiRF expert, tells us:
 * 
 * "I don't see any indication in any of my material that PDOP, GDOP
 * or VDOP are output. There are quantities called Estimated
 * {Horizontal Position, Vertical Position, Time, Horizonal Velocity}
 * Error, but those are apparently only valid when SiRFDRive is
 * active."
 *
 * "(SiRFdrive is their Dead Reckoning augmented firmware. It
 * allows you to feed odometer ticks, gyro and possibly 
 * accelerometer inputs to the chip to allow it to continue 
 * to navigate in the absence of satellite information, and 
 * to improve fixes when you do have satellites.)"
 *
 * "[When we need RINEX data, we can get it from] SiRF Message #5.
 *  If it's no longer implemented on your receiver, messages
 * 7, 28, 29 and 30 will give you the same information."
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <unistd.h>
#include <time.h>
#include <sys/types.h>
#include <stdio.h>

#include "gpsd.h"
#include "bits.h"
#if defined(SIRFII_ENABLE) && defined(BINARY_ENABLE)

#define HI(n)		((n) >> 8)
#define LO(n)		((n) & 0xff)

bool sirf_write(int fd, unsigned char *msg) {
   unsigned int       crc;
   size_t    i, len;
   char	     buf[MAX_PACKET_LENGTH*2];
   bool      ok;

   len = (size_t)((msg[2] << 8) | msg[3]);

   /* calculate CRC */
   crc = 0;
   for (i = 0; i < len; i++)
	crc += (int)msg[4 + i];
   crc &= 0x7fff;

   /* enter CRC after payload */
   msg[len + 4] = (unsigned char)((crc & 0xff00) >> 8);
   msg[len + 5] = (unsigned char)( crc & 0x00ff);

   buf[0] = '\0';
   for (i = 0; i < len+8; i++)
       (void)snprintf(buf+strlen(buf),sizeof(buf)-strlen(buf),
		      " %02x", (unsigned)msg[i]);
   gpsd_report(4, "Writing SiRF control type %02x:%s\n", msg[4], buf);
   ok = (write(fd, msg, len+8) == (ssize_t)(len+8));
   (void)tcdrain(fd);
   return(ok);
}

static bool sirf_speed(int ttyfd, speed_t speed) 
/* change speed in binary mode */
{
    /*@ +charint @*/
   static unsigned char msg[] = {0xa0, 0xa2, 0x00, 0x09,
                     0x86, 
                     0x0, 0x0, 0x12, 0xc0,	/* 4800 bps */
		     0x08,			/* 8 data bits */
		     0x01,			/* 1 stop bit */
		     0x00,			/* no parity */
		     0x00,			/* reserved */
                     0x00, 0x00, 0xb0, 0xb3};
   /*@ -charint @*/

   msg[7] = (unsigned char)HI(speed);
   msg[8] = (unsigned char)LO(speed);
   return (sirf_write(ttyfd, msg));
}

static bool sirf_to_nmea(int ttyfd, speed_t speed) 
/* switch from binary to NMEA at specified baud */
{
    /*@ +charint @*/
   static unsigned char msg[] = {0xa0, 0xa2, 0x00, 0x18,
                     0x81, 0x02,
                     0x01, 0x01, /* GGA */
                     0x00, 0x00, /* suppress GLL */
                     0x01, 0x01, /* GSA */
                     0x05, 0x01, /* GSV */
                     0x01, 0x01, /* RMC */
                     0x00, 0x00, /* suppress VTG */
                     0x00, 0x01, 0x00, 0x01,
                     0x00, 0x01, 0x00, 0x01,
                     0x12, 0xc0, /* 4800 bps */
                     0x00, 0x00, 0xb0, 0xb3};
   /*@ -charint @*/

   msg[26] = (unsigned char)HI(speed);
   msg[27] = (unsigned char)LO(speed);
   return (sirf_write(ttyfd, msg));
}

static void sirfbin_mode(struct gps_device_t *session, int mode)
{
    if (mode == 0) {
	(void)gpsd_switch_driver(session, "SiRF-II NMEA");
	(void)sirf_to_nmea(session->gpsdata.gps_fd,session->gpsdata.baudrate);
	session->gpsdata.driver_mode = 0;
    }
}

gps_mask_t sirf_parse(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    int	st, i, j, cn;
    unsigned short navtype;
    gps_mask_t mask;
    char buf2[MAX_PACKET_LENGTH*3+2];
    double fv;
    /*@ +charint @*/
    static unsigned char enablesubframe[] = {0xa0, 0xa2, 0x00, 0x19,
				 0x80, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x00,
				 0x00, 0x00, 0x00, 0x0C,
				 0x10,
				 0x00, 0x00, 0xb0, 0xb3};
    static unsigned char disablesubframe[] = {0xa0, 0xa2, 0x00, 0x19,
				  0x80, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x00,
				  0x00, 0x00, 0x00, 0x0C,
				  0x00,
				  0x00, 0x00, 0xb0, 0xb3};

    /*@ -charint @*/
    if (len == 0)
	return 0;

    buf += 4;
    len -= 8;
    gpsd_report(5, "Raw SiRF packet type 0x%02x length %d: %s\n", buf[0],len,
		gpsd_hexdump(buf, len));
    (void)snprintf(session->gpsdata.tag, sizeof(session->gpsdata.tag),
		   "MID%d",(int)buf[0]);

    switch (buf[0])
    {
    case 0x02:		/* Measure Navigation Data Out */
	mask = 0;
	session->gpsdata.satellites_used = (int)getub(buf, 28);
	memset(session->gpsdata.used,0,sizeof(session->gpsdata.used));
	for (i = 0; i < SIRF_CHANNELS; i++)
	    session->gpsdata.used[i] = (int)getub(buf, 29+i);
	if ((session->driver.sirf.driverstate & (SIRF_GE_232 | UBLOX))==0) {
	    /* position/velocity is bytes 1-18 */
	    ecef_to_wgs84fix(&session->gpsdata, 
			     getsl(buf, 1)*1.0, getsl(buf, 5)*1.0, getsl(buf, 9)*1.0,
			     getsw(buf, 13)/8.0, getsw(buf, 15)/8.0, getsw(buf, 17)/8.0);
	    /* fix status is byte 19 */
	    navtype = (unsigned short)getub(buf, 19);
	    session->gpsdata.status = STATUS_NO_FIX;
	    session->gpsdata.newdata.mode = MODE_NO_FIX;
	    if ((navtype & 0x80) != 0)
		session->gpsdata.status = STATUS_DGPS_FIX;
	    else if ((navtype & 0x07) > 0 && (navtype & 0x07) < 7)
		session->gpsdata.status = STATUS_FIX;
	    if ((navtype & 0x07) == 4 || (navtype & 0x07) == 6)
		session->gpsdata.newdata.mode = MODE_3D;
	    else if (session->gpsdata.status != 0)
		session->gpsdata.newdata.mode = MODE_2D;
	    if (session->gpsdata.newdata.mode == MODE_3D)
		mask |= ALTITUDE_SET | CLIMB_SET;
	    gpsd_report(4, "MND 0x02: Navtype = 0x%0x, Status = %d, mode = %d\n", 
			navtype,session->gpsdata.status,session->gpsdata.newdata.mode);
	    /* byte 20 is HDOP, see below */
	    /* byte 21 is "mode 2", not clear how to interpret that */ 
	    session->gpsdata.newdata.time = session->gpsdata.sentence_time
		= gpstime_to_unix(getsw(buf, 22), getul(buf, 24)*1e-2) - session->context->leap_seconds;
#ifdef NTPSHM_ENABLE
	    if (session->gpsdata.newdata.mode > MODE_NO_FIX) {
		if ((session->driver.sirf.time_seen & TIME_SEEN_GPS_2) == 0)
		    gpsd_report(4, "valid time in message 0x02, seen=0x%02x\n",
				session->driver.sirf.time_seen);
		session->driver.sirf.time_seen |= TIME_SEEN_GPS_2;
		if (IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_GPS_2))
		    (void)ntpshm_put(session, session->gpsdata.newdata.time + 0.8);
	    }
#endif /* NTPSHM_ENABLE */
	    /* fix quality data */
	    session->gpsdata.hdop = (double)getub(buf, 20)/5.0;
	    mask |= TIME_SET | LATLON_SET | TRACK_SET | SPEED_SET | STATUS_SET | MODE_SET | HDOP_SET | USED_SET | CYCLE_START_SET;
	}
	return mask;

    case 0x04:		/* Measured tracker data out */
	gpsd_zero_satellites(&session->gpsdata);
	session->gpsdata.sentence_time
	    = gpstime_to_unix(getsw(buf, 1), getul(buf, 3)*1e-2) - session->context->leap_seconds;
	for (i = st = 0; i < SIRF_CHANNELS; i++) {
	    int off = 8 + 15 * i;
	    bool good;
	    session->gpsdata.PRN[st]       = (int)getub(buf, off);
	    session->gpsdata.azimuth[st]   = (int)(((unsigned)getub(buf, off+1)*3)/2.0);
	    session->gpsdata.elevation[st] = (int)((unsigned)getub(buf, off+2)/2.0);
	    cn = 0;
	    for (j = 0; j < 10; j++)
		cn += (int)getub(buf, off+5+j);
	    session->gpsdata.ss[st] = cn/10;
	    good = session->gpsdata.PRN[st]!=0 && 
		session->gpsdata.azimuth[st]!=0 && 
		session->gpsdata.elevation[st]!=0;
#ifdef __UNUSED__
	    gpsd_report(4, "PRN=%2d El=%3.2f Az=%3.2f ss=%3d stat=%04x %c\n",
			getub(buf, off), 
			getub(buf, off+2)/2.0, 
			(getub(buf, off+1)*3)/2.0,
			cn/10, 
			getuw(buf, off+3),
			good ? '*' : ' ');
#endif /* UNUSED */
	    if (good!=0)
		st += 1;
	}
	session->gpsdata.satellites = st;
#ifdef NTPSHM_ENABLE
	if (st > 3) {
	    if ((session->driver.sirf.time_seen & TIME_SEEN_GPS_1)==0)
		gpsd_report(4, "valid time in message 0x04, seen=0x%02x\n",
			    session->driver.sirf.time_seen);
	    session->driver.sirf.time_seen |= TIME_SEEN_GPS_1;
	    if (IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_GPS_1))
		(void)ntpshm_put(session,session->gpsdata.sentence_time+0.8);
	}
#endif /* NTPSHM_ENABLE */
	/*
	 * The freaking brain-dead SiRF chip doesn't obey its own
	 * rate-control command for 04, at least at firmware rev. 231, 
	 * so we have to do our own rate-limiting here...
	 */
	if ((session->driver.sirf.satcounter++ % 5) != 0)
	    break;
	gpsd_report(4, "MTD 0x04: %d satellites\n", st);
	return TIME_SET | SATELLITE_SET;

    case 0x05:		/* Raw Tracker Data Out */
	return 0;

    case 0x06:		/* Software Version String */
	gpsd_report(4, "FV  0x06: Firmware version: %s\n", buf+1);
	fv = atof((char *)(buf+1));
	if (fv < 231) {
	    session->driver.sirf.driverstate |= SIRF_LT_231;
	    if (fv > 200)
		sirfbin_mode(session, 0);
	} else if (fv < 232) 
	    session->driver.sirf.driverstate |= SIRF_EQ_231;
	else {
	    /*@ +charint @*/
	    unsigned char enablemid52[] = {
		0xa0, 0xa2, 0x00, 0x08, 
		0xa6, 0x00, 0x34, 0x01, 0x00, 0x00, 0x00, 0x00,
		0x00, 0xdb, 0xb0, 0xb3};
	    /*@ -charint @*/
	    gpsd_report(4, "Enabling PPS message...\n");
	    (void)sirf_write(session->gpsdata.gps_fd, enablemid52);
	    session->driver.sirf.driverstate |= SIRF_GE_232;
	    session->context->valid |= LEAP_SECOND_VALID;
	}
	if (strstr((char *)(buf+1), "ES"))
	    gpsd_report(4, "Firmware has XTrac capability\n");
	gpsd_report(4, "Driver state flags are: %0x\n", session->driver.sirf.driverstate);
	session->driver.sirf.time_seen = 0;
	if ((session->context->valid & LEAP_SECOND_VALID)==0) {
	    gpsd_report(4, "Enabling subframe transmission...\n");
	    (void)sirf_write(session->gpsdata.gps_fd, enablesubframe);
	}
	return 0;

    case 0x08:		/* subframe data -- extract leap-second from this */
	/*
	 * Chris Kuethe says:
	 * "Message 8 is generated as the data is received. It is not
	 * buffered on the chip. So when you enable message 8, you'll
	 * get one subframe every 6 seconds.  Of the data received, the
	 * almanac and ephemeris are buffered and stored, so you can
	 * query them at will. Alas, the time parameters are not
	 * stored, which is really lame, as the UTC-GPS correction
	 * changes 1 second every few years. Maybe."
	 */
       {
	    unsigned int words[10];
	    //unsigned int chan = (unsigned int)getub(buf, 1);
	    //unsigned int svid = (unsigned int)getub(buf, 2);
	    words[0] = (unsigned int)getul(buf, 3);
	    words[1] = (unsigned int)getul(buf, 7);
	    words[2] = (unsigned int)getul(buf, 11);
	    words[3] = (unsigned int)getul(buf, 15);
	    words[4] = (unsigned int)getul(buf, 19);
	    words[5] = (unsigned int)getul(buf, 23);
	    words[6] = (unsigned int)getul(buf, 27);
	    words[7] = (unsigned int)getul(buf, 31);
	    words[8] = (unsigned int)getul(buf, 35);
	    words[9] = (unsigned int)getul(buf, 39);
	    gpsd_interpret_subframe(session, words);

	    if (session->context->valid & LEAP_SECOND_VALID) {
		gpsd_report(4, "Disabling subframe transmission...\n");
		(void)sirf_write(session->gpsdata.gps_fd, disablesubframe);
	    }
	}
	break;
    case 0x09:		/* CPU Throughput */
	gpsd_report(4, 
		    "THR 0x09: SegStatMax=%.3f, SegStatLat=%3.f, AveTrkTime=%.3f, Last MS=%3.f\n", 
		    (float)getuw(buf, 1)/186, (float)getuw(buf, 3)/186, 
		    (float)getuw(buf, 5)/186, getuw(buf, 7));
    	return 0;

    case 0x0a:		/* Error ID Data */
	switch (getuw(buf, 1))
	{
	case 2:
	    gpsd_report(4, "EID 0x0a type 2: Subframe %d error on PRN %ld\n", getul(buf, 9), getul(buf, 5));
	    break;

	case 4107:
	    gpsd_report(4, "EID 0x0a type 4107: neither KF nor LSQ fix.\n", getul(buf, 5));
	    break;

	default:
	    gpsd_report(4, "EID 0x0a: Error ID type %d\n", getuw(buf, 1));
	    break;
	}
	return 0;

    case 0x0b:		/* Command Acknowledgement */
	gpsd_report(4, "ACK 0x0b: %02x\n",getub(buf, 1));
    	return 0;

    case 0x0c:		/* Command NAcknowledgement */
	gpsd_report(4, "NAK 0x0c: %02x\n",getub(buf, 1));
    	return 0;

    case 0x0d:		/* Visible List */
	return 0;

    case 0x12:		/* OK To Send */
	gpsd_report(4, "OTS 0x12: send indicator = %d\n",getub(buf, 1));
	return 0;

    case 0x1b:		/* DGPS status (undocumented) */
	/******************************************************************
	 Not actually documented in any published materials.