sirf.c

gpsd, a popular GPS daemon.

#ifdef __UNUSED__
	gpsd_report(LOG_PROG, "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(LOG_PROG, "valid time in message 0x04, seen=0x%02x\n",
		session->driver.sirf.time_seen);
	session->driver.sirf.time_seen |= TIME_SEEN_GPS_1;
	if (session->context->enable_ntpshm && 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...
     */
    gpsd_report(LOG_PROG, "MTD 0x04: %d satellites\n", st);
    if ((session->driver.sirf.satcounter++ % 5) != 0)
	return 0;
    else
	return TIME_SET | SATELLITE_SET;
}

static gps_mask_t sirf_msg_navsol(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    int i;
    unsigned short navtype;
    gps_mask_t mask = 0;

    if (len != 41)
	return 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.fix.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.fix.mode = MODE_3D;
	else if (session->gpsdata.status != 0)
	    session->gpsdata.fix.mode = MODE_2D;
	if (session->gpsdata.fix.mode == MODE_3D)
	    mask |= ALTITUDE_SET | CLIMB_SET;
	gpsd_report(LOG_PROG, "MND 0x02: Navtype = 0x%0x, Status = %d, mode = %d\n",
	    navtype,session->gpsdata.status,session->gpsdata.fix.mode);
	/* byte 20 is HDOP, see below */
	/* byte 21 is "mode 2", not clear how to interpret that */
	session->gpsdata.fix.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.fix.mode > MODE_NO_FIX) {
	    if ((session->driver.sirf.time_seen & TIME_SEEN_GPS_2) == 0)
		gpsd_report(LOG_PROG, "valid time in message 0x02, seen=0x%02x\n",
		    session->driver.sirf.time_seen);
		session->driver.sirf.time_seen |= TIME_SEEN_GPS_2;
		if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_GPS_2))
		    (void)ntpshm_put(session, session->gpsdata.fix.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;
}

static gps_mask_t sirf_msg_geodetic(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    unsigned short navtype;
    gps_mask_t mask = 0;

    if (len != 91)
	return 0;

    if (session->driver.sirf.driverstate & SIRF_GE_232) {
	struct tm unpacked_date;
	double subseconds;
	/*
	 * Many versions of the SiRF protocol manual don't document 
	 * this sentence at all.  Those that do may incorrectly
	 * describe UTC Day, Hour, and Minute as 2-byte quantities,
	 * not 1-byte. Chris Kuethe, our SiRF expert, tells us:
	 *
	 * "The Geodetic Navigation packet (0x29) was not fully
	 * implemented in firmware prior to version 2.3.2. So for
	 * anyone running 231.000.000 or earlier (including ES,
	 * SiRFDRive, XTrac trains) you won't get UTC time. I don't
	 * know what's broken in firmwares before 2.3.1..."
	 *
	 * To work around the incomplete implementation of this
	 * packet in 231, we used to assume that only the altitude field
	 * from this packet is valid.  But even this doesn't necessarily
	 * seem to be the case.  Instead, we do our own computation 
	 * of geoid separation now.
	 */
	navtype = (unsigned short)getuw(buf, 3);
	session->gpsdata.status = STATUS_NO_FIX;
	session->gpsdata.fix.mode = MODE_NO_FIX;
	if (navtype & 0x80)
	    session->gpsdata.status = STATUS_DGPS_FIX;
	else if ((navtype & 0x07) > 0 && (navtype & 0x07) < 7)
	    session->gpsdata.status = STATUS_FIX;
	session->gpsdata.fix.mode = MODE_NO_FIX;
	if ((navtype & 0x07) == 4 || (navtype & 0x07) == 6)
	    session->gpsdata.fix.mode = MODE_3D;
	else if (session->gpsdata.status)
	    session->gpsdata.fix.mode = MODE_2D;
	gpsd_report(LOG_PROG, "GNI 0x29: Navtype = 0x%0x, Status = %d, mode = %d\n",
	    navtype, session->gpsdata.status, session->gpsdata.fix.mode);
	/*
	 * UTC is left all zeros in 231 and older firmware versions, 
	 * and misdocumented in the Protocol Reference (version 1.4).
	 *            Documented:        Real:
	 * UTC year       2               2
	 * UTC month      1               1
	 * UTC day        2               1
	 * UTC hour       2               1
	 * UTC minute     2               1
	 * UTC second     2               2
	 *                11              8
	 */
	unpacked_date.tm_year = (int)getuw(buf, 11)-1900;
	unpacked_date.tm_mon = (int)getub(buf, 13)-1;
	unpacked_date.tm_mday = (int)getub(buf, 14);
	unpacked_date.tm_hour = (int)getub(buf, 15);
	unpacked_date.tm_min = (int)getub(buf, 16);
	unpacked_date.tm_sec = 0;
	subseconds = getuw(buf, 17)*1e-3;
	/*@ -compdef -unrecog */
	session->gpsdata.fix.time = session->gpsdata.sentence_time =
	    (double)timegm(&unpacked_date)+subseconds;
	/*@ +compdef +unrecog */
	gpsd_report(LOG_PROG, "MID 41 UTC: %lf\n", session->gpsdata.fix.time);
#ifdef NTPSHM_ENABLE
	if (session->gpsdata.fix.mode > MODE_NO_FIX && unpacked_date.tm_year != 0) {
	    if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_1) == 0)
		gpsd_report(LOG_PROG, "valid time in message 0x29, seen=0x%02x\n",
		    session->driver.sirf.time_seen);
		session->driver.sirf.time_seen |= TIME_SEEN_UTC_1;
		if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_1))
		    (void)ntpshm_put(session, session->gpsdata.fix.time + 0.8);
	}
#endif /* NTPSHM_ENABLE */
	/* skip 4 bytes of satellite map */
	session->gpsdata.fix.latitude = getsl(buf, 23)*1e-7;
	session->gpsdata.fix.longitude = getsl(buf, 27)*1e-7;
	/* skip 4 bytes of altitude from ellipsoid */
	mask = TIME_SET | LATLON_SET | STATUS_SET | MODE_SET;
	session->gpsdata.fix.altitude = getsl(buf, 31)*1e-2;
	/* skip 1 byte of map datum */
	session->gpsdata.fix.speed = getsw(buf, 36)*1e-2;
	session->gpsdata.fix.track = getsw(buf, 38)*1e-2;
	/* skip 2 bytes of magnetic variation */
	session->gpsdata.fix.climb = getsw(buf, 42)*1e-2;
	/* HDOP should be available at byte 89, but in 231 it's zero. */
	mask |= SPEED_SET | TRACK_SET | CLIMB_SET | CYCLE_START_SET;
	session->gpsdata.sentence_length = 91;
	(void)strlcpy(session->gpsdata.tag, "GND",MAXTAGLEN+1);
    }
    return mask;
}

static gps_mask_t sirf_msg_sysparam(struct gps_device_t *session, unsigned char *buf, size_t len)
{

    if (len != 65)
	return 0;

#ifdef ALLOW_RECONFIGURE
    /* save these to restore them in the revert method */
    session->driver.sirf.nav_parameters_seen = true;
    session->driver.sirf.altitude_hold_mode = getub(buf, 5);
    session->driver.sirf.altitude_hold_source = getub(buf, 6);
    session->driver.sirf.altitude_source_input = getsw(buf, 7);
    session->driver.sirf.degraded_mode = getub(buf, 9);
    session->driver.sirf.degraded_timeout = getub(buf, 10);
    session->driver.sirf.dr_timeout = getub(buf, 11);
    session->driver.sirf.track_smooth_mode = getub(buf, 12);
    gpsd_report(LOG_PROG, "Setting Navigation Parameters\n");
    (void)sirf_write(session->gpsdata.gps_fd, modecontrol);
#endif /* ALLOW_RECONFIGURE */
    return 0;
}

static gps_mask_t sirf_msg_ublox(struct gps_device_t *session, unsigned char *buf, size_t len UNUSED)
{
    gps_mask_t mask;
    unsigned short navtype;

    if (len != 39)
	return 0;

    /* this packet is only sent by uBlox firmware from version 1.32 */
    mask = LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET | CLIMB_SET |
	STATUS_SET | MODE_SET | HDOP_SET | VDOP_SET | PDOP_SET;
    session->gpsdata.fix.latitude = getsl(buf, 1) * RAD_2_DEG * 1e-8;
    session->gpsdata.fix.longitude = getsl(buf, 5) * RAD_2_DEG * 1e-8;
    session->gpsdata.separation = wgs84_separation(session->gpsdata.fix.latitude, session->gpsdata.fix.longitude);
    session->gpsdata.fix.altitude = getsl(buf, 9) * 1e-3 - session->gpsdata.separation;
    session->gpsdata.fix.speed = getsl(buf, 13) * 1e-3;
    session->gpsdata.fix.climb = getsl(buf, 17) * 1e-3;
    session->gpsdata.fix.track = getsl(buf, 21) * RAD_2_DEG * 1e-8;

    navtype = (unsigned short)getub(buf, 25);
    session->gpsdata.status = STATUS_NO_FIX;
    session->gpsdata.fix.mode = MODE_NO_FIX;
    if (navtype & 0x80)
	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.fix.mode = MODE_3D;
    else if (session->gpsdata.status)
	session->gpsdata.fix.mode = MODE_2D;
    gpsd_report(LOG_PROG, "EMND 0x62: Navtype = 0x%0x, Status = %d, mode = %d\n",
	 navtype, session->gpsdata.status, session->gpsdata.fix.mode);

    if (navtype & 0x40) {		/* UTC corrected timestamp? */
	struct tm unpacked_date;
	double subseconds;
	mask |= TIME_SET;
	unpacked_date.tm_year = (int)getuw(buf, 26) - 1900;
	unpacked_date.tm_mon = (int)getub(buf, 28) - 1;
	unpacked_date.tm_mday = (int)getub(buf, 29);
	unpacked_date.tm_hour = (int)getub(buf, 30);
	unpacked_date.tm_min = (int)getub(buf, 31);
	unpacked_date.tm_sec = 0;
	subseconds = ((unsigned short)getuw(buf, 32))*1e-3;
	/*@ -compdef */
	session->gpsdata.fix.time = session->gpsdata.sentence_time =
	    (double)mkgmtime(&unpacked_date)+subseconds;
	/*@ +compdef */
#ifdef NTPSHM_ENABLE
	if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_2) == 0)
	    gpsd_report(LOG_PROG, "valid time in message 0x62, seen=0x%02x\n",
		session->driver.sirf.time_seen);
	    session->driver.sirf.time_seen |= TIME_SEEN_UTC_2;
	if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_2))
	    (void)ntpshm_put(session, session->gpsdata.fix.time + 0.8);
#endif /* NTPSHM_ENABLE */
	session->context->valid |= LEAP_SECOND_VALID;
    }

    session->gpsdata.gdop = (int)getub(buf, 34) / 5.0;
    session->gpsdata.pdop = (int)getub(buf, 35) / 5.0;
    session->gpsdata.hdop = (int)getub(buf, 36) / 5.0;
    session->gpsdata.vdop = (int)getub(buf, 37) / 5.0;
    session->gpsdata.tdop = (int)getub(buf, 38) / 5.0;
    session->driver.sirf.driverstate |= UBLOX;
    return mask;
}

static gps_mask_t sirf_msg_ppstime(struct gps_device_t *session, unsigned char *buf, size_t len)
{
    gps_mask_t mask = 0;

    if (len != 19)
	return 0;

    gpsd_report(LOG_PROG, "PPS 0x34: Status = 0x%02x\n", getub(buf, 14));
    if (((int)getub(buf, 14) & 0x07) == 0x07) {	/* valid UTC time? */
	struct tm unpacked_date;
	unpacked_date.tm_hour = (int)getub(buf, 1);
	unpacked_date.tm_min = (int)getub(buf, 2);
	unpacked_date.tm_sec = (int)getub(buf, 3);
	unpacked_date.tm_mday = (int)getub(buf, 4);
	unpacked_date.tm_mon = (int)getub(buf, 5) - 1;
	unpacked_date.tm_year = (int)getuw(buf, 6) - 1900;
	/*@ -compdef */
	session->gpsdata.fix.time = session->gpsdata.sentence_time =
	    (double)mkgmtime(&unpacked_date);
	/*@ +compdef */
	session->context->leap_seconds = (int)getuw(buf, 8);
	session->context->valid |= LEAP_SECOND_VALID;
#ifdef NTPSHM_ENABLE
	if ((session->driver.sirf.time_seen & TIME_SEEN_UTC_2) == 0)
	    gpsd_report(LOG_PROG, "valid time in message 0x34, seen=0x%02x\n",
		session->driver.sirf.time_seen);
	session->driver.sirf.time_seen |= TIME_SEEN_UTC_2;
	if (session->context->enable_ntpshm && IS_HIGHEST_BIT(session->driver.sirf.time_seen,TIME_SEEN_UTC_2))
	    (void)ntpshm_put(session, session->gpsdata.fix.time + 0.3);
#endif /* NTPSHM_ENABLE */
	mask |= TIME_SET;
    }
    return mask;
}

gps_mask_t sirf_parse(struct gps_device_t *session, unsigned char *buf, size_t len)
{

    if (len == 0)
	return 0;

    buf += 4;
    len -= 8;
    gpsd_report(LOG_RAW, "Raw SiRF packet type 0x%02x length %d: %s\n", buf[0],len,