From 75fb682db4b9c5f95550cbaf402526730c5efa33 Mon Sep 17 00:00:00 2001 From: "Eric S. Raymond" Date: Tue, 1 Mar 2011 03:25:46 -0500 Subject: More creative macro abuse. --- driver_rtcm3.c | 145 +++++++++++++++++++++------------------------------------ 1 file changed, 53 insertions(+), 92 deletions(-) (limited to 'driver_rtcm3.c') diff --git a/driver_rtcm3.c b/driver_rtcm3.c index 18bbefc2..d69cdf33 100644 --- a/driver_rtcm3.c +++ b/driver_rtcm3.c @@ -72,10 +72,16 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) #define sgrab(width) (bitcount += width, sbits(buf, bitcount-width, width)) #define GPS_PSEUDORANGE(fld, len) \ {temp = (unsigned long)ugrab(len); \ - if (temp == GPS_INVALID_PSEUDORANGE) \ + if (temp == GPS_INVALID_PSEUDORANGE) \ fld.pseudorange = 0; \ else \ fld.pseudorange = temp * GPS_PSEUDORANGE_RESOLUTION;} +#define RANGEDIFF(fld, len) \ + temp = (long)sgrab(len); \ + if (temp == GPS_INVALID_PSEUDORANGE) \ + fld.rangediff = 0; \ + else \ + fld.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; assert(ugrab(8) == 0xD3); assert(ugrab(6) == 0x00); @@ -96,11 +102,7 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) R1001.ident = (ushort)ugrab(6); R1001.L1.indicator = (unsigned char)ugrab(1); GPS_PSEUDORANGE(R1001.L1, 24); - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1001.L1.rangediff = 0; - else - R1001.L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1001.L1, 20); R1001.L1.locktime = (unsigned char)sgrab(7); } #undef R1001 @@ -120,11 +122,7 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) R1002.ident = (ushort)ugrab(6); R1002.L1.indicator = (unsigned char)ugrab(1); GPS_PSEUDORANGE(R1002.L1, 24); - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1002.L1.rangediff = 0; - else - R1002.L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1002.L1, 20); R1002.L1.locktime = (unsigned char)sgrab(7); R1002.L1.ambiguity = (unsigned char)ugrab(8); R1002.L1.CNR = (ugrab(8)) * CARRIER_NOISE_RATIO_UNITS; @@ -147,11 +145,7 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) R1003.L1.indicator = (unsigned char)ugrab(1); GPS_PSEUDORANGE(R1003.L1, 24); - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1003.L1.rangediff = 0; - else - R1003.L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1003.L1, 20); R1003.L1.locktime = (unsigned char)sgrab(7); R1003.L2.indicator = (unsigned char)ugrab(2); GPS_PSEUDORANGE(R1003.L2, 24); @@ -179,21 +173,13 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) R1004.ident = (ushort)ugrab(6); R1004.L1.indicator = (bool)ugrab(1); GPS_PSEUDORANGE(R1004.L1, 24); - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1004.L1.rangediff = 0; - else - R1004.L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1004.L1, 20); R1004.L1.locktime = (unsigned char)sgrab(7); R1004.L1.ambiguity = (unsigned char)ugrab(8); R1004.L1.CNR = ugrab(8) * CARRIER_NOISE_RATIO_UNITS; R1004.L2.indicator = (unsigned char)ugrab(2); GPS_PSEUDORANGE(R1004.L2, 24); - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1004.L2.rangediff = 0; - else - R1004.L2.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1004.L2, 20); R1004.L2.locktime = (unsigned char)sgrab(7); R1004.L2.ambiguity = (unsigned char)ugrab(8); R1004.L2.CNR = ugrab(8) * CARRIER_NOISE_RATIO_UNITS; @@ -204,48 +190,47 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) break; case 1005: /* Stationary Antenna Reference Point, No Height Information */ - rtcm->rtcmtypes.rtcm3_1005.station_id = (unsigned short)ugrab(12); +#define R1005 rtcm->rtcmtypes.rtcm3_1005 + R1005.station_id = (unsigned short)ugrab(12); ugrab(6); /* reserved */ - if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1005.system = NAVSYSTEM_GPS; - else if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1005.system = NAVSYSTEM_GLONASS; - else if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1005.system = NAVSYSTEM_GALILEO; - rtcm->rtcmtypes.rtcm3_1005.reference_station = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1005.ecef_x = - sgrab(38) * ANTENNA_POSITION_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1005.single_receiver = ugrab(1); + temp = ugrab(3); + if ((temp & 0x04)!=0) + R1005.system = NAVSYSTEM_GPS; + if ((temp & 0x02)!=0) + R1005.system = NAVSYSTEM_GLONASS; + if ((temp & 0x01)!=0) + R1005.system = NAVSYSTEM_GALILEO; + R1005.reference_station = (bool)ugrab(1); + R1005.ecef_x = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + R1005.single_receiver = ugrab(1); ugrab(1); - rtcm->rtcmtypes.rtcm3_1005.ecef_y = - sgrab(38) * ANTENNA_POSITION_RESOLUTION; + R1005.ecef_y = sgrab(38) * ANTENNA_POSITION_RESOLUTION; ugrab(2); - rtcm->rtcmtypes.rtcm3_1005.ecef_z = - sgrab(38) * ANTENNA_POSITION_RESOLUTION; + R1005.ecef_z = sgrab(38) * ANTENNA_POSITION_RESOLUTION; +#undef R1005 assert(bitcount == 152); break; case 1006: /* Stationary Antenna Reference Point, with Height Information */ - rtcm->rtcmtypes.rtcm3_1006.station_id = (unsigned short)ugrab(12); +#define R1006 rtcm->rtcmtypes.rtcm3_1006 + R1006.station_id = (unsigned short)ugrab(12); + temp = ugrab(3); + if ((temp & 0x04)!=0) + R1006.system = NAVSYSTEM_GPS; + if ((temp & 0x02)!=0) + R1006.system = NAVSYSTEM_GLONASS; + if ((temp & 0x01)!=0) + R1006.system = NAVSYSTEM_GALILEO; ugrab(6); /* reserved */ - if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1006.system = NAVSYSTEM_GPS; - else if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1006.system = NAVSYSTEM_GLONASS; - else if ((bool)ugrab(1)) - rtcm->rtcmtypes.rtcm3_1006.system = NAVSYSTEM_GALILEO; - rtcm->rtcmtypes.rtcm3_1006.reference_station = (bool)ugrab(1); - rtcm->rtcmtypes.rtcm3_1006.ecef_x = - sgrab(38) * ANTENNA_POSITION_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1006.single_receiver = ugrab(1); + R1006.reference_station = (bool)ugrab(1); + R1006.ecef_x = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + R1006.single_receiver = ugrab(1); ugrab(1); - rtcm->rtcmtypes.rtcm3_1006.ecef_y = - sgrab(38) * ANTENNA_POSITION_RESOLUTION; + R1006.ecef_y = sgrab(38) * ANTENNA_POSITION_RESOLUTION; ugrab(2); - rtcm->rtcmtypes.rtcm3_1006.ecef_z = - sgrab(38) * ANTENNA_POSITION_RESOLUTION; - rtcm->rtcmtypes.rtcm3_1006.height = - ugrab(16) * ANTENNA_POSITION_RESOLUTION; + R1006.ecef_z = sgrab(38) * ANTENNA_POSITION_RESOLUTION; + R1006.height = ugrab(16) * ANTENNA_POSITION_RESOLUTION; +#undef R1006 assert(bitcount == 168); break; @@ -287,13 +272,8 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) R1009.L1.indicator = (bool)ugrab(1); R1009.L1.channel = (ushort)ugrab(5); R1009.L1.pseudorange = ugrab(25) * GLONASS_PSEUDORANGE_RESOLUTION; - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1009.L1.rangediff = 0; - else - R1009.L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; - R1009.L1.locktime = - (unsigned char)sgrab(7); + RANGEDIFF(R1009.L1, 20); + R1009.L1.locktime = (unsigned char)sgrab(7); } #undef R1009 assert(bitcount == @@ -314,13 +294,8 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) R1010.L1.indicator = (bool)ugrab(1); R1010.L1.channel = (ushort)ugrab(5); R1010.L1.pseudorange = ugrab(25) * GLONASS_PSEUDORANGE_RESOLUTION; - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1010.L1.rangediff = 0; - else - R1010.L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; - R1010.L1.locktime = - (unsigned char)sgrab(7); + RANGEDIFF(R1010.L1, 20); + R1010.L1.locktime = (unsigned char)sgrab(7); R1010.L1.ambiguity = (unsigned char)ugrab(7); R1010.L1.CNR = ugrab(8) * CARRIER_NOISE_RATIO_UNITS; } @@ -343,22 +318,14 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) R1011.L1.indicator = (bool)ugrab(1); R1011.L1.channel = (ushort)ugrab(5); R1011.L1.pseudorange = ugrab(25) * GLONASS_PSEUDORANGE_RESOLUTION; - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1011.L1.rangediff = 0; - else - R1011.L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1011.L1, 20); R1011.L1.locktime = (unsigned char)sgrab(7); R1011.L1.ambiguity = (unsigned char)ugrab(7); R1011.L1.CNR = ugrab(8) * CARRIER_NOISE_RATIO_UNITS; R1011.L2.indicator = (bool)ugrab(1); R1011.L2.channel = (ushort)ugrab(5); R1011.L2.pseudorange = ugrab(25) * GLONASS_PSEUDORANGE_RESOLUTION; - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1011.L2.rangediff = 0; - else - R1011.L2.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1011.L2, 20); R1011.L2.locktime = (unsigned char)sgrab(7); R1011.L2.ambiguity = (unsigned char)ugrab(7); R1011.L2.CNR = ugrab(8) * CARRIER_NOISE_RATIO_UNITS; @@ -382,19 +349,11 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) R1012.L1.indicator = (bool)ugrab(1); R1012.L1.channel = (ushort)ugrab(5); R1012.L1.pseudorange = ugrab(25) * GLONASS_PSEUDORANGE_RESOLUTION; - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1012.L1.rangediff = 0; - else - R1012.L1.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1012.L1, 20); R1012.L1.locktime = (unsigned char)sgrab(7); R1012.L2.indicator = (bool)ugrab(1); R1012.L2.pseudorange = ugrab(25) * GLONASS_PSEUDORANGE_RESOLUTION; - temp = (long)sgrab(20); - if (temp == GPS_INVALID_PSEUDORANGE) - R1012.L2.rangediff = 0; - else - R1012.L2.rangediff = temp * PSEUDORANGE_DIFF_RESOLUTION; + RANGEDIFF(R1012.L2, 20); R1012.L2.locktime = (unsigned char)sgrab(7); } #undef R1012 @@ -468,6 +427,8 @@ void rtcm3_unpack( /*@out@*/ struct rtcm3_t *rtcm, char *buf) // (RTCM2_WORDS_MAX - 2) * sizeof(isgps30bits_t)); break; } +#undef RANGEDIFF +#undef GPS_PSEUDORANGE #undef sgrab #undef ugrab /*@ +evalorder +sefparams +mayaliasunique @*/ -- cgit v1.2.1