diff options
| -rw-r--r-- | Makefile.am | 9 | ||||
| -rw-r--r-- | gps.h | 2 | ||||
| -rw-r--r-- | gpsd.h | 3 | ||||
| -rw-r--r-- | isgps.c | 12 | ||||
| -rw-r--r-- | rtcm.c | 136 | ||||
| -rw-r--r-- | rtcmdecode.c | 16 | ||||
| -rw-r--r-- | test/undump.sample | 30 |
7 files changed, 144 insertions, 64 deletions
diff --git a/Makefile.am b/Makefile.am index 93d03964..52cfb945 100644 --- a/Makefile.am +++ b/Makefile.am @@ -288,5 +288,12 @@ packet-regress: packet_test packet-makeregress: packet_test packet_test >test/packet.test.chk -testregress: gps-regress rtcm-regress packet-regress +# Test RTCM encoding +undump-regress: rtcmdecode + @echo "Testing RTCM encoding..." + @rtcmdecode -p <test/undump.sample >/tmp/undump.test + @diff -u test/undump.sample /tmp/undump.test + @rm /tmp/undump.test + +testregress: gps-regress rtcm-regress packet-regress undump-regress @echo "Regressions complete." @@ -85,7 +85,7 @@ struct gps_fix_t { */ #define RTCM_WORDS_MAX 33 #define MAXCORRECTIONS 18 /* max correction count in type 1 or 9 */ -#define MAXSTATIONS 3 /* maximum stations in almanac, type 5 */ +#define MAXSTATIONS 10 /* maximum stations in almanac, type 5 */ /* RTCM104 doesn't specify this, so give it the largest reasonable value */ #define MAXHEALTH (RTCM_WORDS_MAX-2) @@ -202,7 +202,8 @@ struct gps_device_t { * be able to build rtcmdecode even when RTCM support is not * configured in the daemon. It doesn't take up extra space. */ - struct { + struct { + /* ISGPS200 decodng */ bool locked; int curr_offset; isgps30bits_t curr_word; @@ -98,7 +98,7 @@ static unsigned int reverse_bits[] = { }; /*@ -charint @*/ -static unsigned int isgpsparity(isgps30bits_t th) +static unsigned int isgps_parity(isgps30bits_t th) { #define P_30_MASK 0x40000000u @@ -142,7 +142,7 @@ static unsigned int isgpsparity(isgps30bits_t th) } -#define isgpsparityok(w) (isgpsparity(w) == ((w) & 0x3f)) +#define isgps_parityok(w) (isgps_parity(w) == ((w) & 0x3f)) #if 0 /* @@ -152,7 +152,7 @@ static unsigned int isgpsparity(isgps30bits_t th) * The symptom is that parity computation is screwed up and the decoder * never achieves sync lock. Something steps on the argument to * isgpsparity(); the lossage appears to be related to the compiler's - * attempt to fold the isgpsparity() call into isgpsparityok() in some + * attempt to fold the isgps_parity() call into isgps_parityok() in some * tail-recursion-like manner. This happens under -O2, but not -O1, on * both i386 and amd64. Disabling all of the individual -O2 suboptions * does *not* fix it. @@ -164,7 +164,7 @@ static unsigned int isgpsparity(isgps30bits_t th) * * gcc 4.0 does not manifest these bugs. */ -static bool isgpsparityok(isgps30bits_t w) +static bool isgps_parityok(isgps30bits_t w) { return (isgpsparity(w) == (w & 0x3f)); } @@ -210,7 +210,7 @@ enum isgpsstat_t isgps_decode(struct gps_device_t *session, gpsd_report(RTCM_ERRLEVEL_BASE+2, "syncing at byte %d: %0x%08x\n", session->char_counter, session->driver.isgps.curr_word); if (preamble_match(&session->driver.isgps.curr_word)) { - if (isgpsparityok(session->driver.isgps.curr_word)) { + if (isgps_parityok(session->driver.isgps.curr_word)) { gpsd_report(RTCM_ERRLEVEL_BASE+1, "preamble ok, parity ok -- locked\n"); session->driver.isgps.locked = true; @@ -237,7 +237,7 @@ enum isgpsstat_t isgps_decode(struct gps_device_t *session, if (session->driver.isgps.curr_word & P_30_MASK) session->driver.isgps.curr_word ^= W_DATA_MASK; - if (isgpsparityok(session->driver.isgps.curr_word)) { + if (isgps_parityok(session->driver.isgps.curr_word)) { #if 0 /* * Don't clobber the buffer just because we spot @@ -725,10 +725,10 @@ bool rtcm_repack(struct gps_device_t *session) m->w4.x_l = x & 0xff; m->w3.x_h = x >> 8; - m->w5.y_l = y & 0xff; - m->w4.y_h = y >> 8; - m->w6.z_l = z & 0xff; - m->w5.z_h = z >> 8; + m->w5.y_l = y & 0xffff; + m->w4.y_h = y >> 16; + m->w6.z_l = z & 0xffffff; + m->w5.z_h = z >> 24; } break; case 4: @@ -776,7 +776,7 @@ bool rtcm_repack(struct gps_device_t *session) m->sat_id = csp->ident; m->issue_of_data_link = (unsigned)csp->iodl; m->data_health = csp->health; - m->cn0 = (csp->snr == SNR_BAD) ? 0 : (unsigned)csp->snr - CNR_OFFSET; + m->cn0 = (csp->snr == SNR_BAD) ? 0 : (unsigned)csp->snr-CNR_OFFSET; m->health_enable = csp->health_en; m->new_nav_data = (unsigned)csp->new_data; m->loss_warn = (unsigned)csp->los_warning; @@ -785,7 +785,7 @@ bool rtcm_repack(struct gps_device_t *session) break; case 7: for (w = 0; w < (RTCM_WORDS_MAX - 2)/ 3; w++) { - struct station_t *np = &tp->msg_data.almanac.station[n]; + struct station_t *np = &tp->msg_data.almanac.station[n++]; struct b_station_t *mp = &msg->msg_type.type7.almanac[w]; mp->w3.lat = (int)(np->latitude / LA_SCALE); @@ -795,7 +795,7 @@ bool rtcm_repack(struct gps_device_t *session) /*@ +shiftimplementation @*/ mp->w4.lon_l = (unsigned)sval & 0xff; mp->w4.range = np->range; - uval = (unsigned)((np->frequency / FREQ_SCALE) - FREQ_OFFSET); + uval = (unsigned)(((np->frequency-FREQ_OFFSET) / FREQ_SCALE)); mp->w4.freq_h = uval >> 6; mp->w5.freq_l = uval % 0x3f; mp->w5.health = np->health; @@ -912,8 +912,7 @@ void rtcm_dump(struct gps_device_t *session, /*@out@*/char buf[], size_t buflen) for (n = 0; n < session->gpsdata.rtcm.msg_data.conhealth.nentries; n++) { struct consat_t *csp = &session->gpsdata.rtcm.msg_data.conhealth.sat[n]; (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - /* FIXME: turn these spaces to tabs someday */ - "C\t%2u\t%1u %1u\t%2d\t%1u %1u %1u\t%2u\n", + "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n", csp->ident, (unsigned)csp->iodl, (unsigned)csp->health, @@ -945,26 +944,29 @@ void rtcm_dump(struct gps_device_t *session, /*@out@*/char buf[], size_t buflen) break; case 16: (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "T \"%s\"\n", session->gpsdata.rtcm.msg_data.message); + "T\t\"%s\"\n", session->gpsdata.rtcm.msg_data.message); break; default: for (n = 0; n < session->gpsdata.rtcm.length; n++) (void)snprintf(buf + strlen(buf), buflen - strlen(buf), - "U 0x%08x\n", session->gpsdata.rtcm.msg_data.words[n]); + "U\t0x%08x\n", session->gpsdata.rtcm.msg_data.words[n]); break; } } int rtcm_undump(/*@out@*/struct rtcm_t *rtcmp, char *buf) +/* merge a line of data into an RTCM structure, return 0 if done */ { int fldcount, v; unsigned n; + char buf2[BUFSIZ]; + /*@ -usedef @*/ switch (rtcmp->type) { case 0: - fldcount = sscanf(buf, "H\t%u\t%u\t%1lf\t%u\t%u\t%u\n", + fldcount = sscanf(buf, "H\t%u\t%u\t%lf\t%u\t%u\t%u\n", &rtcmp->type, &rtcmp->refstaid, &rtcmp->zcount, @@ -973,7 +975,9 @@ int rtcm_undump(/*@out@*/struct rtcm_t *rtcmp, char *buf) &rtcmp->stathlth); if (fldcount != 6) return -1; - break; + else + return 1; + //break; case 1: case 9: @@ -987,10 +991,14 @@ int rtcm_undump(/*@out@*/struct rtcm_t *rtcmp, char *buf) &rsp->issuedata, &rsp->rangerr, &rsp->rangerate); - if (fldcount != 5) - return (int)rtcmp->type; + if (fldcount != 5 || (rtcmp->type != 1 && rtcmp->type != 9)) + return (int)(-rtcmp->type-1); + else if (rtcmp->msg_data.ranges.nentries != rtcmp->length*3/5) + return (int)(rtcmp->type+1); + else + return 0; } - break; + //break; case 3: fldcount = sscanf(buf, @@ -998,40 +1006,44 @@ int rtcm_undump(/*@out@*/struct rtcm_t *rtcmp, char *buf) &rtcmp->msg_data.ecef.x, &rtcmp->msg_data.ecef.y, &rtcmp->msg_data.ecef.z); - if (fldcount == 3) + if (fldcount != 3 || rtcmp->type != 3) + return -4; + else { rtcmp->msg_data.ecef.valid = true; - else - return 3; - break; + return 0; + } + //break; case 4: fldcount = sscanf(buf, "D\t%s\t%1d\t%s\t%lf\t%lf\t%lf\n", - buf, + buf2, &v, (char *)&rtcmp->msg_data.reference.datum, &rtcmp->msg_data.reference.dx, &rtcmp->msg_data.reference.dy, &rtcmp->msg_data.reference.dz); - if (fldcount != 6) - return 4; - if (strcmp(buf, "GPS") == 0) - rtcmp->msg_data.reference.system = gps; - else if (strcmp(buf, "GLONASS") == 0) - rtcmp->msg_data.reference.system = glonass; - else - rtcmp->msg_data.reference.system = unknown; - rtcmp->msg_data.reference.sense = (int)v; - rtcmp->msg_data.reference.valid = true; - break; + if (fldcount != 6 || rtcmp->type != 4) + return -5; + else { + if (strcmp(buf2, "GPS") == 0) + rtcmp->msg_data.reference.system = gps; + else if (strcmp(buf2, "GLONASS") == 0) + rtcmp->msg_data.reference.system = glonass; + else + rtcmp->msg_data.reference.system = unknown; + rtcmp->msg_data.reference.sense = (v == 1) ? global : ((v == 0) ? local : invalid); + rtcmp->msg_data.reference.valid = true; + return 0; + } + //break; case 5: { struct consat_t *csp = &rtcmp->msg_data.conhealth.sat[rtcmp->msg_data.conhealth.nentries++]; fldcount = sscanf(buf, - /* FIXME: turn these spaces to tabs someday */ - "C\t%2u\t%1u %1u\t%2d\t%1u %1u %1u\t%2u\n", + "C\t%2u\t%1u\t%1u\t%2d\t%1u\t%1u\t%1u\t%2u\n", &csp->ident, (unsigned int *)&csp->iodl, &csp->health, @@ -1040,13 +1052,21 @@ int rtcm_undump(/*@out@*/struct rtcm_t *rtcmp, char *buf) (unsigned int *)&csp->new_data, (unsigned int *)&csp->los_warning, &csp->tou); - if (fldcount != 8) - return 5; + if (fldcount != 8 || rtcmp->type != 5) + return -6; + else if (rtcmp->msg_data.conhealth.nentries < rtcmp->length) + return 6; + else + return 0; } - break; + //break; case 6: /* NOP msg */ - break; + if (buf[0] != 'N') + return -7; + else + return 0; + //break; case 7: { @@ -1060,28 +1080,40 @@ int rtcm_undump(/*@out@*/struct rtcm_t *rtcmp, char *buf) &ssp->health, &ssp->station_id, &ssp->bitrate); - if (fldcount != 7) - return 7; + if (fldcount != 7 || rtcmp->type != 7) + return 8; + else if (rtcmp->msg_data.almanac.nentries < rtcmp->length/3) + return 8; + else + return 0; } - break; + //break; + case 16: - fldcount = sscanf(buf, "T \"%[^\"]\"\n", rtcmp->msg_data.message); + fldcount = sscanf(buf, "T\t\"%[^\"]\"\n", rtcmp->msg_data.message); if (fldcount != 1) return 16; - break; + else + return 0; + //break; default: - for (n = 0; n < sizeof(rtcmp->msg_data.words)/sizeof(rtcmp->msg_data.words[0]); n++) - if (rtcmp->msg_data.words[n] == 0) - break; - if (sscanf(buf, "U 0x%08x\n", &v) == 1) - rtcmp->msg_data.words[n] = (isgps30bits_t)v; + for (n = 0; n < (unsigned)(sizeof(rtcmp->msg_data.words)/sizeof(rtcmp->msg_data.words[0])); n++) + if (rtcmp->msg_data.words[n] == 0) + break; + fldcount = sscanf(buf, "U\t0x%08x\n", &v); + if (fldcount != 1) + return (int)(-rtcmp->type-1); + else { + rtcmp->msg_data.words[n] = (isgps30bits_t)v; + if (n == rtcmp->length-1) + return 0; else - return (int)rtcmp->type; - break; + return (int)(rtcmp->type+1); + } + //break; } - - return 0; + /*@ +usedef @*/ } #ifdef __UNUSED__ diff --git a/rtcmdecode.c b/rtcmdecode.c index 3454baf7..972a2467 100644 --- a/rtcmdecode.c +++ b/rtcmdecode.c @@ -56,15 +56,25 @@ static void passthrough(FILE *fpin, FILE *fpout) char buf[BUFSIZ]; struct gps_device_t rtcmdata; + memset(&rtcmdata, 0, sizeof(rtcmdata)); while (fgets(buf, sizeof(buf), fpin) != NULL) { - int status = rtcm_undump(&rtcmdata.gpsdata.rtcm, buf); + int status; - /* repack/unpack goes here */ + /* pass through comment lines without interpreting */ + if (buf[0] == '#') { + (void)fputs(buf, fpout); + continue; + } + + status = rtcm_undump(&rtcmdata.gpsdata.rtcm, buf); if (status == 0) { + //rtcm_repack(&rtcmdata); + //rtcm_unpack(&rtcmdata); (void)rtcm_dump(&rtcmdata, buf, sizeof(buf)); (void)fputs(buf, fpout); - } else { + memset(&rtcmdata, 0, sizeof(rtcmdata)); + } else if (status < 0) { (void) fprintf(stderr, "rtcmdecode: bailing out with status %d\n", status); exit(1); } diff --git a/test/undump.sample b/test/undump.sample new file mode 100644 index 00000000..4628e21f --- /dev/null +++ b/test/undump.sample @@ -0,0 +1,30 @@ +# Synthetic test load for the RTCM104 undumper. +# Has examples of each message type in it. +H 9 268 249.6 0 5 0 +S 13 0 3 249.6 -26.120 0.068 +S 2 0 73 249.6 1.220 -0.080 +S 8 0 22 249.6 23.760 0.030 +H 3 268 250.8 1 5 0 +R 222.20 333.30 444.40 +H 9 268 252.0 2 5 0 +S 27 0 62 252.0 -39.680 -0.016 +S 7 0 15 252.0 25.660 0.026 +S 26 0 128 252.0 12.840 0.118 +H 4 268 252.6 3 4 0 +D GPS 1 WGS84 25.8 30.5 33.0 +H 5 268 253.8 4 4 0 +C 29 0 0 53 0 0 0 0 +C 12 0 0 26 0 0 0 0 +C 3 0 0 50 0 0 0 0 +C 15 0 0 41 0 0 0 0 +H 6 268 255.0 5 0 0 +N +H 7 268 255.6 6 12 0 +A 38.8406 -121.3532 250 314.0 0 764 200 +A 39.4229 -121.6059 402 318.0 0 878 100 +A 37.1816 -122.3969 333 287.0 0 883 100 +A 40.4364 -124.4020 333 292.0 0 885 100 +H 16 268 255.8 7 7 0 +T "ABRACADABRA SHEMHAMPHORASH" +H 23 268 256.0 0 1 0 +U 0xffeeddcc |