diff options
| -rw-r--r-- | bits.h | 16 | ||||
| -rw-r--r-- | zodiac.c | 203 |
2 files changed, 112 insertions, 107 deletions
@@ -75,11 +75,11 @@ union long_double { /* Zodiac protocol description uses 1-origin indexing by little-endian word */ -#define getword(n) ( (session->packet.outbuffer[2*(n)-2]) \ - | (session->packet.outbuffer[2*(n)-1] << 8)) -#define getlong(n) ( (session->packet.outbuffer[2*(n)-2]) \ - | (session->packet.outbuffer[2*(n)-1] << 8) \ - | (session->packet.outbuffer[2*(n)+0] << 16) \ - | (session->packet.outbuffer[2*(n)+1] << 24)) -#define getstring(t, s, e) \ - (void)memcpy(t, session->packet.outbuffer+2*(s)-2, 2*((e)-(s)+1)) +#define getwordz(buf, n) ( (buf[2*(n)-2]) \ + | (buf[2*(n)-1] << 8)) +#define getlongz(buf, n) ( (buf[2*(n)-2]) \ + | (buf[2*(n)-1] << 8) \ + | (buf[2*(n)+0] << 16) \ + | (buf[2*(n)+1] << 24)) +#define getstringz(to, from, s, e) \ + (void)memcpy(to, from+2*(s)-2, 2*((e)-(s)+1)) @@ -142,34 +142,37 @@ static ssize_t zodiac_send_rtcm(struct gps_device_t *session, return 1; } +#define getzword(n) getwordz(session->packet.outbuffer, n) +#define getzlong(n) getlongz(session->packet.outbuffer, n) + static gps_mask_t handle1000(struct gps_device_t *session) { double subseconds; struct tm unpacked_date; - /* ticks = getlong(6); */ - /* sequence = getword(8); */ - /* measurement_sequence = getword(9); */ + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + /* measurement_sequence = getzword(9); */ /*@ -boolops -predboolothers @*/ - session->gpsdata.status = (getword(10) & 0x1c) ? 0 : 1; + session->gpsdata.status = (getzword(10) & 0x1c) ? 0 : 1; if (session->gpsdata.status != 0) - session->gpsdata.fix.mode = (getword(10) & 1) ? MODE_2D : MODE_3D; + session->gpsdata.fix.mode = (getzword(10) & 1) ? MODE_2D : MODE_3D; else session->gpsdata.fix.mode = MODE_NO_FIX; /*@ +boolops -predboolothers @*/ - /* solution_type = getword(11); */ - session->gpsdata.satellites_used = (int)getword(12); - /* polar_navigation = getword(13); */ - /* gps_week = getword(14); */ - /* gps_seconds = getlong(15); */ - /* gps_nanoseconds = getlong(17); */ - unpacked_date.tm_mday = (int)getword(19); - unpacked_date.tm_mon = (int)getword(20) - 1; - unpacked_date.tm_year = (int)getword(21) - 1900; - unpacked_date.tm_hour = (int)getword(22); - unpacked_date.tm_min = (int)getword(23); - unpacked_date.tm_sec = (int)getword(24); - subseconds = (int)getlong(25) / 1e9; + /* solution_type = getzword(11); */ + session->gpsdata.satellites_used = (int)getzword(12); + /* polar_navigation = getzword(13); */ + /* gps_week = getzword(14); */ + /* gps_seconds = getzlong(15); */ + /* gps_nanoseconds = getzlong(17); */ + unpacked_date.tm_mday = (int)getzword(19); + unpacked_date.tm_mon = (int)getzword(20) - 1; + unpacked_date.tm_year = (int)getzword(21) - 1900; + unpacked_date.tm_hour = (int)getzword(22); + unpacked_date.tm_min = (int)getzword(23); + unpacked_date.tm_sec = (int)getzword(24); + subseconds = (int)getzlong(25) / 1e9; /*@ -compdef */ session->gpsdata.fix.time = session->gpsdata.sentence_time = (double)mkgmtime(&unpacked_date) + subseconds; @@ -183,53 +186,53 @@ static gps_mask_t handle1000(struct gps_device_t *session) (void)ntpshm_put(session, session->gpsdata.fix.time + 1.1); #endif /*@ -type @*/ - session->gpsdata.fix.latitude = ((long)getlong(27)) * RAD_2_DEG * 1e-8; - session->gpsdata.fix.longitude = ((long)getlong(29)) * RAD_2_DEG * 1e-8; + session->gpsdata.fix.latitude = ((long)getzlong(27)) * RAD_2_DEG * 1e-8; + session->gpsdata.fix.longitude = ((long)getzlong(29)) * RAD_2_DEG * 1e-8; /* * The Rockwell Jupiter TU30-D140 reports altitude as uncorrected height * above WGS84 geoid. The Zodiac binary protocol manual does not * specify whether word 31 is geodetic or WGS 84. */ - session->gpsdata.fix.altitude = ((long)getlong(31)) * 1e-2; + session->gpsdata.fix.altitude = ((long)getzlong(31)) * 1e-2; /*@ +type @*/ - session->gpsdata.separation = ((short)getword(33)) * 1e-2; + session->gpsdata.separation = ((short)getzword(33)) * 1e-2; session->gpsdata.fix.altitude -= session->gpsdata.separation; - session->gpsdata.fix.speed = (int)getlong(34) * 1e-2; - session->gpsdata.fix.track = (int)getword(36) * RAD_2_DEG * 1e-3; - session->mag_var = ((short)getword(37)) * RAD_2_DEG * 1e-4; - session->gpsdata.fix.climb = ((short)getword(38)) * 1e-2; - /* map_datum = getword(39); */ + session->gpsdata.fix.speed = (int)getzlong(34) * 1e-2; + session->gpsdata.fix.track = (int)getzword(36) * RAD_2_DEG * 1e-3; + session->mag_var = ((short)getzword(37)) * RAD_2_DEG * 1e-4; + session->gpsdata.fix.climb = ((short)getzword(38)) * 1e-2; + /* map_datum = getzword(39); */ /* manual says these are 1-sigma */ - session->gpsdata.fix.eph = (int)getlong(40) * 1e-2 * GPSD_CONFIDENCE; - session->gpsdata.fix.epv = (int)getlong(42) * 1e-2 * GPSD_CONFIDENCE; - session->gpsdata.fix.ept = (int)getlong(44) * 1e-2 * GPSD_CONFIDENCE; - session->gpsdata.fix.eps = (int)getword(46) * 1e-2 * GPSD_CONFIDENCE; - /* clock_bias = (int)getlong(47) * 1e-2; */ - /* clock_bias_sd = (int)getlong(49) * 1e-2; */ - /* clock_drift = (int)getlong(51) * 1e-2; */ - /* clock_drift_sd = (int)getlong(53) * 1e-2; */ + session->gpsdata.fix.eph = (int)getzlong(40) * 1e-2 * GPSD_CONFIDENCE; + session->gpsdata.fix.epv = (int)getzlong(42) * 1e-2 * GPSD_CONFIDENCE; + session->gpsdata.fix.ept = (int)getzlong(44) * 1e-2 * GPSD_CONFIDENCE; + session->gpsdata.fix.eps = (int)getzword(46) * 1e-2 * GPSD_CONFIDENCE; + /* clock_bias = (int)getzlong(47) * 1e-2; */ + /* clock_bias_sd = (int)getzlong(49) * 1e-2; */ + /* clock_drift = (int)getzlong(51) * 1e-2; */ + /* clock_drift_sd = (int)getzlong(53) * 1e-2; */ #if 0 gpsd_report(LOG_INF, "date: %lf\n", session->gpsdata.fix.time); gpsd_report(LOG_INF, " solution invalid:\n"); - gpsd_report(LOG_INF, " altitude: %d\n", (getword(10) & 1) ? 1 : 0); - gpsd_report(LOG_INF, " no diff gps: %d\n", (getword(10) & 2) ? 1 : 0); - gpsd_report(LOG_INF, " not enough satellites: %d\n", (getword(10) & 4) ? 1 : 0); - gpsd_report(LOG_INF, " exceed max EHPE: %d\n", (getword(10) & 8) ? 1 : 0); - gpsd_report(LOG_INF, " exceed max EVPE: %d\n", (getword(10) & 16) ? 1 : 0); + gpsd_report(LOG_INF, " altitude: %d\n", (getzword(10) & 1) ? 1 : 0); + gpsd_report(LOG_INF, " no diff gps: %d\n", (getzword(10) & 2) ? 1 : 0); + gpsd_report(LOG_INF, " not enough satellites: %d\n", (getzword(10) & 4) ? 1 : 0); + gpsd_report(LOG_INF, " exceed max EHPE: %d\n", (getzword(10) & 8) ? 1 : 0); + gpsd_report(LOG_INF, " exceed max EVPE: %d\n", (getzword(10) & 16) ? 1 : 0); gpsd_report(LOG_INF, " solution type:\n"); - gpsd_report(LOG_INF, " propagated: %d\n", (getword(11) & 1) ? 1 : 0); - gpsd_report(LOG_INF, " altitude: %d\n", (getword(11) & 2) ? 1 : 0); - gpsd_report(LOG_INF, " differential: %d\n", (getword(11) & 4) ? 1 : 0); - gpsd_report(LOG_INF, "Number of measurements in solution: %d\n", getword(12)); - gpsd_report(LOG_INF, "Lat: %f\n", getlong(27) * RAD_2_DEG * 1e-8); - gpsd_report(LOG_INF, "Lon: %f\n", getlong(29) * RAD_2_DEG * 1e-8); - gpsd_report(LOG_INF, "Alt: %f\n", (double) getlong(31) * 1e-2); - gpsd_report(LOG_INF, "Speed: %f\n", (double) getlong(34) * 1e-2 * MPS_TO_KNOTS); - gpsd_report(LOG_INF, "Map datum: %d\n", getword(39)); - 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); + gpsd_report(LOG_INF, " propagated: %d\n", (getzword(11) & 1) ? 1 : 0); + gpsd_report(LOG_INF, " altitude: %d\n", (getzword(11) & 2) ? 1 : 0); + gpsd_report(LOG_INF, " differential: %d\n", (getzword(11) & 4) ? 1 : 0); + gpsd_report(LOG_INF, "Number of measurements in solution: %d\n", getzword(12)); + gpsd_report(LOG_INF, "Lat: %f\n", getzlong(27) * RAD_2_DEG * 1e-8); + gpsd_report(LOG_INF, "Lon: %f\n", getzlong(29) * RAD_2_DEG * 1e-8); + gpsd_report(LOG_INF, "Alt: %f\n", (double) getzlong(31) * 1e-2); + gpsd_report(LOG_INF, "Speed: %f\n", (double) getzlong(34) * 1e-2 * MPS_TO_KNOTS); + gpsd_report(LOG_INF, "Map datum: %d\n", getzword(39)); + gpsd_report(LOG_INF, "Magnetic variation: %f\n", getzword(37) * RAD_2_DEG * 1e-4); + gpsd_report(LOG_INF, "Course: %f\n", getzword(36) * RAD_2_DEG * 1e-4); + gpsd_report(LOG_INF, "Separation: %f\n", getzword(33) * 1e-2); #endif session->gpsdata.sentence_length = 55; @@ -242,16 +245,16 @@ static gps_mask_t handle1002(struct gps_device_t *session) 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); */ + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + /* measurement_sequence = getzword(9); */ + /* gps_week = getzword(10); */ + /* gps_seconds = getzlong(11); */ + /* gps_nanoseconds = getzlong(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)); + session->driver.zodiac.Zv[i] = status = (int)getzword(15 + (3 * i)); + session->driver.zodiac.Zs[i] = prn = (int)getzword(16 + (3 * i)); /*@ +type @*/ #if 0 gpsd_report(LOG_INF, "Sat%02d:\n", i); @@ -260,14 +263,14 @@ static gps_mask_t handle1002(struct gps_device_t *session) 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))); + gpsd_report(LOG_INF, " C/No:%d\n", getzword(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)); + session->gpsdata.ss[j] = (int)getzword(17 + (3 * i)); break; } } @@ -278,25 +281,25 @@ 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); + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + session->gpsdata.gdop = (unsigned int)getzword(9) * 1e-2; + session->gpsdata.pdop = (unsigned int)getzword(10) * 1e-2; + session->gpsdata.hdop = (unsigned int)getzword(11) * 1e-2; + session->gpsdata.vdop = (unsigned int)getzword(12) * 1e-2; + session->gpsdata.tdop = (unsigned int)getzword(13) * 1e-2; + session->gpsdata.satellites = (int)getzword(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); + session->gpsdata.PRN[i] = (int)getzword(15 + (3 * i)); + session->gpsdata.azimuth[i] = (int)(((short)getzword(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); + session->gpsdata.elevation[i] = (int)(((short)getzword(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))); + i, getzword(15+(3 * i)),getzword(16+(3 * i)),getzword(17+(3 * i))); #endif } else { session->gpsdata.PRN[i] = 0; @@ -309,27 +312,27 @@ static gps_mask_t handle1003(struct gps_device_t *session) static void handle1005(struct gps_device_t *session UNUSED) { - /* ticks = getlong(6); */ - /* sequence = getword(8); */ - int numcorrections = (int)getword(12); + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + int numcorrections = (int)getzword(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)); + gpsd_report(LOG_INF, "Station bad: %d\n", (getzword(9) & 1) ? 1 : 0); + gpsd_report(LOG_INF, "User disabled: %d\n", (getzword(9) & 2) ? 1 : 0); + gpsd_report(LOG_INF, "Station ID: %d\n", getzword(10)); + gpsd_report(LOG_INF, "Age of last correction in seconds: %d\n", getzword(11)); + gpsd_report(LOG_INF, "Number of corrections: %d\n", getzword(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); + gpsd_report(LOG_INF, "Sat%02d:\n", getzword(13+i) & 0x3f); + gpsd_report(LOG_INF, "ephemeris:%d\n", (getzword(13+i) & 64) ? 1 : 0); + gpsd_report(LOG_INF, "rtcm corrections:%d\n", (getzword(13+i) & 128) ? 1 : 0); + gpsd_report(LOG_INF, "rtcm udre:%d\n", (getzword(13+i) & 256) ? 1 : 0); + gpsd_report(LOG_INF, "sat health:%d\n", (getzword(13+i) & 512) ? 1 : 0); + gpsd_report(LOG_INF, "rtcm sat health:%d\n", (getzword(13+i) & 1024) ? 1 : 0); + gpsd_report(LOG_INF, "corrections state:%d\n", (getzword(13+i) & 2048) ? 1 : 0); + gpsd_report(LOG_INF, "iode mismatch:%d\n", (getzword(13+i) & 4096) ? 1 : 0); } #endif if (session->gpsdata.fix.mode == MODE_NO_FIX) @@ -347,7 +350,9 @@ static gps_mask_t handle1011(struct gps_device_t *session) * 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 */ + getstringz(session->subtype, + session->packet.outbuffer, + 19, 28); /* software version field */ gpsd_report(LOG_INF, "Software version: %s\n", session->subtype); return DEVICEID_SET; } @@ -355,15 +360,15 @@ static gps_mask_t handle1011(struct gps_device_t *session) 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); + /* ticks = getzlong(6); */ + /* sequence = getzword(8); */ + /* utc_week_seconds = getzlong(14); */ + /* leap_nanoseconds = getzlong(17); */ + if ((int)(getzword(19) & 3) == 3) + session->context->leap_seconds = (int)getzword(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); + gpsd_report(LOG_INF, "Leap seconds: %d.%09d\n", getzword(16), getzlong(17)); + gpsd_report(LOG_INF, "UTC validity: %d\n", getzword(19) & 3); #endif } |