From 1d29b25210b5d5f162492e21fc4f572f7d7a60eb Mon Sep 17 00:00:00 2001 From: "Eric S. Raymond" Date: Sun, 22 May 2005 16:15:48 +0000 Subject: Various driver fixes and cleanups from Rob Janssen. --- zodiac.c | 72 +++++++++++++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 53 insertions(+), 19 deletions(-) (limited to 'zodiac.c') diff --git a/zodiac.c b/zodiac.c index 7cb1ce26..5805eb68 100644 --- a/zodiac.c +++ b/zodiac.c @@ -54,38 +54,45 @@ static int end_write(int fd, void *d, int len) static void zodiac_spew(struct gps_device_t *session, int type, unsigned short *dat, int dlen) { struct header h; + int i; + char buf[BUFSIZ]; - h.flags = 0; h.sync = 0x81ff; h.id = type; h.ndata = dlen - 1; + h.flags = 0; h.csum = zodiac_checksum((unsigned short *) &h, 4); if (session->gpsdata.gps_fd != -1) { end_write(session->gpsdata.gps_fd, &h, sizeof(h)); end_write(session->gpsdata.gps_fd, dat, sizeof(unsigned short) * dlen); } + + sprintf(buf,"%04x %04x %04x %04x %04x",h.sync,h.id,h.ndata,h.flags,h.csum); + for (i = 0; i < dlen; i++) + sprintf(buf+strlen(buf), " %04x", dat[i]); + + gpsd_report(5, "Sent Zodiac packet: %s\n",buf); } static int zodiac_speed_switch(struct gps_device_t *session, int speed) { - unsigned short data[21]; + unsigned short data[15]; if (session->sn++ > 32767) session->sn = 0; memset(data, 0, sizeof(data)); + /* data is the part of the message starting at word 6 */ data[0] = session->sn; /* sequence number */ data[1] = 1; /* port 1 data valid */ - data[8] = 8; /* port 1 character width */ - data[9] = 1; /* port 1 stop bits */ - data[10] = 0; /* port 1 parity */ - data[11] = (short)(log(speed/300)/log(2))+1; /* port 1 speed */ - data[12] = data[13] = data[14] = data[15] = 0; - data[16] = data[17] = data[18] = data[19] = 0; - data[20] = zodiac_checksum(data, 20); + data[2] = 1; /* port 1 character width (8 bits) */ + data[3] = 0; /* port 1 stop bits (1 stopbit) */ + data[4] = 0; /* port 1 parity (none) */ + data[5] = (short)(round(log(speed/300)/M_LN2))+1; /* port 1 speed */ + data[14] = zodiac_checksum(data, 14); - zodiac_spew(session, 1330, data, 21); + zodiac_spew(session, 1330, data, 15); return speed; /* it would be nice to error-check this */ } @@ -131,13 +138,21 @@ static int zodiac_send_rtcm(struct gps_device_t *session, static int handle1000(struct gps_device_t *session) { - session->gpsdata.status = (getw(10) & 0x1c) ? 0 : 1; + /* ticks = getl(6); */ + /* sequence = getw(8); */ + /* measurement_sequence = getw(9); */ + session->gpsdata.status = (getw(10) & 0x1c) ? 0 : 1; if (session->gpsdata.status) session->gpsdata.fix.mode = (getw(10) & 1) ? MODE_2D : MODE_3D; else session->gpsdata.fix.mode = MODE_NO_FIX; + /* solution_type = getw(11); */ session->gpsdata.satellites_used = getw(12); + /* polar_navigation = getw(13); */ + /* gps_week = getw(14); */ + /* gps_seconds = getl(15); */ + /* gps_nanoseconds = getl(17); */ session->gpsdata.nmea_date.tm_mday = getw(19); session->gpsdata.nmea_date.tm_mon = getw(20) - 1; session->gpsdata.nmea_date.tm_year = getw(21) - 1900; @@ -159,10 +174,15 @@ static int handle1000(struct gps_device_t *session) session->gpsdata.fix.track = getw(36) * RAD_2_DEG * 1e-3; session->mag_var = ((short)getw(37)) * RAD_2_DEG * 1e-4; session->gpsdata.fix.climb = ((short)getw(38)) * 1e-2; + /* map_datum = getw(39); */ session->gpsdata.fix.eph = getl(40) * 1e-2; session->gpsdata.fix.epv = getl(42) * 1e-2; session->gpsdata.fix.ept = getl(44) * 1e-2; session->gpsdata.fix.eps = getw(46) * 1e-2; + /* clock_bias = getl(47) * 1e-2; */ + /* clock_bias_sd = getl(49) * 1e-2; */ + /* clock_drift = getl(51) * 1e-2; */ + /* clock_drift_sd = getl(53) * 1e-2; */ #if 0 gpsd_report(1, "date: %lf\n", session->gpsdata.fix.time); @@ -193,23 +213,29 @@ static int handle1000(struct gps_device_t *session) static int handle1002(struct gps_device_t *session) { - int i, j, prn; + int i, j, status, prn; session->gpsdata.satellites_used = 0; memset(session->gpsdata.used,0,sizeof(session->gpsdata.used)); + /* ticks = getl(6); */ + /* sequence = getw(8); */ + /* measurement_sequence = getw(9); */ + /* gps_week = getw(10); */ + /* gps_seconds = getl(11); */ + /* gps_nanoseconds = getl(13); */ for (i = 0; i < MAXCHANNELS; i++) { + session->Zv[i] = status = getw(15 + (3 * i)); session->Zs[i] = prn = getw(16 + (3 * i)); - session->Zv[i] = (getw(17 + (3 * i)) & 0xf); #if 0 gpsd_report(1, "Sat%02d:", i); - gpsd_report(1, " used:%d", (getw(15 + (3 * i)) & 1) ? 1 : 0); - gpsd_report(1, " eph:%d", (getw(15 + (3 * i)) & 2) ? 1 : 0); - gpsd_report(1, " val:%d", (getw(15 + (3 * i)) & 4) ? 1 : 0); - gpsd_report(1, " dgps:%d", (getw(15 + (3 * i)) & 8) ? 1 : 0); + gpsd_report(1, " used:%d", (status & 1) ? 1 : 0); + gpsd_report(1, " eph:%d", (status & 2) ? 1 : 0); + gpsd_report(1, " val:%d", (status & 4) ? 1 : 0); + gpsd_report(1, " dgps:%d", (status & 8) ? 1 : 0); gpsd_report(1, " PRN:%d", prn); gpsd_report(1, " C/No:%d\n", getw(17 + (3 * i))); #endif - if (getw(15 + (3 * i)) & 1) + if (status & 1) session->gpsdata.used[session->gpsdata.satellites_used++] = prn; for (j = 0; j < MAXCHANNELS; j++) { if (session->gpsdata.PRN[j] != prn) @@ -225,6 +251,8 @@ static int handle1003(struct gps_device_t *session) { int i; + /* ticks = getl(6); */ + /* sequence = getw(8); */ session->gpsdata.gdop = getw(9) * 1e-2; session->gpsdata.pdop = getw(10) * 1e-2; session->gpsdata.hdop = getw(11) * 1e-2; @@ -254,6 +282,8 @@ static int handle1003(struct gps_device_t *session) static void handle1005(struct gps_device_t *session UNUSED) { + /* ticks = getl(6); */ + /* sequence = getw(8); */ #if 0 int i, numcorrections = getw(12); @@ -278,6 +308,10 @@ static void handle1005(struct gps_device_t *session UNUSED) static void handle1108(struct gps_device_t *session) { + /* ticks = getl(6); */ + /* sequence = getw(8); */ + /* utc_week_seconds = getl(14); */ + /* leap_nanoseconds = getl(17); */ if ((getw(19) & 3) == 3) session->context->leap_seconds = getw(16); #if 0 @@ -318,7 +352,7 @@ static int zodiac_analyze(struct gps_device_t *session) mask = handle1002(session); strcpy(buf, "$PRWIZCH"); for (i = 0; i < MAXCHANNELS; i++) { - sprintf(buf+strlen(buf), ",%02d,%X", session->Zs[i], session->Zv[i]); + sprintf(buf+strlen(buf), ",%02d,%X", session->Zs[i], session->Zv[i] & 0x0f); } strcat(buf, "*"); nmea_add_checksum(buf); -- cgit v1.2.1