zodiac.c

来自「gpsd, a popular GPS daemon.」· C语言 代码 · 共 457 行 · 第 1/2 页

    gpsd_report(LOG_INF, "Magnetic variation: %f\n", getword(37) * RAD_2_DEG * 1e-4);
    gpsd_report(LOG_INF, "Course: %f\n", getword(36) * RAD_2_DEG * 1e-4);
    gpsd_report(LOG_INF, "Separation: %f\n", getword(33) * 1e-2);
#endif

    session->gpsdata.sentence_length = 55;
    return TIME_SET|LATLON_SET|ALTITUDE_SET|CLIMB_SET|SPEED_SET|TRACK_SET|STATUS_SET|MODE_SET|CYCLE_START_SET; /* |HERR_SET|VERR_SET|SPEEDERR_SET */
}

static gps_mask_t handle1002(struct gps_device_t *session)
{
    int i, j, status, prn;

    session->gpsdata.satellites_used = 0;
    memset(session->gpsdata.used,0,sizeof(session->gpsdata.used));
    /* ticks                      = getlong(6); */
    /* sequence                   = getword(8); */
    /* measurement_sequence       = getword(9); */
    /* gps_week                   = getword(10); */
    /* gps_seconds                = getlong(11); */
    /* gps_nanoseconds            = getlong(13); */
    for (i = 0; i < ZODIAC_CHANNELS; i++) {
	/*@ -type @*/ 
	session->driver.zodiac.Zv[i] = status = (int)getword(15 + (3 * i));
	session->driver.zodiac.Zs[i] = prn = (int)getword(16 + (3 * i));
	/*@ +type @*/ 
#if 0
	gpsd_report(LOG_INF, "Sat%02d:\n", i);
	gpsd_report(LOG_INF, " used:%d\n", (status & 1) ? 1 : 0);
	gpsd_report(LOG_INF, " eph:%d\n", (status & 2) ? 1 : 0);
	gpsd_report(LOG_INF, " val:%d\n", (status & 4) ? 1 : 0);
	gpsd_report(LOG_INF, " dgps:%d\n", (status & 8) ? 1 : 0);
	gpsd_report(LOG_INF, " PRN:%d\n", prn);
	gpsd_report(LOG_INF, " C/No:%d\n", getword(17 + (3 * i)));
#endif
	if (status & 1)
	    session->gpsdata.used[session->gpsdata.satellites_used++] = prn;
	for (j = 0; j < ZODIAC_CHANNELS; j++) {
	    if (session->gpsdata.PRN[j] != prn)
		continue;
	    session->gpsdata.ss[j] = (int)getword(17 + (3 * i));
	    break;
	}
    }
    return SATELLITE_SET | USED_SET;
}

static gps_mask_t handle1003(struct gps_device_t *session)
{
    int i;

    /* ticks              = getlong(6); */
    /* sequence           = getword(8); */
    session->gpsdata.gdop = (unsigned int)getword(9) * 1e-2;
    session->gpsdata.pdop = (unsigned int)getword(10) * 1e-2;
    session->gpsdata.hdop = (unsigned int)getword(11) * 1e-2;
    session->gpsdata.vdop = (unsigned int)getword(12) * 1e-2;
    session->gpsdata.tdop = (unsigned int)getword(13) * 1e-2;
    session->gpsdata.satellites = (int)getword(14);

    for (i = 0; i < ZODIAC_CHANNELS; i++) {
	if (i < session->gpsdata.satellites) {
	    session->gpsdata.PRN[i] = (int)getword(15 + (3 * i));
	    session->gpsdata.azimuth[i] = (int)(((short)getword(16 + (3 * i))) * RAD_2_DEG * 1e-4);
	    if (session->gpsdata.azimuth[i] < 0)
		session->gpsdata.azimuth[i] += 360;
	    session->gpsdata.elevation[i] = (int)(((short)getword(17 + (3 * i))) * RAD_2_DEG * 1e-4);
#if 0
	    gpsd_report(LOG_INF, "Sat%02d:  PRN:%d az:%d el:%d\n", 
			i, getword(15+(3 * i)),getword(16+(3 * i)),getword(17+(3 * i)));
#endif
	} else {
	    session->gpsdata.PRN[i] = 0;
	    session->gpsdata.azimuth[i] = 0;
	    session->gpsdata.elevation[i] = 0;
	}
    }
    return SATELLITE_SET | HDOP_SET | VDOP_SET | PDOP_SET;
}

static void handle1005(struct gps_device_t *session UNUSED)
{
    /* ticks              = getlong(6); */
    /* sequence           = getword(8); */
    int numcorrections = (int)getword(12);
#if 0
    int i;

    gpsd_report(LOG_INF, "Packet: %d\n", session->driver.zodiac.sn);
    gpsd_report(LOG_INF, "Station bad: %d\n", (getword(9) & 1) ? 1 : 0);
    gpsd_report(LOG_INF, "User disabled: %d\n", (getword(9) & 2) ? 1 : 0);
    gpsd_report(LOG_INF, "Station ID: %d\n", getword(10));
    gpsd_report(LOG_INF, "Age of last correction in seconds: %d\n", getword(11));
    gpsd_report(LOG_INF, "Number of corrections: %d\n", getword(12));
    for (i = 0; i < numcorrections; i++) {
	gpsd_report(LOG_INF, "Sat%02d:\n", getword(13+i) & 0x3f);
	gpsd_report(LOG_INF, "ephemeris:%d\n", (getword(13+i) & 64) ? 1 : 0);
	gpsd_report(LOG_INF, "rtcm corrections:%d\n", (getword(13+i) & 128) ? 1 : 0);
	gpsd_report(LOG_INF, "rtcm udre:%d\n", (getword(13+i) & 256) ? 1 : 0);
	gpsd_report(LOG_INF, "sat health:%d\n", (getword(13+i) & 512) ? 1 : 0);
	gpsd_report(LOG_INF, "rtcm sat health:%d\n", (getword(13+i) & 1024) ? 1 : 0);
	gpsd_report(LOG_INF, "corrections state:%d\n", (getword(13+i) & 2048) ? 1 : 0);
	gpsd_report(LOG_INF, "iode mismatch:%d\n", (getword(13+i) & 4096) ? 1 : 0);
    }
#endif
    if (session->gpsdata.fix.mode == MODE_NO_FIX)
	session->gpsdata.status = STATUS_NO_FIX;
    else if (numcorrections == 0)
	session->gpsdata.status = STATUS_FIX;
    else
	session->gpsdata.status = STATUS_DGPS_FIX;
}

static gps_mask_t handle1011(struct gps_device_t *session)
{
    /*
     * This is UNTESTED -- but harmless if buggy.  Added to support
     * client querying of the ID with firmware version in 2006.
     * The Zodiac is supposed to send one of these messages on startup.
     */
    getstring(session->subtype, 19, 28);	/* software version field */
    gpsd_report(LOG_INF, "Software version: %s\n", session->subtype);
    return DEVICEID_SET;
}


static void handle1108(struct gps_device_t *session)
{
    /* ticks              = getlong(6); */
    /* sequence           = getword(8); */
    /* utc_week_seconds   = getlong(14); */
    /* leap_nanoseconds   = getlong(17); */
    if ((int)(getword(19) & 3) == 3)
	session->context->leap_seconds = (int)getword(16);
#if 0
    gpsd_report(LOG_INF, "Leap seconds: %d.%09d\n", getword(16), getlong(17));
    gpsd_report(LOG_INF, "UTC validity: %d\n", getword(19) & 3);
#endif
}

static gps_mask_t zodiac_analyze(struct gps_device_t *session)
{
    char buf[BUFSIZ];
    int i;
    unsigned int id = (unsigned int)((session->packet.outbuffer[3]<<8) | session->packet.outbuffer[2]);

    if (session->packet.type != ZODIAC_PACKET) {
	struct gps_type_t **dp;
	gpsd_report(LOG_PROG, "zodiac_analyze packet type %d\n",session->packet.type);
 	// Wrong packet type ? 
	// Maybe find a trigger just in case it's an Earthmate
	gpsd_report(LOG_RAW+4, "Is this a trigger: %s ?\n", (char*)session->packet.outbuffer);

	for (dp = gpsd_drivers; *dp; dp++) {
	    char	*trigger = (*dp)->trigger;

	    if (trigger!=NULL && strncmp((char *)session->packet.outbuffer, trigger, strlen(trigger))==0 && isatty(session->gpsdata.gps_fd)!=0) {
		gpsd_report(LOG_PROG, "found %s.\n", trigger);
		    
		(void)gpsd_switch_driver(session, (*dp)->type_name);
		return 0;
	    }
   	}
        return 0;
    }

    buf[0] = '\0';
    for (i = 0; i < (int)session->packet.outbuflen; i++)
	(void)snprintf(buf+strlen(buf), sizeof(buf)-strlen(buf),
		       "%02x", (unsigned int)session->packet.outbuffer[i]);
    (void)strlcat(buf, "\n", BUFSIZ);
    gpsd_report(LOG_RAW, "Raw Zodiac packet type %d length %d: %s\n",id,session->packet.outbuflen,buf);

    if (session->packet.outbuflen < 10)
	return 0;

    (void)snprintf(session->gpsdata.tag,sizeof(session->gpsdata.tag),"%u",id);

    switch (id) {
    case 1000:
	return handle1000(session);
    case 1002:
	return handle1002(session);
    case 1003:
	return handle1003(session);
    case 1005:
	handle1005(session);
	return 0;	
    case 1011:
	return handle1011(session);
    case 1108:
	handle1108(session);
	return 0;
    default:
	return 0;
    }
}

/* caller needs to specify a wrapup function */

/* this is everything we export */
struct gps_type_t zodiac_binary =
{
    .type_name      = "Zodiac binary",	/* full name of type */
    .trigger        = NULL,		/* no trigger */
    .channels       = 12,		/* consumer-grade GPS */
    .probe_wakeup   = NULL,		/* no probe on baud rate change */
    .probe_detect   = NULL,		/* no probe */
    .probe_subtype  = NULL,		/* no initialization */
#ifdef ALLOW_RECONFIGURE
    .configurator   = NULL,		/* no configuration */
#endif /* ALLOW_RECONFIGURE */
    .get_packet     = generic_get,	/* use the generic packet getter */
    .parse_packet   = zodiac_analyze,	/* parse message packets */
    .rtcm_writer    = zodiac_send_rtcm,	/* send DGPS correction */
    .speed_switcher = zodiac_speed_switch,/* we can change baud rate */
    .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 reversion hook */
#endif /* ALLOW_RECONFIGURE */
    .wrapup         = NULL,		/* caller might supply a close hook */
    .cycle          = 1,		/* updates every second */
};

#endif /* ZODIAC_ENABLE */