zodiac.c

很好的一个gps daemon驱动,简单实用

    REFRESH(session->gNMEAdata.satellite_stamp);
}

static void handle1003(struct gps_session_t *session, unsigned short *p)
{
    int j;

    session->gNMEAdata.pdop = p[O(10)];
    session->gNMEAdata.hdop = p[O(11)];
    session->gNMEAdata.vdop = p[O(12)];
    session->gNMEAdata.satellites = p[O(14)];

    for (j = 0; j < MAXCHANNELS; j++) {
	if (j < session->gNMEAdata.satellites) {
	    session->gNMEAdata.PRN[j] = p[O(15 + (3 * j))];
	    session->gNMEAdata.azimuth[j] = p[O(16 + (3 * j))] * 180 / (PI * 10000);
	    session->gNMEAdata.elevation[j] = p[O(17 + (3 * j))] * 180 / (PI * 10000);
#if 0
	    gpsd_report(1, "Sat%02d:  PRN:%d az:%d el:%d\n", 
			i, p[O(15+(3*i))], p[O(16+(3*i))], p[O(17+(3*i))]);
#endif
	} else {
	    session->gNMEAdata.PRN[j] = 0;
	    session->gNMEAdata.azimuth[j] = 0.0;
	    session->gNMEAdata.elevation[j] = 0.0;
	}
    }
    REFRESH(session->gNMEAdata.fix_quality_stamp);
    REFRESH(session->gNMEAdata.satellite_stamp);
}

static void handle1005(struct gps_session_t *session, unsigned short *p)
{
    int i, numcorrections = p[O(12)];

#if 1
  gpsd_report(1, "Station bad: %d\n", (p[O(9)] & 1) ? 1 : 0);
  gpsd_report(1, "User disabled: %d\n", (p[O(9)] & 2) ? 1 : 0);
  gpsd_report(1, "Station ID: %d\n", p[O(10)]);
  gpsd_report(1, "Age of last correction in seconds: %d\n", p[O(11)]);
  gpsd_report(1, "Number of corrections: %d\n", p[O(12)]);
  for (i = 0; i < numcorrections; i++) {
    gpsd_report(1, "Sat%02d:", p[O(13+i)] & 0x3f);
    gpsd_report(1, "ephemeris:%d", (p[O(13+i)] & 64) ? 1 : 0);
    gpsd_report(1, "rtcm corrections:%d", (p[O(13+i)] & 128) ? 1 : 0);
    gpsd_report(1, "rtcm udre:%d", (p[O(13+i)] & 256) ? 1 : 0);
    gpsd_report(1, "sat health:%d", (p[O(13+i)] & 512) ? 1 : 0);
    gpsd_report(1, "rtcm sat health:%d", (p[O(13+i)] & 1024) ? 1 : 0);
    gpsd_report(1, "corrections state:%d", (p[O(13+i)] & 2048) ? 1 : 0);
    gpsd_report(1, "iode mismatch:%d", (p[O(13+i)] & 4096) ? 1 : 0);
  }
#endif
}

static void analyze(struct gps_session_t *session, 
		    struct header *h, unsigned short *p)
{
    unsigned char buf[BUFSIZE], *bufp, *bufp2;
    int i = 0, j = 0, nmea = 0;

    if (p[h->ndata] == zodiac_checksum(p, h->ndata)) {
	gpsd_report(5, "id %d\n", h->id);
	switch (h->id) {
	case 1000:
	    handle1000(session, p);
	    bufp = buf;
	    if (session->gNMEAdata.mode > 1) {
		sprintf(bufp,
			"$GPGGA,%02d%02d%02d,%f,%c,%f,%c,%d,%02d,%.2f,%.1f,%c,%f,%c,%s,%s*",
		   session->hours, session->minutes, session->seconds,
			degtodm(fabs(session->gNMEAdata.latitude)),
			((session->gNMEAdata.latitude > 0) ? 'N' : 'S'),
			degtodm(fabs(session->gNMEAdata.longitude)),
			((session->gNMEAdata.longitude > 0) ? 'E' : 'W'),
		    session->gNMEAdata.mode, session->gNMEAdata.satellites_used, session->gNMEAdata.hdop,
			session->gNMEAdata.altitude, 'M', session->separation, 'M', "", "");
		nmea_add_checksum(bufp + 1);
		bufp = bufp + strlen(bufp);
	    }
	    sprintf(bufp,
		    "$GPRMC,%02d%02d%02d,%c,%f,%c,%f,%c,%f,%f,%02d%02d%02d,%02f,%c*",
		    session->hours, session->minutes, session->seconds,
		    session->gNMEAdata.status ? 'A' : 'V', degtodm(fabs(session->gNMEAdata.latitude)),
		    ((session->gNMEAdata.latitude > 0) ? 'N' : 'S'),
		    degtodm(fabs(session->gNMEAdata.longitude)),
		((session->gNMEAdata.longitude > 0) ? 'E' : 'W'), session->gNMEAdata.speed,
		    session->gNMEAdata.track, session->day, session->month,
		    (session->year % 100), session->mag_var,
		    (session->mag_var > 0) ? 'E' : 'W');
	    nmea_add_checksum(bufp + 1);
	    nmea = 1000;
	    break;
	case 1002:
	    handle1002(session, p);
	    bufp2 = bufp = buf;
	    sprintf(bufp, "$GPGSA,%c,%d,", 'A', session->gNMEAdata.mode);
	    j = 0;
	    for (i = 0; i < MAXCHANNELS; i++) {
		if (session->gNMEAdata.used[i]) {
		    bufp = bufp + strlen(bufp);
		    sprintf(bufp, "%02d,", session->gNMEAdata.PRN[i]);
		    j++;
		}
	    }
	    for (i = j; i < MAXCHANNELS; i++) {
		bufp = bufp + strlen(bufp);
		sprintf(bufp, ",");
	    }
	    bufp = bufp + strlen(bufp);
	    sprintf(bufp, "%.2f,%.2f,%.2f*", session->gNMEAdata.pdop, session->gNMEAdata.hdop,
		    session->gNMEAdata.vdop);
	    nmea_add_checksum(bufp2 + 1);
	    bufp2 = bufp = bufp + strlen(bufp);
	    sprintf(bufp, "$PRWIZCH");
	    bufp = bufp + strlen(bufp);
	    for (i = 0; i < 12; i++) {
		sprintf(bufp, ",%02d,%X", session->Zs[i], session->Zv[i]);
		bufp = bufp + strlen(bufp);
	    }
	    sprintf(bufp, "*");
	    bufp = bufp + strlen(bufp);
	    nmea_add_checksum(bufp2 + 1);
	    nmea = 1002;
	    break;
	case 1003:
	    handle1003(session, p);
	    bufp2 = bufp = buf;
	    j = (session->gNMEAdata.satellites / 4) + (((session->gNMEAdata.satellites % 4) > 0) ? 1 : 0);
	    while (i < 12) {
		if (i % 4 == 0)
		    sprintf(bufp, "$GPGSV,%d,%d,%02d", j, (i / 4) + 1, session->gNMEAdata.satellites);
		bufp += strlen(bufp);
		if (i <= session->gNMEAdata.satellites && session->gNMEAdata.elevation[i])
		    sprintf(bufp, ",%02d,%02d,%03d,%02d", session->gNMEAdata.PRN[i],
			    session->gNMEAdata.elevation[i], session->gNMEAdata.azimuth[i], session->gNMEAdata.ss[i]);
		else
		    sprintf(bufp, ",%02d,00,000,%02d,", session->gNMEAdata.PRN[i],
			    session->gNMEAdata.ss[i]);
		bufp += strlen(bufp);
		if (i % 4 == 3) {
		    sprintf(bufp, "*");
		    nmea_add_checksum(bufp2 + 1);
		    bufp += strlen(bufp);
		    bufp2 = bufp;
		}
		i++;
	    }
	    nmea = 1003;
	    break;	
	case 1005:
	    handle1005(session, p);
	    break;	
	}
    }
    if (nmea > 0) {
	gpsd_report(4, "%s", buf);
	if (session->gNMEAdata.raw_hook)
	    session->gNMEAdata.raw_hook(buf);
    }
}

static int putword(unsigned short *p, unsigned char c, unsigned int n)
{
    *(((unsigned char *) p) + n) = c;
    return (n == 0);
}

static void zodiac_handle_input(struct gps_session_t *session)
{
    unsigned char c;

    if (read(session->gNMEAdata.gps_fd, &c, 1) == 1)
    {
	static int state = ZODIAC_HUNT_FF;
	static struct header h;
	static unsigned int byte, words;
	static unsigned short *data;

	switch (state) {
	case ZODIAC_HUNT_FF:
	    if (c == 0xff)
		state = ZODIAC_HUNT_81;
	    if (c == 'E')
		state = ZODIAC_HUNT_A;
	    break;
	case ZODIAC_HUNT_A:
	    /* A better be right after E */
	    if ((c == 'A') && (session->gNMEAdata.gps_fd != -1))
		write(session->gNMEAdata.gps_fd, "EARTHA\r\n", 8);
	    state = ZODIAC_HUNT_FF;
	    break;
	case ZODIAC_HUNT_81:
	    if (c == 0x81)
		state = ZODIAC_HUNT_ID;
	    h.sync = 0x81ff;
	    byte = 0;
	    break;
	case ZODIAC_HUNT_ID:
	    if (!(byte = putword(&(h.id), c, byte)))
		state = ZODIAC_HUNT_WC;
	    break;
	case ZODIAC_HUNT_WC:
	    if (!(byte = putword(&(h.ndata), c, byte)))
		state = ZODIAC_HUNT_FLAGS;
	    break;
	case ZODIAC_HUNT_FLAGS:
	    if (!(byte = putword(&(h.flags), c, byte)))
		state = ZODIAC_HUNT_CS;
	    break;
	case ZODIAC_HUNT_CS:
	    if (!(byte = putword(&(h.csum), c, byte))) {
		if (h.csum == zodiac_checksum((unsigned short *) &h, 4)) {
		    state = ZODIAC_HUNT_DATA;
		    data = (unsigned short *) malloc((h.ndata + 1) * 2);
		    words = 0;
		} else
		    state = ZODIAC_HUNT_FF;
	    }
	    break;
	case ZODIAC_HUNT_DATA:
	    if (!(byte = putword(data + words, c, byte)))
		words++;
	    if (words == h.ndata + 1) {
		analyze(session, &h, data);
		free(data);
		state = ZODIAC_HUNT_FF;
	    }
	    break;
	}
    }
}

/* caller needs to specify a wrapup function */

/* this is everything we export */
struct gps_type_t zodiac_binary =
{
    '\0',		/* cannot be explicitly selected */
    "Zodiac binary",	/* full name of type */
    NULL,		/* only switched to by some other driver */
    zodiac_init,	/* initialize the device */
    zodiac_handle_input,/* read and parse message packets */
    zodiac_send_rtcm,	/* send DGPS correction */
    NULL,		/* caller needs to supply a close hook */
    9600,		/* 4800 won't work */
    1,			/* 1 stop bit */
    1,			/* updates every second */
};

#endif /* ZODIAC_ENABLE */