Index: trunk/BNC/src/RTCM3/RTCM3Decoder.cpp =================================================================== --- trunk/BNC/src/RTCM3/RTCM3Decoder.cpp (revision 8156) +++ trunk/BNC/src/RTCM3/RTCM3Decoder.cpp (revision 8158) @@ -64,14 +64,19 @@ //////////////////////////////////////////////////////////////////////////// RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) : - GPSDecoder() { - - _staID = staID; + GPSDecoder() { + + _staID = staID; _rawFile = rawFile; - connect(this, SIGNAL(newGPSEph(t_ephGPS)), BNC_CORE, SLOT(slotNewGPSEph(t_ephGPS))); - connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE, SLOT(slotNewGlonassEph(t_ephGlo))); - connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE, SLOT(slotNewGalileoEph(t_ephGal))); - connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE, SLOT(slotNewSBASEph(t_ephSBAS))); - connect(this, SIGNAL(newBDSEph(t_ephBDS)), BNC_CORE, SLOT(slotNewBDSEph(t_ephBDS))); + connect(this, SIGNAL(newGPSEph(t_ephGPS)), BNC_CORE, + SLOT(slotNewGPSEph(t_ephGPS))); + connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE, + SLOT(slotNewGlonassEph(t_ephGlo))); + connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE, + SLOT(slotNewGalileoEph(t_ephGal))); + connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE, + SLOT(slotNewSBASEph(t_ephSBAS))); + connect(this, SIGNAL(newBDSEph(t_ephBDS)), BNC_CORE, + SLOT(slotNewBDSEph(t_ephBDS))); _MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0; @@ -82,5 +87,5 @@ RTCM3Decoder::~RTCM3Decoder() { QMapIterator it(_coDecoders); - while(it.hasNext()) + while (it.hasNext()) { it.next(); @@ -91,6 +96,5 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size) { bool decoded = false; bncTime CurrentObsTime; @@ -102,10 +106,10 @@ GETBITS(type, 12) - SKIPBITS(12) /* id */ - GETBITS(i,30) - + SKIPBITS(12) + /* id */ + GETBITS(i, 30) CurrentObsTime.set(i); - if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime) - { + + if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) { decoded = true; _obsList.append(_CurrentObsList); @@ -115,82 +119,83 @@ _CurrentTime = CurrentObsTime; - GETBITS(syncf,1) /* sync */ - GETBITS(numsats,5) - SKIPBITS(4) /* smind, smint */ - - while(numsats--) - { - int sv, code, l1range, amb=0; + GETBITS(syncf, 1) + /* sync */ + GETBITS(numsats, 5) + SKIPBITS(4) + /* smind, smint */ + + while (numsats--) { + int sv, code, l1range, amb = 0; t_satObs CurrentObs; CurrentObs._time = CurrentObsTime; GETBITS(sv, 6) - if(sv < 40) + if (sv < 40) CurrentObs._prn.set('G', sv); else - CurrentObs._prn.set('S', sv-20); + CurrentObs._prn.set('S', sv - 20); t_frqObs *frqObs = new t_frqObs; /* L1 */ GETBITS(code, 1); - (code) ? frqObs->_rnxType2ch.assign("1W") : frqObs->_rnxType2ch.assign("1C"); + (code) ? + frqObs->_rnxType2ch.assign("1W") : frqObs->_rnxType2ch.assign("1C"); GETBITS(l1range, 24); GETBITSSIGN(i, 20); - if((i&((1<<20)-1)) != 0x80000) - { - frqObs->_code = l1range*0.02; - frqObs->_phase = (l1range*0.02+i*0.0005)/GPS_WAVELENGTH_L1; + if ((i & ((1 << 20) - 1)) != 0x80000) { + frqObs->_code = l1range * 0.02; + frqObs->_phase = (l1range * 0.02 + i * 0.0005) / GPS_WAVELENGTH_L1; frqObs->_codeValid = frqObs->_phaseValid = true; } GETBITS(i, 7); frqObs->_slipCounter = i; - if(type == 1002 || type == 1004) - { - GETBITS(amb,8); - if(amb) - { - frqObs->_code += amb*299792.458; - frqObs->_phase += (amb*299792.458)/GPS_WAVELENGTH_L1; + if (type == 1002 || type == 1004) { + GETBITS(amb, 8); + if (amb) { + frqObs->_code += amb * 299792.458; + frqObs->_phase += (amb * 299792.458) / GPS_WAVELENGTH_L1; } GETBITS(i, 8); - if(i) - { - frqObs->_snr = i*0.25; + if (i) { + frqObs->_snr = i * 0.25; frqObs->_snrValid = true; } } CurrentObs._obs.push_back(frqObs); - if(type == 1003 || type == 1004) - { + if (type == 1003 || type == 1004) { frqObs = new t_frqObs; /* L2 */ - GETBITS(code,2); - switch(code) - { - case 3: frqObs->_rnxType2ch.assign("2W"); /* or "2Y"? */ break; - case 2: frqObs->_rnxType2ch.assign("2W"); break; - case 1: frqObs->_rnxType2ch.assign("2P"); break; - case 0: frqObs->_rnxType2ch.assign("2X"); /* or "2S" or "2L"? */ break; - } - GETBITSSIGN(i,14); - if((i&((1<<14)-1)) != 0x2000) - { - frqObs->_code = l1range*0.02+i*0.02+amb*299792.458; + GETBITS(code, 2); + switch (code) { + case 3: + frqObs->_rnxType2ch.assign("2W"); /* or "2Y"? */ + break; + case 2: + frqObs->_rnxType2ch.assign("2W"); + break; + case 1: + frqObs->_rnxType2ch.assign("2P"); + break; + case 0: + frqObs->_rnxType2ch.assign("2X"); /* or "2S" or "2L"? */ + break; + } + GETBITSSIGN(i, 14); + if ((i & ((1 << 14) - 1)) != 0x2000) { + frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 299792.458; frqObs->_codeValid = true; } - GETBITSSIGN(i,20); - if((i&((1<<20)-1)) != 0x80000) - { - frqObs->_phase = (l1range*0.02+i*0.0005+amb*299792.458)/GPS_WAVELENGTH_L2; + GETBITSSIGN(i, 20); + if ((i & ((1 << 20) - 1)) != 0x80000) { + frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 299792.458) + / GPS_WAVELENGTH_L2; frqObs->_phaseValid = true; } - GETBITS(i,7); + GETBITS(i, 7); frqObs->_slipCounter = i; - if(type == 1004) - { + if (type == 1004) { GETBITS(i, 8); - if(i) - { - frqObs->_snr = i*0.25; + if (i) { + frqObs->_snr = i * 0.25; frqObs->_snrValid = true; } @@ -201,6 +206,5 @@ } - if(!syncf) - { + if (!syncf) { decoded = true; _obsList.append(_CurrentObsList); @@ -230,39 +234,38 @@ /** MSM signal types for GPS and SBAS */ -static struct CodeData gps[RTCM3_MSM_NUMSIG] = -{ - {0.0,0}, - {GPS_WAVELENGTH_L1,"1C"}, - {GPS_WAVELENGTH_L1,"1P"}, - {GPS_WAVELENGTH_L1,"1W"}, - {0.0,0}/*{GPS_WAVELENGTH_L1,"1Y"}*/, - {0.0,0}, - {0.0,0}, - {GPS_WAVELENGTH_L2,"2C"}, - {GPS_WAVELENGTH_L2,"2P"}, - {GPS_WAVELENGTH_L2,"2W"}, - {0.0,0}/*{GPS_WAVELENGTH_L2,"2Y"}*/, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {GPS_WAVELENGTH_L2,"2S"}, - {GPS_WAVELENGTH_L2,"2L"}, - {GPS_WAVELENGTH_L2,"2X"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {GPS_WAVELENGTH_L5,"5I"}, - {GPS_WAVELENGTH_L5,"5Q"}, - {GPS_WAVELENGTH_L5,"5X"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {GPS_WAVELENGTH_L1,"1S"}, - {GPS_WAVELENGTH_L1,"1L"}, - {GPS_WAVELENGTH_L1,"1X"} -}; +static struct CodeData gps[RTCM3_MSM_NUMSIG] = { + {0.0, 0}, + {GPS_WAVELENGTH_L1, "1C"}, + {GPS_WAVELENGTH_L1, "1P"}, + {GPS_WAVELENGTH_L1, "1W"}, + {0.0, 0}/*{GPS_WAVELENGTH_L1,"1Y"}*/, + {0.0, 0}, + {0.0, 0}, + {GPS_WAVELENGTH_L2, "2C"}, + {GPS_WAVELENGTH_L2, "2P"}, + {GPS_WAVELENGTH_L2, "2W"}, + {0.0, 0}/*{GPS_WAVELENGTH_L2,"2Y"}*/, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {GPS_WAVELENGTH_L2, "2S"}, + {GPS_WAVELENGTH_L2, "2L"}, + {GPS_WAVELENGTH_L2, "2X"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {GPS_WAVELENGTH_L5, "5I"}, + {GPS_WAVELENGTH_L5, "5Q"}, + {GPS_WAVELENGTH_L5, "5X"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {GPS_WAVELENGTH_L1, "1S"}, + {GPS_WAVELENGTH_L1, "1L"}, + {GPS_WAVELENGTH_L1, "1X"} + }; /** @@ -271,150 +274,146 @@ * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later! */ -static struct CodeData glo[RTCM3_MSM_NUMSIG] = -{ - {0.0,0}, - {0.0,"1C"}, - {0.0,"1P"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {1.0,"2C"}, - {1.0,"2P"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0} -}; +static struct CodeData glo[RTCM3_MSM_NUMSIG] = { + {0.0, 0}, + {0.0, "1C"}, + {0.0, "1P"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {1.0, "2C"}, + {1.0, "2P"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0} + }; /** MSM signal types for Galileo */ -static struct CodeData gal[RTCM3_MSM_NUMSIG] = -{ - {0.0,0}, - {GAL_WAVELENGTH_E1,"1C"}, - {GAL_WAVELENGTH_E1,"1A"}, - {GAL_WAVELENGTH_E1,"1B"}, - {GAL_WAVELENGTH_E1,"1X"}, - {GAL_WAVELENGTH_E1,"1Z"}, - {0.0,0}, - {GAL_WAVELENGTH_E6,"6C"}, - {GAL_WAVELENGTH_E6,"6A"}, - {GAL_WAVELENGTH_E6,"6B"}, - {GAL_WAVELENGTH_E6,"6X"}, - {GAL_WAVELENGTH_E6,"6Z"}, - {0.0,0}, - {GAL_WAVELENGTH_E5B,"7I"}, - {GAL_WAVELENGTH_E5B,"7Q"}, - {GAL_WAVELENGTH_E5B,"7X"}, - {0.0,0}, - {GAL_WAVELENGTH_E5AB,"8I"}, - {GAL_WAVELENGTH_E5AB,"8Q"}, - {GAL_WAVELENGTH_E5AB,"8X"}, - {0.0,0}, - {GAL_WAVELENGTH_E5A,"5I"}, - {GAL_WAVELENGTH_E5A,"5Q"}, - {GAL_WAVELENGTH_E5A,"5X"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, -}; +static struct CodeData gal[RTCM3_MSM_NUMSIG] = { + {0.0, 0}, + {GAL_WAVELENGTH_E1, "1C"}, + {GAL_WAVELENGTH_E1, "1A"}, + {GAL_WAVELENGTH_E1, "1B"}, + {GAL_WAVELENGTH_E1, "1X"}, + {GAL_WAVELENGTH_E1, "1Z"}, + {0.0, 0}, + {GAL_WAVELENGTH_E6, "6C"}, + {GAL_WAVELENGTH_E6, "6A"}, + {GAL_WAVELENGTH_E6, "6B"}, + {GAL_WAVELENGTH_E6, "6X"}, + {GAL_WAVELENGTH_E6, "6Z"}, + {0.0, 0}, + {GAL_WAVELENGTH_E5B, "7I"}, + {GAL_WAVELENGTH_E5B, "7Q"}, + {GAL_WAVELENGTH_E5B, "7X"}, + {0.0, 0}, + {GAL_WAVELENGTH_E5AB, "8I"}, + {GAL_WAVELENGTH_E5AB, "8Q"}, + {GAL_WAVELENGTH_E5AB, "8X"}, + {0.0, 0}, + {GAL_WAVELENGTH_E5A, "5I"}, + {GAL_WAVELENGTH_E5A, "5Q"}, + {GAL_WAVELENGTH_E5A, "5X"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + }; /** MSM signal types for QZSS */ -static struct CodeData qzss[RTCM3_MSM_NUMSIG] = -{ - {0.0,0}, - {GPS_WAVELENGTH_L1,"1C"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {GPS_WAVELENGTH_L1,"1Z"}, - {0.0,0}, - {0.0,0}, - {QZSS_WAVELENGTH_LEX,"6S"}, - {QZSS_WAVELENGTH_LEX,"6L"}, - {QZSS_WAVELENGTH_LEX,"6X"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {GPS_WAVELENGTH_L2,"2S"}, - {GPS_WAVELENGTH_L2,"2L"}, - {GPS_WAVELENGTH_L2,"2X"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {GPS_WAVELENGTH_L5,"5I"}, - {GPS_WAVELENGTH_L5,"5Q"}, - {GPS_WAVELENGTH_L5,"5X"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {GPS_WAVELENGTH_L1,"1D"}, - {GPS_WAVELENGTH_L1,"1P"}, - {GPS_WAVELENGTH_L1,"1X"} -}; +static struct CodeData qzss[RTCM3_MSM_NUMSIG] = { + {0.0, 0}, + {GPS_WAVELENGTH_L1, "1C"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {GPS_WAVELENGTH_L1, "1Z"}, + {0.0, 0}, + {0.0, 0}, + {QZSS_WAVELENGTH_LEX, "6S"}, + {QZSS_WAVELENGTH_LEX, "6L"}, + {QZSS_WAVELENGTH_LEX, "6X"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {GPS_WAVELENGTH_L2, "2S"}, + {GPS_WAVELENGTH_L2, "2L"}, + {GPS_WAVELENGTH_L2, "2X"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {GPS_WAVELENGTH_L5, "5I"}, + {GPS_WAVELENGTH_L5, "5Q"}, + {GPS_WAVELENGTH_L5, "5X"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {GPS_WAVELENGTH_L1, "1D"}, + {GPS_WAVELENGTH_L1, "1P"}, + {GPS_WAVELENGTH_L1, "1X"} + }; /** MSM signal types for Beidou/BDS */ -static struct CodeData bds[RTCM3_MSM_NUMSIG] = -{ - {0.0,0}, - {BDS_WAVELENGTH_B1,"2I"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {BDS_WAVELENGTH_B3,"6I"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {BDS_WAVELENGTH_B2,"7I"}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, - {0.0,0}, -}; +static struct CodeData bds[RTCM3_MSM_NUMSIG] = { + {0.0, 0}, + {BDS_WAVELENGTH_B1, "2I"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {BDS_WAVELENGTH_B3, "6I"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {BDS_WAVELENGTH_B2, "7I"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + }; #define UINT64(c) c ## ULL @@ -423,5 +422,5 @@ //////////////////////////////////////////////////////////////////////////// bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size) -{ + { bool decoded = false; int type, syncf, i; @@ -432,15 +431,16 @@ GETBITS(type, 12) - SKIPBITS(12) /* id */ + SKIPBITS(12) + /* id */ char sys; - if(type >= 1121) + if (type >= 1121) sys = 'C'; - else if(type >= 1111) + else if (type >= 1111) sys = 'J'; - else if(type >= 1101) + else if (type >= 1101) sys = 'S'; - else if(type >= 1091) + else if (type >= 1091) sys = 'E'; - else if(type >= 1081) + else if (type >= 1081) sys = 'R'; else @@ -448,22 +448,19 @@ bncTime CurrentObsTime; - if(sys == 'C') /* BDS */ - { - GETBITS(i,30) + if (sys == 'C') /* BDS */ { + GETBITS(i, 30) CurrentObsTime.setBDS(i); } - else if(sys == 'R') /* GLONASS */ - { + else if (sys == 'R') /* GLONASS */ { SKIPBITS(3) - GETBITS(i,27) /* tk */ + GETBITS(i, 27) + /* tk */ CurrentObsTime.setTk(i); } - else /* GPS style date */ - { - GETBITS(i,30) + else /* GPS style date */ { + GETBITS(i, 30) CurrentObsTime.set(i); } - if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime) - { + if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) { decoded = true; _obsList = _CurrentObsList; @@ -479,352 +476,345 @@ * the full cycles and can't be used later we skip interpretation here already. */ - if(type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7) - { + if (type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7) { int sigmask, numsat = 0, numsig = 0; uint64_t satmask, cellmask, ui; double rrmod[RTCM3_MSM_NUMSAT]; int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT], - extsat[RTCM3_MSM_NUMSAT]; + extsat[RTCM3_MSM_NUMSAT]; int ll[RTCM3_MSM_NUMCELLS]/*, hc[RTCM3_MSM_NUMCELLS]*/; double cnr[RTCM3_MSM_NUMCELLS]; double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS], - dop[RTCM3_MSM_NUMCELLS]; - - SKIPBITS(3+7+2+2+1+3) + dop[RTCM3_MSM_NUMCELLS]; + + SKIPBITS(3 + 7 + 2 + 2 + 1 + 3) GETBITS64(satmask, RTCM3_MSM_NUMSAT) /* http://gurmeetsingh.wordpress.com/2008/08/05/fast-bit-counting-routines/ */ - for(ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */) + for (ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */) ++numsat; GETBITS(sigmask, RTCM3_MSM_NUMSIG) - for(i = sigmask; i; i &= (i - 1) /* remove rightmost bit */) + for (i = sigmask; i; i &= (i - 1) /* remove rightmost bit */) ++numsig; - for(i = 0; i < RTCM3_MSM_NUMSAT; ++i) + for (i = 0; i < RTCM3_MSM_NUMSAT; ++i) extsat[i] = 15; - i = numsat*numsig; - GETBITS64(cellmask, (unsigned)i) - - switch(type % 10) - { - case 1: case 2: case 3: - /* partial data, already skipped above, but implemented for future expansion ! */ - for(int j = numsat; j--;) - GETFLOAT(rrmod[j], 10, 1.0/1024.0) - break; - case 4: case 6: - for(int j = numsat; j--;) - GETBITS(rrint[j], 8) - for(int j = numsat; j--;) - GETFLOAT(rrmod[j], 10, 1.0/1024.0) - break; - case 5: case 7: - for(int j = numsat; j--;) - GETBITS(rrint[j], 8) - for(int j = numsat; j--;) - GETBITS(extsat[j], 4) - for(int j = numsat; j--;) - GETFLOAT(rrmod[j], 10, 1.0/1024.0) - for(int j = numsat; j--;) - GETBITSSIGN(rdop[j], 14) - break; + i = numsat * numsig; + GETBITS64(cellmask, (unsigned )i) + + switch (type % 10) { + case 1: + case 2: + case 3: + /* partial data, already skipped above, but implemented for future expansion ! */ + for (int j = numsat; j--;) + GETFLOAT(rrmod[j], 10, 1.0 / 1024.0) + break; + case 4: + case 6: + for (int j = numsat; j--;) + GETBITS(rrint[j], 8) + for (int j = numsat; j--;) + GETFLOAT(rrmod[j], 10, 1.0 / 1024.0) + break; + case 5: + case 7: + for (int j = numsat; j--;) + GETBITS(rrint[j], 8) + for (int j = numsat; j--;) + GETBITS(extsat[j], 4) + for (int j = numsat; j--;) + GETFLOAT(rrmod[j], 10, 1.0 / 1024.0) + for (int j = numsat; j--;) + GETBITSSIGN(rdop[j], 14) + break; } - int numcells = numsat*numsig; + int numcells = numsat * numsig; /** Drop anything which exceeds our cell limit. Increase limit definition * when that happens. */ - if(numcells <= RTCM3_MSM_NUMCELLS) - { - switch(type % 10) - { - case 1: - for(int count = numcells; count--;) - if(cellmask & (UINT64(1)<= 0 && !(sigmask&(1<<--j))) + for (int count = numcells; count--;) { + while (j >= 0 && !(sigmask & (1 << --j))) ; - if(j < 0) - { - while(!(satmask&(UINT64(1)<<(--i)))) /* next satellite */ + if (j < 0) { + while (!(satmask & (UINT64(1) << (--i)))) + /* next satellite */ ; - if(CurrentObs._obs.size() > 0) + if (CurrentObs._obs.size() > 0) _CurrentObsList.push_back(CurrentObs); CurrentObs.clear(); CurrentObs._time = CurrentObsTime; - if(sys == 'S') - CurrentObs._prn.set(sys, 20-1+RTCM3_MSM_NUMSAT-i); + if (sys == 'S') + CurrentObs._prn.set(sys, 20 - 1 + RTCM3_MSM_NUMSAT - i); else - CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT-i); + CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT - i); j = RTCM3_MSM_NUMSIG; - while(!(sigmask&(1<<--j))) + while (!(sigmask & (1 << --j))) ; --numsat; } - if(cellmask & (UINT64(1)<_rnxType2ch.assign(cd.code); - switch(type % 10) - { - case 1: - if(psr[count] > -1.0/(1<<10)) - { - frqObs->_code = psr[count]*LIGHTSPEED/1000.0 - +(rrmod[numsat])*LIGHTSPEED/1000.0; - frqObs->_codeValid = true; - } - break; - case 2: - if(cp[count] > -1.0/(1<<8)) - { - frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl - +(rrmod[numsat])*LIGHTSPEED/1000.0/cd.wl; - frqObs->_phaseValid = true; - frqObs->_slipCounter = ll[count]; - } - break; - case 3: - if(psr[count] > -1.0/(1<<10)) - { - frqObs->_code = psr[count]*LIGHTSPEED/1000.0 - +(rrmod[numsat])*LIGHTSPEED/1000.0; - frqObs->_codeValid = true; - } - - if(cp[count] > -1.0/(1<<8)) - { - frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl - +rrmod[numsat]*LIGHTSPEED/1000.0/cd.wl; - frqObs->_phaseValid = true; - frqObs->_slipCounter = ll[count]; - } - break; - case 4: - if(psr[count] > -1.0/(1<<10)) - { - frqObs->_code = psr[count]*LIGHTSPEED/1000.0 - +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0; - frqObs->_codeValid = true; - } - - if(cp[count] > -1.0/(1<<8)) - { - frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl - +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl; - frqObs->_phaseValid = true; - frqObs->_slipCounter = ll[count]; - } - - frqObs->_snr = cnr[count]; - frqObs->_snrValid = true; - break; - case 5: - if(psr[count] > -1.0/(1<<10)) - { - frqObs->_code = psr[count]*LIGHTSPEED/1000.0 - +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0; - frqObs->_codeValid = true; - } - - if(cp[count] > -1.0/(1<<8)) - { - frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl - +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl; - frqObs->_phaseValid = true; - frqObs->_slipCounter = ll[count]; - } - - frqObs->_snr = cnr[count]; - frqObs->_snrValid = true; - - if(dop[count] > -1.6384) - { - frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl; - frqObs->_dopplerValid = true; - } - break; - case 6: - if(psr[count] > -1.0/(1<<10)) - { - frqObs->_code = psr[count]*LIGHTSPEED/1000.0 - +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0; - frqObs->_codeValid = true; - } - - if(cp[count] > -1.0/(1<<8)) - { - frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl - +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl; - frqObs->_phaseValid = true; - frqObs->_slipCounter = ll[count]; - } - - frqObs->_snr = cnr[count]; - frqObs->_snrValid = true; - break; - case 7: - if(psr[count] > -1.0/(1<<10)) - { - frqObs->_code = psr[count]*LIGHTSPEED/1000.0 - +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0; - frqObs->_codeValid = true; - } - - if(cp[count] > -1.0/(1<<8)) - { - frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl - +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl; - frqObs->_phaseValid = true; - frqObs->_slipCounter = ll[count]; - } - - frqObs->_snr = cnr[count]; - frqObs->_snrValid = true; - - if(dop[count] > -1.6384) - { - frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl; - frqObs->_dopplerValid = true; - } - break; + switch (type % 10) { + case 1: + if (psr[count] > -1.0 / (1 << 10)) { + frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 + + (rrmod[numsat]) * LIGHTSPEED / 1000.0; + frqObs->_codeValid = true; + } + break; + case 2: + if (cp[count] > -1.0 / (1 << 8)) { + frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl + + (rrmod[numsat]) * LIGHTSPEED / 1000.0 / cd.wl; + frqObs->_phaseValid = true; + frqObs->_slipCounter = ll[count]; + } + break; + case 3: + if (psr[count] > -1.0 / (1 << 10)) { + frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 + + (rrmod[numsat]) * LIGHTSPEED / 1000.0; + frqObs->_codeValid = true; + } + + if (cp[count] > -1.0 / (1 << 8)) { + frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl + + rrmod[numsat] * LIGHTSPEED / 1000.0 / cd.wl; + frqObs->_phaseValid = true; + frqObs->_slipCounter = ll[count]; + } + break; + case 4: + qDebug() << "MSM4"; + if (psr[count] > -1.0 / (1 << 10)) { + frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; + frqObs->_codeValid = true; + qDebug() << "frqObs->_codeValid"; + } + + if (cp[count] > -1.0 / (1 << 8)) { + frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 + / cd.wl; + frqObs->_phaseValid = true; + qDebug() << "frqObs->_phaseValid"; + frqObs->_slipCounter = ll[count]; + } + + frqObs->_snr = cnr[count]; + frqObs->_snrValid = true; + qDebug() << "frqObs->_snrValid"; + break; + case 5: + if (psr[count] > -1.0 / (1 << 10)) { + frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; + frqObs->_codeValid = true; + } + + if (cp[count] > -1.0 / (1 << 8)) { + frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 + / cd.wl; + frqObs->_phaseValid = true; + frqObs->_slipCounter = ll[count]; + } + + frqObs->_snr = cnr[count]; + frqObs->_snrValid = true; + + if (dop[count] > -1.6384) { + frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl; + frqObs->_dopplerValid = true; + } + break; + case 6: + if (psr[count] > -1.0 / (1 << 10)) { + frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; + frqObs->_codeValid = true; + } + + if (cp[count] > -1.0 / (1 << 8)) { + frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 + / cd.wl; + frqObs->_phaseValid = true; + frqObs->_slipCounter = ll[count]; + } + + frqObs->_snr = cnr[count]; + frqObs->_snrValid = true; + break; + case 7: + if (psr[count] > -1.0 / (1 << 10)) { + frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; + frqObs->_codeValid = true; + } + + if (cp[count] > -1.0 / (1 << 8)) { + frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 + / cd.wl; + frqObs->_phaseValid = true; + frqObs->_slipCounter = ll[count]; + } + + frqObs->_snr = cnr[count]; + frqObs->_snrValid = true; + + if (dop[count] > -1.6384) { + frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl; + frqObs->_dopplerValid = true; + } + break; } CurrentObs._obs.push_back(frqObs); @@ -832,15 +822,18 @@ } } - if(CurrentObs._obs.size() > 0) + qDebug() << CurrentObs._time.datestr().c_str() << " " + << CurrentObs._time.timestr().c_str(); + qDebug() << "CurrentObs._obs.size(): " << CurrentObs._obs.size(); + if (CurrentObs._obs.size() > 0) { _CurrentObsList.push_back(CurrentObs); + qDebug() << "_CurrentObsList.push_back(CurrentObs)"; + } } } - else if((type % 10) < 3) - { + else if ((type % 10) < 3) { emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!") - .arg(_staID).arg(type).toAscii(), true)); - } - if(!syncf) - { + .arg(_staID).arg(type).toAscii(), true)); + } + if (!syncf) { decoded = true; _obsList = _CurrentObsList; @@ -853,6 +846,5 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) { bool decoded = false; bncTime CurrentObsTime; @@ -864,10 +856,11 @@ GETBITS(type, 12) - SKIPBITS(12) /* id */ - GETBITS(i,27) /* tk */ + SKIPBITS(12) + /* id */ + GETBITS(i, 27) + /* tk */ CurrentObsTime.setTk(i); - if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime) - { + if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) { decoded = true; _obsList.append(_CurrentObsList); @@ -876,11 +869,12 @@ _CurrentTime = CurrentObsTime; - GETBITS(syncf,1) /* sync */ - GETBITS(numsats,5) - SKIPBITS(4) /* smind, smint */ - - while(numsats--) - { - int sv, code, l1range, amb=0, freq; + GETBITS(syncf, 1) + /* sync */ + GETBITS(numsats, 5) + SKIPBITS(4) + /* smind, smint */ + + while (numsats--) { + int sv, code, l1range, amb = 0, freq; t_satObs CurrentObs; CurrentObs._time = CurrentObsTime; @@ -890,67 +884,68 @@ GETBITS(code, 1) GETBITS(freq, 5) - GLOFreq[sv-1] = 100+freq-7; /* store frequency for other users (MSM) */ + GLOFreq[sv - 1] = 100 + freq - 7; /* store frequency for other users (MSM) */ t_frqObs *frqObs = new t_frqObs; /* L1 */ - (code) ? frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C"); + (code) ? + frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C"); GETBITS(l1range, 25); GETBITSSIGN(i, 20); - if((i&((1<<20)-1)) != 0x80000) - { - frqObs->_code = l1range*0.02; - frqObs->_phase = (l1range*0.02+i*0.0005)/GLO_WAVELENGTH_L1(freq-7); + if ((i & ((1 << 20) - 1)) != 0x80000) { + frqObs->_code = l1range * 0.02; + frqObs->_phase = (l1range * 0.02 + i * 0.0005) + / GLO_WAVELENGTH_L1(freq - 7); frqObs->_codeValid = frqObs->_phaseValid = true; } GETBITS(i, 7); frqObs->_slipCounter = i; - if(type == 1010 || type == 1012) - { - GETBITS(amb,7); - if(amb) - { - frqObs->_code += amb*599584.916; - frqObs->_phase += (amb*599584.916)/GLO_WAVELENGTH_L1(freq-7); + if (type == 1010 || type == 1012) { + GETBITS(amb, 7); + if (amb) { + frqObs->_code += amb * 599584.916; + frqObs->_phase += (amb * 599584.916) / GLO_WAVELENGTH_L1(freq - 7); } GETBITS(i, 8); - if(i) - { - frqObs->_snr = i*0.25; + if (i) { + frqObs->_snr = i * 0.25; frqObs->_snrValid = true; } } CurrentObs._obs.push_back(frqObs); - if(type == 1011 || type == 1012) - { + if (type == 1011 || type == 1012) { frqObs = new t_frqObs; /* L2 */ - GETBITS(code,2); - switch(code) - { - case 3: frqObs->_rnxType2ch.assign("2P"); break; - case 2: frqObs->_rnxType2ch.assign("2P"); break; - case 1: frqObs->_rnxType2ch.assign("2P"); break; - case 0: frqObs->_rnxType2ch.assign("2C"); break; - } - GETBITSSIGN(i,14); - if((i&((1<<14)-1)) != 0x2000) - { - frqObs->_code = l1range*0.02+i*0.02+amb*599584.916; + GETBITS(code, 2); + switch (code) { + case 3: + frqObs->_rnxType2ch.assign("2P"); + break; + case 2: + frqObs->_rnxType2ch.assign("2P"); + break; + case 1: + frqObs->_rnxType2ch.assign("2P"); + break; + case 0: + frqObs->_rnxType2ch.assign("2C"); + break; + } + GETBITSSIGN(i, 14); + if ((i & ((1 << 14) - 1)) != 0x2000) { + frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 599584.916; frqObs->_codeValid = true; } - GETBITSSIGN(i,20); - if((i&((1<<20)-1)) != 0x80000) - { - frqObs->_phase = (l1range*0.02+i*0.0005+amb*599584.916)/GLO_WAVELENGTH_L2(freq-7); + GETBITSSIGN(i, 20); + if ((i & ((1 << 20) - 1)) != 0x80000) { + frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 599584.916) + / GLO_WAVELENGTH_L2(freq - 7); frqObs->_phaseValid = true; } - GETBITS(i,7); + GETBITS(i, 7); frqObs->_slipCounter = i; - if(type == 1012) - { + if (type == 1012) { GETBITS(i, 8); - if(i) - { - frqObs->_snr = i*0.25; + if (i) { + frqObs->_snr = i * 0.25; frqObs->_snrValid = true; } @@ -960,6 +955,5 @@ _CurrentObsList.push_back(CurrentObs); } - if(!syncf) - { + if (!syncf) { decoded = true; _obsList.append(_CurrentObsList); @@ -972,10 +966,8 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) { bool decoded = false; - if(size == 67) - { + if (size == 67) { t_ephGPS eph; int i, week; @@ -999,33 +991,36 @@ GETBITS(i, 16) i <<= 4; - eph._TOC.set(i*1000); - GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25)) - GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13)) - GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1)) + eph._TOC.set(i * 1000); + GETFLOATSIGN(eph._clock_driftrate, 8, + 1.0 / (double )(1 << 30) / (double )(1 << 25)) + GETFLOATSIGN(eph._clock_drift, 16, + 1.0 / (double )(1 << 30) / (double )(1 << 13)) + GETFLOATSIGN(eph._clock_bias, 22, + 1.0 / (double )(1 << 30) / (double )(1 << 1)) GETBITS(eph._IODC, 10) - GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5)) + GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29)) - GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3)) - GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29)) - GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19)) + GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) + GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) + GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) + GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) GETBITS(i, 16) i <<= 4; eph._TOEsec = i; bncTime t; - t.set(i*1000); + t.set(i * 1000); eph._TOEweek = t.gpsw(); /* week from HOW, differs from TOC, TOE week, we use adapted value instead */ - if(eph._TOEweek > week + 1 || eph._TOEweek < week-1) /* invalid week */ + if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */ return false; - GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29)) + GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29)) + GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5)) + GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) - GETFLOATSIGN(eph._TGD, 8, 1.0/(double)(1<<30)/(double)(1<<1)) + GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1)) GETBITS(eph._health, 6) GETBITS(eph._L2PFlag, 1) @@ -1041,10 +1036,8 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) { bool decoded = false; - if(size == 51) - { + if (size == 51) { t_ephGlo eph; int sv, i, tk; @@ -1061,34 +1054,41 @@ GETBITS(i, 5) - eph._frequency_number = i-7; - GLOFreq[sv-1] = 100+i-7; /* store frequency for other users (MSM) */ - _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(eph._frequency_number,2,'f',0); - SKIPBITS(4) /* almanac healthy, almanac health ok, P1 */ + eph._frequency_number = i - 7; + GLOFreq[sv - 1] = 100 + i - 7; /* store frequency for other users (MSM) */ + _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg( + eph._frequency_number, 2, 'f', 0); + SKIPBITS(4) + /* almanac healthy, almanac health ok, P1 */ GETBITS(i, 5) - tk = i*60*60; + tk = i * 60 * 60; GETBITS(i, 6) - tk += i*60; + tk += i * 60; GETBITS(i, 1) - tk += i*30; - eph._tki = tk < 3*60*60 ? tk-3*60*60+86400 : tk-3*60*60; + tk += i * 30; + eph._tki = tk < 3 * 60 * 60 ? tk - 3 * 60 * 60 + 86400 : tk - 3 * 60 * 60; GETBITS(eph._health, 1) - SKIPBITS(1) /* P2 */ + SKIPBITS(1) + /* P2 */ GETBITS(i, 7) - eph._TOC.setTk(i*15*60*1000); /* tb */ - - GETFLOATSIGNM(eph._x_velocity, 24, 1.0/(double)(1<<20)) - GETFLOATSIGNM(eph._x_pos, 27, 1.0/(double)(1<<11)) - GETFLOATSIGNM(eph._x_acceleration, 5, 1.0/(double)(1<<30)) - GETFLOATSIGNM(eph._y_velocity, 24, 1.0/(double)(1<<20)) - GETFLOATSIGNM(eph._y_pos, 27, 1.0/(double)(1<<11)) - GETFLOATSIGNM(eph._y_acceleration, 5, 1.0/(double)(1<<30)) - GETFLOATSIGNM(eph._z_velocity, 24, 1.0/(double)(1<<20)) - GETFLOATSIGNM(eph._z_pos, 27, 1.0/(double)(1<<11)) - GETFLOATSIGNM(eph._z_acceleration, 5, 1.0/(double)(1<<30)) - SKIPBITS(1) /* P3 */ - GETFLOATSIGNM(eph._gamma, 11, 1.0/(double)(1<<30)/(double)(1<<10)) - SKIPBITS(3) /* GLONASS-M P, GLONASS-M ln (third string) */ - GETFLOATSIGNM(eph._tau, 22, 1.0/(double)(1<<30)) /* GLONASS tau n(tb) */ - SKIPBITS(5) /* GLONASS-M delta tau n(tb) */ + eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */ + + GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20)) + GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11)) + GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30)) + GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20)) + GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11)) + GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30)) + GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20)) + GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11)) + GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30)) + SKIPBITS(1) + /* P3 */ + GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10)) + SKIPBITS(3) + /* GLONASS-M P, GLONASS-M ln (third string) */ + GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30)) + /* GLONASS tau n(tb) */ + SKIPBITS(5) + /* GLONASS-M delta tau n(tb) */ GETBITS(eph._E, 5) /* GETBITS(i, 1) / * GLONASS-M P4 */ @@ -1123,10 +1123,8 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) { bool decoded = false; - if(size == 67) - { + if (size == 67) { t_ephGPS eph; int i, week; @@ -1144,17 +1142,20 @@ GETBITS(i, 16) i <<= 4; - eph._TOC.set(i*1000); - - GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25)) - GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13)) - GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1)) + eph._TOC.set(i * 1000); + + GETFLOATSIGN(eph._clock_driftrate, 8, + 1.0 / (double )(1 << 30) / (double )(1 << 25)) + GETFLOATSIGN(eph._clock_drift, 16, + 1.0 / (double )(1 << 30) / (double )(1 << 13)) + GETFLOATSIGN(eph._clock_bias, 22, + 1.0 / (double )(1 << 30) / (double )(1 << 1)) GETBITS(eph._IODE, 8) - GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5)) + GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29)) - GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3)) - GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29)) - GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19)) + GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) + GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) + GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) + GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) GETBITS(i, 16) i <<= 4; @@ -1163,9 +1164,9 @@ t.set(i); - GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29)) + GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29)) + GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5)) + GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) @@ -1176,14 +1177,14 @@ eph._TOEweek = t.gpsw(); /* week from HOW, differs from TOC, TOE week, we use adapted value instead */ - if(eph._TOEweek > week + 1 || eph._TOEweek < week-1) /* invalid week */ + if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */ return false; GETBITS(i, 4) - if(i <= 6) - eph._ura = ceil(10.0*pow(2.0, 1.0+i/2.0))/10.0; + if (i <= 6) + eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0; else - eph._ura = ceil(10.0*pow(2.0, i/2.0))/10.0; + eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0; GETBITS(eph._health, 6) - GETFLOATSIGN(eph._TGD, 8, 1.0/(double)(1<<30)/(double)(1<<1)) + GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1)) GETBITS(eph._IODC, 10) GETBITS(eph._fitInterval, 1) @@ -1199,10 +1200,8 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) { bool decoded = false; - if(size == 35) - { + if (size == 35) { t_ephSBAS eph; int i; @@ -1216,9 +1215,9 @@ GETBITS(i, 6) - eph._prn.set('S', 20+i); + eph._prn.set('S', 20 + i); GETBITS(eph._IODN, 8) GETBITS(i, 13) i <<= 4; - eph._TOC.setTOD(i*1000); + eph._TOC.setTOD(i * 1000); GETBITS(i, 4) eph._ura = accuracyFromIndex(i, eph.type()); @@ -1232,6 +1231,6 @@ GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125) GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625) - GETFLOATSIGN(eph._agf0, 12, 1.0/(1<<30)/(1<<1)) - GETFLOATSIGN(eph._agf1, 8, 1.0/(1<<30)/(1<<10)) + GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1)) + GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10)) eph._TOW = 0.9999E9; @@ -1246,6 +1245,5 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) { bool decoded = false; uint64_t numbits = 0, bitfield = 0; @@ -1256,6 +1254,5 @@ GETBITS(i, 12) - if((i == 1046 && size == 61) || (i == 1045 && size == 60)) - { + if ((i == 1046 && size == 61) || (i == 1045 && size == 60)) { t_ephGal eph; @@ -1273,33 +1270,37 @@ GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13)) GETBITSFACTOR(i, 14, 60) - eph._TOC.set(1024+eph._TOEweek, i); - GETFLOATSIGN(eph._clock_driftrate, 6, 1.0/(double)(1<<30)/(double)(1<<29)) - GETFLOATSIGN(eph._clock_drift, 21, 1.0/(double)(1<<30)/(double)(1<<16)) - GETFLOATSIGN(eph._clock_bias, 31, 1.0/(double)(1<<30)/(double)(1<<4)) - GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5)) + eph._TOC.set(1024 + eph._TOEweek, i); + GETFLOATSIGN(eph._clock_driftrate, 6, + 1.0 / (double )(1 << 30) / (double )(1 << 29)) + GETFLOATSIGN(eph._clock_drift, 21, + 1.0 / (double )(1 << 30) / (double )(1 << 16)) + GETFLOATSIGN(eph._clock_bias, 31, + 1.0 / (double )(1 << 30) / (double )(1 << 4)) + GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29)) - GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3)) - GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29)) - GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19)) + GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) + GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) + GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) + GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) GETBITSFACTOR(eph._TOEsec, 14, 60) /* FIXME: overwrite value, copied from old code */ eph._TOEsec = eph._TOC.gpssec(); - GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29)) + GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29)) + GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5)) + GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) - GETFLOATSIGN(eph._BGD_1_5A, 10, 1.0/(double)(1<<30)/(double)(1<<2)) - if(eph._inav) - { + GETFLOATSIGN(eph._BGD_1_5A, 10, + 1.0 / (double )(1 << 30) / (double )(1 << 2)) + if (eph._inav) { /* set unused F/NAV values */ eph._E5aHS = 0.0; eph._e5aDataInValid = false; - GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0/(double)(1<<30)/(double)(1<<2)) + GETFLOATSIGN(eph._BGD_1_5B, 10, + 1.0 / (double )(1 << 30) / (double )(1 << 2)) GETBITS(eph._E5bHS, 2) GETBITS(eph._e5bDataInValid, 1) @@ -1307,6 +1308,5 @@ GETBITS(eph._e1DataInValid, 1) } - else - { + else { /* set unused I/NAV values */ eph._BGD_1_5B = 0.0; @@ -1329,10 +1329,8 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) { bool decoded = false; - if(size == 70) - { + if (size == 70) { t_ephBDS eph; int i; @@ -1348,5 +1346,6 @@ eph._prn.set('C', i); - SKIPBITS(13) /* week */ + SKIPBITS(13) + /* week */ GETBITS(i, 4) eph._URA = accuracyFromIndex(i, eph.type()); @@ -1355,25 +1354,28 @@ GETBITS(i, 17) i <<= 3; - eph._TOC.setBDS(i*1000); - GETFLOATSIGN(eph._clock_driftrate, 11, 1.0/(double)(1<<30)/(double)(1<<30)/(double)(1<<6)) - GETFLOATSIGN(eph._clock_drift, 22, 1.0/(double)(1<<30)/(double)(1<<20)) - GETFLOATSIGN(eph._clock_bias, 24, 1.0/(double)(1<<30)/(double)(1<<3)) + eph._TOC.setBDS(i * 1000); + GETFLOATSIGN(eph._clock_driftrate, 11, + 1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6)) + GETFLOATSIGN(eph._clock_drift, 22, + 1.0 / (double )(1 << 30) / (double )(1 << 20)) + GETFLOATSIGN(eph._clock_bias, 24, + 1.0 / (double )(1 << 30) / (double )(1 << 3)) GETBITS(eph._AODC, 5) - GETFLOATSIGN(eph._Crs, 18, 1.0/(double)(1<<6)) + GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6)) GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Cuc, 18, 1.0/(double)(1<<30)/(double)(1<<1)) - GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3)) - GETFLOATSIGN(eph._Cus, 18, 1.0/(double)(1<<30)/(double)(1<<1)) - GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19)) + GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) + GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) + GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) + GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) GETBITS(i, 17) i <<= 3; eph._TOEsec = i; - eph._TOE.setBDS(i*1000); - GETFLOATSIGN(eph._Cic, 18, 1.0/(double)(1<<30)/(double)(1<<1)) + eph._TOE.setBDS(i * 1000); + GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Cis, 18, 1.0/(double)(1<<30)/(double)(1<<1)) + GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) - GETFLOATSIGN(eph._Crc, 18, 1.0/(double)(1<<6)) + GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6)) GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) @@ -1391,6 +1393,5 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size) { char *antenna, type[256]; int antnum = -1; @@ -1414,6 +1415,5 @@ // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) -{ +bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) { int type; uint64_t numbits = 0, bitfield = 0; @@ -1435,6 +1435,6 @@ GETBITSSIGN(z, 38) _antList.back().zz = z * 1e-4; - if(type == 1006) - { + if (type == 1006) + { double h; GETBITS(h, 16) @@ -1442,5 +1442,5 @@ _antList.back().height_f = true; } - _antList.back().message = type; + _antList.back().message = type; return true; @@ -1449,24 +1449,21 @@ // //////////////////////////////////////////////////////////////////////////// -t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector& errmsg) -{ +t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector& errmsg) { bool decoded = false; errmsg.clear(); - while(bufLen && _MessageSize < sizeof(_Message)) - { + while (bufLen && _MessageSize < sizeof(_Message)) { int l = sizeof(_Message) - _MessageSize; - if(l > bufLen) + if (l > bufLen) l = bufLen; - memcpy(_Message+_MessageSize, buffer, l); + memcpy(_Message + _MessageSize, buffer, l); _MessageSize += l; bufLen -= l; buffer += l; int id; - while((id = GetMessage())) - { + while ((id = GetMessage())) { /* reset station ID for file loading as it can change */ - if(_rawFile) + if (_rawFile) _staID = _rawFile->staID(); /* store the id into the list of loaded blocks */ @@ -1476,70 +1473,75 @@ * extracted data block. That does no harm, as it anyway skip everything * else. */ - if((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) - { + if ((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) { if (!_coDecoders.contains(_staID.toAscii())) _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID); RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()]; - if(coDecoder->Decode(reinterpret_cast(_Message), _BlockSize, - errmsg) == success) - { + if (coDecoder->Decode(reinterpret_cast(_Message), _BlockSize, + errmsg) == success) { decoded = true; } } - else if(id >= 1070 && id <= 1229) /* MSM */ - { - if(DecodeRTCM3MSM(_Message, _BlockSize)) + else if (id >= 1070 && id <= 1229) /* MSM */ { + if (DecodeRTCM3MSM(_Message, _BlockSize)) decoded = true; } - else - { - switch(id) - { - case 1001: case 1003: - emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!") - .arg(_staID).arg(id).toAscii(), true)); - break; /* no use decoding partial data ATM, remove break when data can be used */ - case 1002: case 1004: - if(DecodeRTCM3GPS(_Message, _BlockSize)) - decoded = true; - break; - case 1009: case 1011: - emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!") - .arg(_staID).arg(id).toAscii(), true)); - break; /* no use decoding partial data ATM, remove break when data can be used */ - case 1010: case 1012: - if(DecodeRTCM3GLONASS(_Message, _BlockSize)) - decoded = true; - break; - case 1019: - if(DecodeGPSEphemeris(_Message, _BlockSize)) - decoded = true; - break; - case 1020: - if(DecodeGLONASSEphemeris(_Message, _BlockSize)) - decoded = true; - break; - case 1043: - if(DecodeSBASEphemeris(_Message, _BlockSize)) - decoded = true; - break; - case 1044: - if(DecodeQZSSEphemeris(_Message, _BlockSize)) - decoded = true; - break; - case 1045: case 1046: - if(DecodeGalileoEphemeris(_Message, _BlockSize)) - decoded = true; - break; - case RTCM3ID_BDS: - if(DecodeBDSEphemeris(_Message, _BlockSize)) - decoded = true; - break; - case 1007: case 1008: case 1033: - DecodeAntenna(_Message, _BlockSize); - break; - case 1005: case 1006: - DecodeAntennaPosition(_Message, _BlockSize); - break; + else { + switch (id) { + case 1001: + case 1003: + emit(newMessage( + QString("%1: Block %2 contain partial data! Ignored!") + .arg(_staID).arg(id).toAscii(), true)); + break; /* no use decoding partial data ATM, remove break when data can be used */ + case 1002: + case 1004: + if (DecodeRTCM3GPS(_Message, _BlockSize)) + decoded = true; + break; + case 1009: + case 1011: + emit(newMessage( + QString("%1: Block %2 contain partial data! Ignored!") + .arg(_staID).arg(id).toAscii(), true)); + break; /* no use decoding partial data ATM, remove break when data can be used */ + case 1010: + case 1012: + if (DecodeRTCM3GLONASS(_Message, _BlockSize)) + decoded = true; + break; + case 1019: + if (DecodeGPSEphemeris(_Message, _BlockSize)) + decoded = true; + break; + case 1020: + if (DecodeGLONASSEphemeris(_Message, _BlockSize)) + decoded = true; + break; + case 1043: + if (DecodeSBASEphemeris(_Message, _BlockSize)) + decoded = true; + break; + case 1044: + if (DecodeQZSSEphemeris(_Message, _BlockSize)) + decoded = true; + break; + case 1045: + case 1046: + if (DecodeGalileoEphemeris(_Message, _BlockSize)) + decoded = true; + break; + case RTCM3ID_BDS: + if (DecodeBDSEphemeris(_Message, _BlockSize)) + decoded = true; + break; + case 1007: + case 1008: + case 1033: + DecodeAntenna(_Message, _BlockSize); + break; + case 1005: + case 1006: + DecodeAntennaPosition(_Message, _BlockSize); + break; } } @@ -1547,20 +1549,19 @@ } return decoded ? success : failure; -}; +} +; // //////////////////////////////////////////////////////////////////////////// -uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) -{ +uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) { uint32_t crc = 0; int i; - while(size--) - { + while (size--) { crc ^= (*buf++) << (16); - for(i = 0; i < 8; i++) - { + for (i = 0; i < 8; i++) + { crc <<= 1; - if(crc & 0x1000000) + if (crc & 0x1000000) crc ^= 0x01864cfb; } @@ -1571,23 +1572,19 @@ // //////////////////////////////////////////////////////////////////////////// -int RTCM3Decoder::GetMessage(void) -{ +int RTCM3Decoder::GetMessage(void) { unsigned char *m, *e; int i; - m = _Message+_SkipBytes; - e = _Message+_MessageSize; + m = _Message + _SkipBytes; + e = _Message + _MessageSize; _NeedBytes = _SkipBytes = 0; - while(e-m >= 3) - { - if(m[0] == 0xD3) - { - _BlockSize = ((m[1]&3)<<8)|m[2]; - if(e-m >= static_cast(_BlockSize+6)) - { - if(static_cast((m[3+_BlockSize]<<16)|(m[3+_BlockSize+1]<<8) - |(m[3+_BlockSize+2])) == CRC24(_BlockSize+3, m)) - { - _BlockSize +=6; + while (e - m >= 3) { + if (m[0] == 0xD3) { + _BlockSize = ((m[1] & 3) << 8) | m[2]; + if (e - m >= static_cast(_BlockSize + 6)) { + if (static_cast((m[3 + _BlockSize] << 16) + | (m[3 + _BlockSize + 1] << 8) + | (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) { + _BlockSize += 6; _SkipBytes = _BlockSize; break; @@ -1596,6 +1593,5 @@ ++m; } - else - { + else { _NeedBytes = _BlockSize; break; @@ -1605,14 +1601,14 @@ ++m; } - if(e-m < 3) + if (e - m < 3) _NeedBytes = 3; /* copy buffer to front */ i = m - _Message; - if(i && m < e) - memmove(_Message, m, static_cast(_MessageSize-i)); + if (i && m < e) + memmove(_Message, m, static_cast(_MessageSize - i)); _MessageSize -= i; - return !_NeedBytes ? ((_Message[3]<<4)|(_Message[4]>>4)) : 0; + return !_NeedBytes ? ((_Message[3] << 4) | (_Message[4] >> 4)) : 0; } @@ -1620,4 +1616,6 @@ ////////////////////////////////////////////////////////////////////////////// int RTCM3Decoder::corrGPSEpochTime() const { - return _coDecoders.size() > 0 ? _coDecoders.begin().value()->corrGPSEpochTime() : -1; -} + return + _coDecoders.size() > 0 ? + _coDecoders.begin().value()->corrGPSEpochTime() : -1; +}