Changeset 8158 in ntrip
- Timestamp:
- Oct 23, 2017, 10:06:07 AM (6 years ago)
- File:
-
- 1 edited
-
trunk/BNC/src/RTCM3/RTCM3Decoder.cpp (modified) (47 diffs)
Legend:
- Unmodified
- Added
- Removed
-
trunk/BNC/src/RTCM3/RTCM3Decoder.cpp
r7874 r8158 64 64 //////////////////////////////////////////////////////////////////////////// 65 65 RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) : 66 67 68 _staID 66 GPSDecoder() { 67 68 _staID = staID; 69 69 _rawFile = rawFile; 70 70 71 connect(this, SIGNAL(newGPSEph(t_ephGPS)), BNC_CORE, SLOT(slotNewGPSEph(t_ephGPS))); 72 connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE, SLOT(slotNewGlonassEph(t_ephGlo))); 73 connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE, SLOT(slotNewGalileoEph(t_ephGal))); 74 connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE, SLOT(slotNewSBASEph(t_ephSBAS))); 75 connect(this, SIGNAL(newBDSEph(t_ephBDS)), BNC_CORE, SLOT(slotNewBDSEph(t_ephBDS))); 71 connect(this, SIGNAL(newGPSEph(t_ephGPS)), BNC_CORE, 72 SLOT(slotNewGPSEph(t_ephGPS))); 73 connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE, 74 SLOT(slotNewGlonassEph(t_ephGlo))); 75 connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE, 76 SLOT(slotNewGalileoEph(t_ephGal))); 77 connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE, 78 SLOT(slotNewSBASEph(t_ephSBAS))); 79 connect(this, SIGNAL(newBDSEph(t_ephBDS)), BNC_CORE, 80 SLOT(slotNewBDSEph(t_ephBDS))); 76 81 77 82 _MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0; … … 82 87 RTCM3Decoder::~RTCM3Decoder() { 83 88 QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders); 84 while (it.hasNext())89 while (it.hasNext()) 85 90 { 86 91 it.next(); … … 91 96 // 92 97 //////////////////////////////////////////////////////////////////////////// 93 bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size) 94 { 98 bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size) { 95 99 bool decoded = false; 96 100 bncTime CurrentObsTime; … … 102 106 103 107 GETBITS(type, 12) 104 SKIPBITS(12) /* id */105 GETBITS(i,30)106 108 SKIPBITS(12) 109 /* id */ 110 GETBITS(i, 30) 107 111 CurrentObsTime.set(i); 108 if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime) 109 {112 113 if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) { 110 114 decoded = true; 111 115 _obsList.append(_CurrentObsList); … … 115 119 _CurrentTime = CurrentObsTime; 116 120 117 GETBITS(syncf,1) /* sync */ 118 GETBITS(numsats,5) 119 SKIPBITS(4) /* smind, smint */ 120 121 while(numsats--) 122 { 123 int sv, code, l1range, amb=0; 121 GETBITS(syncf, 1) 122 /* sync */ 123 GETBITS(numsats, 5) 124 SKIPBITS(4) 125 /* smind, smint */ 126 127 while (numsats--) { 128 int sv, code, l1range, amb = 0; 124 129 t_satObs CurrentObs; 125 130 CurrentObs._time = CurrentObsTime; 126 131 127 132 GETBITS(sv, 6) 128 if (sv < 40)133 if (sv < 40) 129 134 CurrentObs._prn.set('G', sv); 130 135 else 131 CurrentObs._prn.set('S', sv -20);136 CurrentObs._prn.set('S', sv - 20); 132 137 133 138 t_frqObs *frqObs = new t_frqObs; 134 139 /* L1 */ 135 140 GETBITS(code, 1); 136 (code) ? frqObs->_rnxType2ch.assign("1W") : frqObs->_rnxType2ch.assign("1C"); 141 (code) ? 142 frqObs->_rnxType2ch.assign("1W") : frqObs->_rnxType2ch.assign("1C"); 137 143 GETBITS(l1range, 24); 138 144 GETBITSSIGN(i, 20); 139 if((i&((1<<20)-1)) != 0x80000) 140 { 141 frqObs->_code = l1range*0.02; 142 frqObs->_phase = (l1range*0.02+i*0.0005)/GPS_WAVELENGTH_L1; 145 if ((i & ((1 << 20) - 1)) != 0x80000) { 146 frqObs->_code = l1range * 0.02; 147 frqObs->_phase = (l1range * 0.02 + i * 0.0005) / GPS_WAVELENGTH_L1; 143 148 frqObs->_codeValid = frqObs->_phaseValid = true; 144 149 } 145 150 GETBITS(i, 7); 146 151 frqObs->_slipCounter = i; 147 if(type == 1002 || type == 1004) 148 { 149 GETBITS(amb,8); 150 if(amb) 151 { 152 frqObs->_code += amb*299792.458; 153 frqObs->_phase += (amb*299792.458)/GPS_WAVELENGTH_L1; 152 if (type == 1002 || type == 1004) { 153 GETBITS(amb, 8); 154 if (amb) { 155 frqObs->_code += amb * 299792.458; 156 frqObs->_phase += (amb * 299792.458) / GPS_WAVELENGTH_L1; 154 157 } 155 158 GETBITS(i, 8); 156 if(i) 157 { 158 frqObs->_snr = i*0.25; 159 if (i) { 160 frqObs->_snr = i * 0.25; 159 161 frqObs->_snrValid = true; 160 162 } 161 163 } 162 164 CurrentObs._obs.push_back(frqObs); 163 if(type == 1003 || type == 1004) 164 { 165 if (type == 1003 || type == 1004) { 165 166 frqObs = new t_frqObs; 166 167 /* L2 */ 167 GETBITS(code,2); 168 switch(code) 169 { 170 case 3: frqObs->_rnxType2ch.assign("2W"); /* or "2Y"? */ break; 171 case 2: frqObs->_rnxType2ch.assign("2W"); break; 172 case 1: frqObs->_rnxType2ch.assign("2P"); break; 173 case 0: frqObs->_rnxType2ch.assign("2X"); /* or "2S" or "2L"? */ break; 174 } 175 GETBITSSIGN(i,14); 176 if((i&((1<<14)-1)) != 0x2000) 177 { 178 frqObs->_code = l1range*0.02+i*0.02+amb*299792.458; 168 GETBITS(code, 2); 169 switch (code) { 170 case 3: 171 frqObs->_rnxType2ch.assign("2W"); /* or "2Y"? */ 172 break; 173 case 2: 174 frqObs->_rnxType2ch.assign("2W"); 175 break; 176 case 1: 177 frqObs->_rnxType2ch.assign("2P"); 178 break; 179 case 0: 180 frqObs->_rnxType2ch.assign("2X"); /* or "2S" or "2L"? */ 181 break; 182 } 183 GETBITSSIGN(i, 14); 184 if ((i & ((1 << 14) - 1)) != 0x2000) { 185 frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 299792.458; 179 186 frqObs->_codeValid = true; 180 187 } 181 GETBITSSIGN(i, 20);182 if ((i&((1<<20)-1)) != 0x80000)183 {184 frqObs->_phase = (l1range*0.02+i*0.0005+amb*299792.458)/GPS_WAVELENGTH_L2;188 GETBITSSIGN(i, 20); 189 if ((i & ((1 << 20) - 1)) != 0x80000) { 190 frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 299792.458) 191 / GPS_WAVELENGTH_L2; 185 192 frqObs->_phaseValid = true; 186 193 } 187 GETBITS(i, 7);194 GETBITS(i, 7); 188 195 frqObs->_slipCounter = i; 189 if(type == 1004) 190 { 196 if (type == 1004) { 191 197 GETBITS(i, 8); 192 if(i) 193 { 194 frqObs->_snr = i*0.25; 198 if (i) { 199 frqObs->_snr = i * 0.25; 195 200 frqObs->_snrValid = true; 196 201 } … … 201 206 } 202 207 203 if(!syncf) 204 { 208 if (!syncf) { 205 209 decoded = true; 206 210 _obsList.append(_CurrentObsList); … … 230 234 231 235 /** MSM signal types for GPS and SBAS */ 232 static struct CodeData gps[RTCM3_MSM_NUMSIG] = 233 { 234 {0.0,0}, 235 {GPS_WAVELENGTH_L1,"1C"}, 236 {GPS_WAVELENGTH_L1,"1P"}, 237 {GPS_WAVELENGTH_L1,"1W"}, 238 {0.0,0}/*{GPS_WAVELENGTH_L1,"1Y"}*/, 239 {0.0,0}, 240 {0.0,0}, 241 {GPS_WAVELENGTH_L2,"2C"}, 242 {GPS_WAVELENGTH_L2,"2P"}, 243 {GPS_WAVELENGTH_L2,"2W"}, 244 {0.0,0}/*{GPS_WAVELENGTH_L2,"2Y"}*/, 245 {0.0,0}, 246 {0.0,0}, 247 {0.0,0}, 248 {GPS_WAVELENGTH_L2,"2S"}, 249 {GPS_WAVELENGTH_L2,"2L"}, 250 {GPS_WAVELENGTH_L2,"2X"}, 251 {0.0,0}, 252 {0.0,0}, 253 {0.0,0}, 254 {0.0,0}, 255 {GPS_WAVELENGTH_L5,"5I"}, 256 {GPS_WAVELENGTH_L5,"5Q"}, 257 {GPS_WAVELENGTH_L5,"5X"}, 258 {0.0,0}, 259 {0.0,0}, 260 {0.0,0}, 261 {0.0,0}, 262 {0.0,0}, 263 {GPS_WAVELENGTH_L1,"1S"}, 264 {GPS_WAVELENGTH_L1,"1L"}, 265 {GPS_WAVELENGTH_L1,"1X"} 266 }; 236 static struct CodeData gps[RTCM3_MSM_NUMSIG] = { 237 {0.0, 0}, 238 {GPS_WAVELENGTH_L1, "1C"}, 239 {GPS_WAVELENGTH_L1, "1P"}, 240 {GPS_WAVELENGTH_L1, "1W"}, 241 {0.0, 0}/*{GPS_WAVELENGTH_L1,"1Y"}*/, 242 {0.0, 0}, 243 {0.0, 0}, 244 {GPS_WAVELENGTH_L2, "2C"}, 245 {GPS_WAVELENGTH_L2, "2P"}, 246 {GPS_WAVELENGTH_L2, "2W"}, 247 {0.0, 0}/*{GPS_WAVELENGTH_L2,"2Y"}*/, 248 {0.0, 0}, 249 {0.0, 0}, 250 {0.0, 0}, 251 {GPS_WAVELENGTH_L2, "2S"}, 252 {GPS_WAVELENGTH_L2, "2L"}, 253 {GPS_WAVELENGTH_L2, "2X"}, 254 {0.0, 0}, 255 {0.0, 0}, 256 {0.0, 0}, 257 {0.0, 0}, 258 {GPS_WAVELENGTH_L5, "5I"}, 259 {GPS_WAVELENGTH_L5, "5Q"}, 260 {GPS_WAVELENGTH_L5, "5X"}, 261 {0.0, 0}, 262 {0.0, 0}, 263 {0.0, 0}, 264 {0.0, 0}, 265 {0.0, 0}, 266 {GPS_WAVELENGTH_L1, "1S"}, 267 {GPS_WAVELENGTH_L1, "1L"}, 268 {GPS_WAVELENGTH_L1, "1X"} 269 }; 267 270 268 271 /** … … 271 274 * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later! 272 275 */ 273 static struct CodeData glo[RTCM3_MSM_NUMSIG] = 274 { 275 {0.0,0}, 276 {0.0,"1C"}, 277 {0.0,"1P"}, 278 {0.0,0}, 279 {0.0,0}, 280 {0.0,0}, 281 {0.0,0}, 282 {1.0,"2C"}, 283 {1.0,"2P"}, 284 {0.0,0}, 285 {0.0,0}, 286 {0.0,0}, 287 {0.0,0}, 288 {0.0,0}, 289 {0.0,0}, 290 {0.0,0}, 291 {0.0,0}, 292 {0.0,0}, 293 {0.0,0}, 294 {0.0,0}, 295 {0.0,0}, 296 {0.0,0}, 297 {0.0,0}, 298 {0.0,0}, 299 {0.0,0}, 300 {0.0,0}, 301 {0.0,0}, 302 {0.0,0}, 303 {0.0,0}, 304 {0.0,0}, 305 {0.0,0}, 306 {0.0,0} 307 }; 276 static struct CodeData glo[RTCM3_MSM_NUMSIG] = { 277 {0.0, 0}, 278 {0.0, "1C"}, 279 {0.0, "1P"}, 280 {0.0, 0}, 281 {0.0, 0}, 282 {0.0, 0}, 283 {0.0, 0}, 284 {1.0, "2C"}, 285 {1.0, "2P"}, 286 {0.0, 0}, 287 {0.0, 0}, 288 {0.0, 0}, 289 {0.0, 0}, 290 {0.0, 0}, 291 {0.0, 0}, 292 {0.0, 0}, 293 {0.0, 0}, 294 {0.0, 0}, 295 {0.0, 0}, 296 {0.0, 0}, 297 {0.0, 0}, 298 {0.0, 0}, 299 {0.0, 0}, 300 {0.0, 0}, 301 {0.0, 0}, 302 {0.0, 0}, 303 {0.0, 0}, 304 {0.0, 0}, 305 {0.0, 0}, 306 {0.0, 0}, 307 {0.0, 0}, 308 {0.0, 0} 309 }; 308 310 309 311 /** MSM signal types for Galileo */ 310 static struct CodeData gal[RTCM3_MSM_NUMSIG] = 311 { 312 {0.0,0}, 313 {GAL_WAVELENGTH_E1,"1C"}, 314 {GAL_WAVELENGTH_E1,"1A"}, 315 {GAL_WAVELENGTH_E1,"1B"}, 316 {GAL_WAVELENGTH_E1,"1X"}, 317 {GAL_WAVELENGTH_E1,"1Z"}, 318 {0.0,0}, 319 {GAL_WAVELENGTH_E6,"6C"}, 320 {GAL_WAVELENGTH_E6,"6A"}, 321 {GAL_WAVELENGTH_E6,"6B"}, 322 {GAL_WAVELENGTH_E6,"6X"}, 323 {GAL_WAVELENGTH_E6,"6Z"}, 324 {0.0,0}, 325 {GAL_WAVELENGTH_E5B,"7I"}, 326 {GAL_WAVELENGTH_E5B,"7Q"}, 327 {GAL_WAVELENGTH_E5B,"7X"}, 328 {0.0,0}, 329 {GAL_WAVELENGTH_E5AB,"8I"}, 330 {GAL_WAVELENGTH_E5AB,"8Q"}, 331 {GAL_WAVELENGTH_E5AB,"8X"}, 332 {0.0,0}, 333 {GAL_WAVELENGTH_E5A,"5I"}, 334 {GAL_WAVELENGTH_E5A,"5Q"}, 335 {GAL_WAVELENGTH_E5A,"5X"}, 336 {0.0,0}, 337 {0.0,0}, 338 {0.0,0}, 339 {0.0,0}, 340 {0.0,0}, 341 {0.0,0}, 342 {0.0,0}, 343 {0.0,0}, 344 }; 312 static struct CodeData gal[RTCM3_MSM_NUMSIG] = { 313 {0.0, 0}, 314 {GAL_WAVELENGTH_E1, "1C"}, 315 {GAL_WAVELENGTH_E1, "1A"}, 316 {GAL_WAVELENGTH_E1, "1B"}, 317 {GAL_WAVELENGTH_E1, "1X"}, 318 {GAL_WAVELENGTH_E1, "1Z"}, 319 {0.0, 0}, 320 {GAL_WAVELENGTH_E6, "6C"}, 321 {GAL_WAVELENGTH_E6, "6A"}, 322 {GAL_WAVELENGTH_E6, "6B"}, 323 {GAL_WAVELENGTH_E6, "6X"}, 324 {GAL_WAVELENGTH_E6, "6Z"}, 325 {0.0, 0}, 326 {GAL_WAVELENGTH_E5B, "7I"}, 327 {GAL_WAVELENGTH_E5B, "7Q"}, 328 {GAL_WAVELENGTH_E5B, "7X"}, 329 {0.0, 0}, 330 {GAL_WAVELENGTH_E5AB, "8I"}, 331 {GAL_WAVELENGTH_E5AB, "8Q"}, 332 {GAL_WAVELENGTH_E5AB, "8X"}, 333 {0.0, 0}, 334 {GAL_WAVELENGTH_E5A, "5I"}, 335 {GAL_WAVELENGTH_E5A, "5Q"}, 336 {GAL_WAVELENGTH_E5A, "5X"}, 337 {0.0, 0}, 338 {0.0, 0}, 339 {0.0, 0}, 340 {0.0, 0}, 341 {0.0, 0}, 342 {0.0, 0}, 343 {0.0, 0}, 344 {0.0, 0}, 345 }; 345 346 346 347 /** MSM signal types for QZSS */ 347 static struct CodeData qzss[RTCM3_MSM_NUMSIG] = 348 { 349 {0.0,0}, 350 {GPS_WAVELENGTH_L1,"1C"}, 351 {0.0,0}, 352 {0.0,0}, 353 {0.0,0}, 354 {GPS_WAVELENGTH_L1,"1Z"}, 355 {0.0,0}, 356 {0.0,0}, 357 {QZSS_WAVELENGTH_LEX,"6S"}, 358 {QZSS_WAVELENGTH_LEX,"6L"}, 359 {QZSS_WAVELENGTH_LEX,"6X"}, 360 {0.0,0}, 361 {0.0,0}, 362 {0.0,0}, 363 {GPS_WAVELENGTH_L2,"2S"}, 364 {GPS_WAVELENGTH_L2,"2L"}, 365 {GPS_WAVELENGTH_L2,"2X"}, 366 {0.0,0}, 367 {0.0,0}, 368 {0.0,0}, 369 {0.0,0}, 370 {GPS_WAVELENGTH_L5,"5I"}, 371 {GPS_WAVELENGTH_L5,"5Q"}, 372 {GPS_WAVELENGTH_L5,"5X"}, 373 {0.0,0}, 374 {0.0,0}, 375 {0.0,0}, 376 {0.0,0}, 377 {0.0,0}, 378 {GPS_WAVELENGTH_L1,"1D"}, 379 {GPS_WAVELENGTH_L1,"1P"}, 380 {GPS_WAVELENGTH_L1,"1X"} 381 }; 348 static struct CodeData qzss[RTCM3_MSM_NUMSIG] = { 349 {0.0, 0}, 350 {GPS_WAVELENGTH_L1, "1C"}, 351 {0.0, 0}, 352 {0.0, 0}, 353 {0.0, 0}, 354 {GPS_WAVELENGTH_L1, "1Z"}, 355 {0.0, 0}, 356 {0.0, 0}, 357 {QZSS_WAVELENGTH_LEX, "6S"}, 358 {QZSS_WAVELENGTH_LEX, "6L"}, 359 {QZSS_WAVELENGTH_LEX, "6X"}, 360 {0.0, 0}, 361 {0.0, 0}, 362 {0.0, 0}, 363 {GPS_WAVELENGTH_L2, "2S"}, 364 {GPS_WAVELENGTH_L2, "2L"}, 365 {GPS_WAVELENGTH_L2, "2X"}, 366 {0.0, 0}, 367 {0.0, 0}, 368 {0.0, 0}, 369 {0.0, 0}, 370 {GPS_WAVELENGTH_L5, "5I"}, 371 {GPS_WAVELENGTH_L5, "5Q"}, 372 {GPS_WAVELENGTH_L5, "5X"}, 373 {0.0, 0}, 374 {0.0, 0}, 375 {0.0, 0}, 376 {0.0, 0}, 377 {0.0, 0}, 378 {GPS_WAVELENGTH_L1, "1D"}, 379 {GPS_WAVELENGTH_L1, "1P"}, 380 {GPS_WAVELENGTH_L1, "1X"} 381 }; 382 382 383 383 /** MSM signal types for Beidou/BDS */ 384 static struct CodeData bds[RTCM3_MSM_NUMSIG] = 385 { 386 {0.0,0}, 387 {BDS_WAVELENGTH_B1,"2I"}, 388 {0.0,0}, 389 {0.0,0}, 390 {0.0,0}, 391 {0.0,0}, 392 {0.0,0}, 393 {BDS_WAVELENGTH_B3,"6I"}, 394 {0.0,0}, 395 {0.0,0}, 396 {0.0,0}, 397 {0.0,0}, 398 {0.0,0}, 399 {BDS_WAVELENGTH_B2,"7I"}, 400 {0.0,0}, 401 {0.0,0}, 402 {0.0,0}, 403 {0.0,0}, 404 {0.0,0}, 405 {0.0,0}, 406 {0.0,0}, 407 {0.0,0}, 408 {0.0,0}, 409 {0.0,0}, 410 {0.0,0}, 411 {0.0,0}, 412 {0.0,0}, 413 {0.0,0}, 414 {0.0,0}, 415 {0.0,0}, 416 {0.0,0}, 417 {0.0,0}, 418 }; 384 static struct CodeData bds[RTCM3_MSM_NUMSIG] = { 385 {0.0, 0}, 386 {BDS_WAVELENGTH_B1, "2I"}, 387 {0.0, 0}, 388 {0.0, 0}, 389 {0.0, 0}, 390 {0.0, 0}, 391 {0.0, 0}, 392 {BDS_WAVELENGTH_B3, "6I"}, 393 {0.0, 0}, 394 {0.0, 0}, 395 {0.0, 0}, 396 {0.0, 0}, 397 {0.0, 0}, 398 {BDS_WAVELENGTH_B2, "7I"}, 399 {0.0, 0}, 400 {0.0, 0}, 401 {0.0, 0}, 402 {0.0, 0}, 403 {0.0, 0}, 404 {0.0, 0}, 405 {0.0, 0}, 406 {0.0, 0}, 407 {0.0, 0}, 408 {0.0, 0}, 409 {0.0, 0}, 410 {0.0, 0}, 411 {0.0, 0}, 412 {0.0, 0}, 413 {0.0, 0}, 414 {0.0, 0}, 415 {0.0, 0}, 416 {0.0, 0}, 417 }; 419 418 420 419 #define UINT64(c) c ## ULL … … 423 422 //////////////////////////////////////////////////////////////////////////// 424 423 bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size) 425 {424 { 426 425 bool decoded = false; 427 426 int type, syncf, i; … … 432 431 433 432 GETBITS(type, 12) 434 SKIPBITS(12) /* id */ 433 SKIPBITS(12) 434 /* id */ 435 435 char sys; 436 if (type >= 1121)436 if (type >= 1121) 437 437 sys = 'C'; 438 else if (type >= 1111)438 else if (type >= 1111) 439 439 sys = 'J'; 440 else if (type >= 1101)440 else if (type >= 1101) 441 441 sys = 'S'; 442 else if (type >= 1091)442 else if (type >= 1091) 443 443 sys = 'E'; 444 else if (type >= 1081)444 else if (type >= 1081) 445 445 sys = 'R'; 446 446 else … … 448 448 449 449 bncTime CurrentObsTime; 450 if(sys == 'C') /* BDS */ 451 { 452 GETBITS(i,30) 450 if (sys == 'C') /* BDS */ { 451 GETBITS(i, 30) 453 452 CurrentObsTime.setBDS(i); 454 453 } 455 else if(sys == 'R') /* GLONASS */ 456 { 454 else if (sys == 'R') /* GLONASS */ { 457 455 SKIPBITS(3) 458 GETBITS(i,27) /* tk */ 456 GETBITS(i, 27) 457 /* tk */ 459 458 CurrentObsTime.setTk(i); 460 459 } 461 else /* GPS style date */ 462 { 463 GETBITS(i,30) 460 else /* GPS style date */ { 461 GETBITS(i, 30) 464 462 CurrentObsTime.set(i); 465 463 } 466 if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime) 467 { 464 if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) { 468 465 decoded = true; 469 466 _obsList = _CurrentObsList; … … 479 476 * the full cycles and can't be used later we skip interpretation here already. 480 477 */ 481 if(type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7) 482 { 478 if (type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7) { 483 479 int sigmask, numsat = 0, numsig = 0; 484 480 uint64_t satmask, cellmask, ui; 485 481 double rrmod[RTCM3_MSM_NUMSAT]; 486 482 int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT], 487 extsat[RTCM3_MSM_NUMSAT];483 extsat[RTCM3_MSM_NUMSAT]; 488 484 int ll[RTCM3_MSM_NUMCELLS]/*, hc[RTCM3_MSM_NUMCELLS]*/; 489 485 double cnr[RTCM3_MSM_NUMCELLS]; 490 486 double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS], 491 dop[RTCM3_MSM_NUMCELLS];492 493 SKIPBITS(3 +7+2+2+1+3)487 dop[RTCM3_MSM_NUMCELLS]; 488 489 SKIPBITS(3 + 7 + 2 + 2 + 1 + 3) 494 490 GETBITS64(satmask, RTCM3_MSM_NUMSAT) 495 491 496 492 /* http://gurmeetsingh.wordpress.com/2008/08/05/fast-bit-counting-routines/ */ 497 for (ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */)493 for (ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */) 498 494 ++numsat; 499 495 GETBITS(sigmask, RTCM3_MSM_NUMSIG) 500 for (i = sigmask; i; i &= (i - 1) /* remove rightmost bit */)496 for (i = sigmask; i; i &= (i - 1) /* remove rightmost bit */) 501 497 ++numsig; 502 for (i = 0; i < RTCM3_MSM_NUMSAT; ++i)498 for (i = 0; i < RTCM3_MSM_NUMSAT; ++i) 503 499 extsat[i] = 15; 504 500 505 i = numsat*numsig; 506 GETBITS64(cellmask, (unsigned)i) 507 508 switch(type % 10) 509 { 510 case 1: case 2: case 3: 511 /* partial data, already skipped above, but implemented for future expansion ! */ 512 for(int j = numsat; j--;) 513 GETFLOAT(rrmod[j], 10, 1.0/1024.0) 514 break; 515 case 4: case 6: 516 for(int j = numsat; j--;) 517 GETBITS(rrint[j], 8) 518 for(int j = numsat; j--;) 519 GETFLOAT(rrmod[j], 10, 1.0/1024.0) 520 break; 521 case 5: case 7: 522 for(int j = numsat; j--;) 523 GETBITS(rrint[j], 8) 524 for(int j = numsat; j--;) 525 GETBITS(extsat[j], 4) 526 for(int j = numsat; j--;) 527 GETFLOAT(rrmod[j], 10, 1.0/1024.0) 528 for(int j = numsat; j--;) 529 GETBITSSIGN(rdop[j], 14) 530 break; 501 i = numsat * numsig; 502 GETBITS64(cellmask, (unsigned )i) 503 504 switch (type % 10) { 505 case 1: 506 case 2: 507 case 3: 508 /* partial data, already skipped above, but implemented for future expansion ! */ 509 for (int j = numsat; j--;) 510 GETFLOAT(rrmod[j], 10, 1.0 / 1024.0) 511 break; 512 case 4: 513 case 6: 514 for (int j = numsat; j--;) 515 GETBITS(rrint[j], 8) 516 for (int j = numsat; j--;) 517 GETFLOAT(rrmod[j], 10, 1.0 / 1024.0) 518 break; 519 case 5: 520 case 7: 521 for (int j = numsat; j--;) 522 GETBITS(rrint[j], 8) 523 for (int j = numsat; j--;) 524 GETBITS(extsat[j], 4) 525 for (int j = numsat; j--;) 526 GETFLOAT(rrmod[j], 10, 1.0 / 1024.0) 527 for (int j = numsat; j--;) 528 GETBITSSIGN(rdop[j], 14) 529 break; 531 530 } 532 531 533 int numcells = numsat *numsig;532 int numcells = numsat * numsig; 534 533 /** Drop anything which exceeds our cell limit. Increase limit definition 535 534 * when that happens. */ 536 if(numcells <= RTCM3_MSM_NUMCELLS) 537 { 538 switch(type % 10) 539 { 540 case 1: 541 for(int count = numcells; count--;) 542 if(cellmask & (UINT64(1)<<count)) 543 GETFLOATSIGN(psr[count], 15, 1.0/(1<<24)) 544 break; 545 case 2: 546 for(int count = numcells; count--;) 547 if(cellmask & (UINT64(1)<<count)) 548 GETFLOATSIGN(cp[count], 22, 1.0/(1<<29)) 549 for(int count = numcells; count--;) 550 if(cellmask & (UINT64(1)<<count)) 551 GETBITS(ll[count], 4) 552 for(int count = numcells; count--;) 553 if(cellmask & (UINT64(1)<<count)) 554 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 555 break; 556 case 3: 557 for(int count = numcells; count--;) 558 if(cellmask & (UINT64(1)<<count)) 559 GETFLOATSIGN(psr[count], 15, 1.0/(1<<24)) 560 for(int count = numcells; count--;) 561 if(cellmask & (UINT64(1)<<count)) 562 GETFLOATSIGN(cp[count], 22, 1.0/(1<<29)) 563 for(int count = numcells; count--;) 564 if(cellmask & (UINT64(1)<<count)) 565 GETBITS(ll[count], 4) 566 for(int count = numcells; count--;) 567 if(cellmask & (UINT64(1)<<count)) 568 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 569 break; 570 case 4: 571 for(int count = numcells; count--;) 572 if(cellmask & (UINT64(1)<<count)) 573 GETFLOATSIGN(psr[count], 15, 1.0/(1<<24)) 574 for(int count = numcells; count--;) 575 if(cellmask & (UINT64(1)<<count)) 576 GETFLOATSIGN(cp[count], 22, 1.0/(1<<29)) 577 for(int count = numcells; count--;) 578 if(cellmask & (UINT64(1)<<count)) 579 GETBITS(ll[count], 4) 580 for(int count = numcells; count--;) 581 if(cellmask & (UINT64(1)<<count)) 582 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 583 for(int count = numcells; count--;) 584 if(cellmask & (UINT64(1)<<count)) 585 GETBITS(cnr[count], 6) 586 break; 587 case 5: 588 for(int count = numcells; count--;) 589 if(cellmask & (UINT64(1)<<count)) 590 GETFLOATSIGN(psr[count], 15, 1.0/(1<<24)) 591 for(int count = numcells; count--;) 592 if(cellmask & (UINT64(1)<<count)) 593 GETFLOATSIGN(cp[count], 22, 1.0/(1<<29)) 594 for(int count = numcells; count--;) 595 if(cellmask & (UINT64(1)<<count)) 596 GETBITS(ll[count], 4) 597 for(int count = numcells; count--;) 598 if(cellmask & (UINT64(1)<<count)) 599 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 600 for(int count = numcells; count--;) 601 if(cellmask & (UINT64(1)<<count)) 602 GETFLOAT(cnr[count], 6, 1.0) 603 for(int count = numcells; count--;) 604 if(cellmask & (UINT64(1)<<count)) 605 GETFLOATSIGN(dop[count], 15, 0.0001) 606 break; 607 case 6: 608 for(int count = numcells; count--;) 609 if(cellmask & (UINT64(1)<<count)) 610 GETFLOATSIGN(psr[count], 20, 1.0/(1<<29)) 611 for(int count = numcells; count--;) 612 if(cellmask & (UINT64(1)<<count)) 613 GETFLOATSIGN(cp[count], 24, 1.0/(1U<<31)) 614 for(int count = numcells; count--;) 615 if(cellmask & (UINT64(1)<<count)) 616 GETBITS(ll[count], 10) 617 for(int count = numcells; count--;) 618 if(cellmask & (UINT64(1)<<count)) 619 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 620 for(int count = numcells; count--;) 621 if(cellmask & (UINT64(1)<<count)) 622 GETFLOAT(cnr[count], 10, 1.0/(1<<4)) 623 break; 624 case 7: 625 for(int count = numcells; count--;) 626 if(cellmask & (UINT64(1)<<count)) 627 GETFLOATSIGN(psr[count], 20, 1.0/(1<<29)) 628 for(int count = numcells; count--;) 629 if(cellmask & (UINT64(1)<<count)) 630 GETFLOATSIGN(cp[count], 24, 1.0/(1U<<31)) 631 for(int count = numcells; count--;) 632 if(cellmask & (UINT64(1)<<count)) 633 GETBITS(ll[count], 10) 634 for(int count = numcells; count--;) 635 if(cellmask & (UINT64(1)<<count)) 636 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 637 for(int count = numcells; count--;) 638 if(cellmask & (UINT64(1)<<count)) 639 GETFLOAT(cnr[count], 10, 1.0/(1<<4)) 640 for(int count = numcells; count--;) 641 if(cellmask & (UINT64(1)<<count)) 642 GETFLOATSIGN(dop[count], 15, 0.0001) 643 break; 535 if (numcells <= RTCM3_MSM_NUMCELLS) { 536 switch (type % 10) { 537 case 1: 538 for (int count = numcells; count--;) 539 if (cellmask & (UINT64(1) << count)) 540 GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24)) 541 break; 542 case 2: 543 for (int count = numcells; count--;) 544 if (cellmask & (UINT64(1) << count)) 545 GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29)) 546 for (int count = numcells; count--;) 547 if (cellmask & (UINT64(1) << count)) 548 GETBITS(ll[count], 4) 549 for (int count = numcells; count--;) 550 if (cellmask & (UINT64(1) << count)) 551 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 552 break; 553 case 3: 554 for (int count = numcells; count--;) 555 if (cellmask & (UINT64(1) << count)) 556 GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24)) 557 for (int count = numcells; count--;) 558 if (cellmask & (UINT64(1) << count)) 559 GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29)) 560 for (int count = numcells; count--;) 561 if (cellmask & (UINT64(1) << count)) 562 GETBITS(ll[count], 4) 563 for (int count = numcells; count--;) 564 if (cellmask & (UINT64(1) << count)) 565 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 566 break; 567 case 4: 568 for (int count = numcells; count--;) 569 if (cellmask & (UINT64(1) << count)) 570 GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24)) 571 for (int count = numcells; count--;) 572 if (cellmask & (UINT64(1) << count)) 573 GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29)) 574 for (int count = numcells; count--;) 575 if (cellmask & (UINT64(1) << count)) 576 GETBITS(ll[count], 4) 577 for (int count = numcells; count--;) 578 if (cellmask & (UINT64(1) << count)) 579 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 580 for (int count = numcells; count--;) 581 if (cellmask & (UINT64(1) << count)) 582 GETBITS(cnr[count], 6) 583 break; 584 case 5: 585 for (int count = numcells; count--;) 586 if (cellmask & (UINT64(1) << count)) 587 GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24)) 588 for (int count = numcells; count--;) 589 if (cellmask & (UINT64(1) << count)) 590 GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29)) 591 for (int count = numcells; count--;) 592 if (cellmask & (UINT64(1) << count)) 593 GETBITS(ll[count], 4) 594 for (int count = numcells; count--;) 595 if (cellmask & (UINT64(1) << count)) 596 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 597 for (int count = numcells; count--;) 598 if (cellmask & (UINT64(1) << count)) 599 GETFLOAT(cnr[count], 6, 1.0) 600 for (int count = numcells; count--;) 601 if (cellmask & (UINT64(1) << count)) 602 GETFLOATSIGN(dop[count], 15, 0.0001) 603 break; 604 case 6: 605 for (int count = numcells; count--;) 606 if (cellmask & (UINT64(1) << count)) 607 GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29)) 608 for (int count = numcells; count--;) 609 if (cellmask & (UINT64(1) << count)) 610 GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31)) 611 for (int count = numcells; count--;) 612 if (cellmask & (UINT64(1) << count)) 613 GETBITS(ll[count], 10) 614 for (int count = numcells; count--;) 615 if (cellmask & (UINT64(1) << count)) 616 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 617 for (int count = numcells; count--;) 618 if (cellmask & (UINT64(1) << count)) 619 GETFLOAT(cnr[count], 10, 1.0 / (1 << 4)) 620 break; 621 case 7: 622 for (int count = numcells; count--;) 623 if (cellmask & (UINT64(1) << count)) 624 GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29)) 625 for (int count = numcells; count--;) 626 if (cellmask & (UINT64(1) << count)) 627 GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31)) 628 for (int count = numcells; count--;) 629 if (cellmask & (UINT64(1) << count)) 630 GETBITS(ll[count], 10) 631 for (int count = numcells; count--;) 632 if (cellmask & (UINT64(1) << count)) 633 SKIPBITS(1)/*GETBITS(hc[count], 1)*/ 634 for (int count = numcells; count--;) 635 if (cellmask & (UINT64(1) << count)) 636 GETFLOAT(cnr[count], 10, 1.0 / (1 << 4)) 637 for (int count = numcells; count--;) 638 if (cellmask & (UINT64(1) << count)) 639 GETFLOATSIGN(dop[count], 15, 0.0001) 640 break; 644 641 } 645 642 i = RTCM3_MSM_NUMSAT; 646 643 int j = -1; 647 644 t_satObs CurrentObs; 648 for(int count = numcells; count--;) 649 { 650 while(j >= 0 && !(sigmask&(1<<--j))) 645 for (int count = numcells; count--;) { 646 while (j >= 0 && !(sigmask & (1 << --j))) 651 647 ; 652 if (j < 0)653 {654 while(!(satmask&(UINT64(1)<<(--i))))/* next satellite */648 if (j < 0) { 649 while (!(satmask & (UINT64(1) << (--i)))) 650 /* next satellite */ 655 651 ; 656 if (CurrentObs._obs.size() > 0)652 if (CurrentObs._obs.size() > 0) 657 653 _CurrentObsList.push_back(CurrentObs); 658 654 CurrentObs.clear(); 659 655 CurrentObs._time = CurrentObsTime; 660 if (sys == 'S')661 CurrentObs._prn.set(sys, 20 -1+RTCM3_MSM_NUMSAT-i);656 if (sys == 'S') 657 CurrentObs._prn.set(sys, 20 - 1 + RTCM3_MSM_NUMSAT - i); 662 658 else 663 CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT -i);659 CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT - i); 664 660 j = RTCM3_MSM_NUMSIG; 665 while (!(sigmask&(1<<--j)))661 while (!(sigmask & (1 << --j))) 666 662 ; 667 663 --numsat; 668 664 } 669 if(cellmask & (UINT64(1)<<count)) 670 { 671 struct CodeData cd = {0.0,0}; 672 switch(sys) 673 { 674 case 'J': 675 cd = qzss[RTCM3_MSM_NUMSIG-j-1]; 676 break; 677 case 'C': 678 cd = bds[RTCM3_MSM_NUMSIG-j-1]; 679 break; 680 case 'G': case 'S': 681 cd = gps[RTCM3_MSM_NUMSIG-j-1]; 682 break; 683 case 'R': 684 cd = glo[RTCM3_MSM_NUMSIG-j-1]; 685 { 686 int k = GLOFreq[RTCM3_MSM_NUMSAT-i-1]; 687 if(extsat[numsat] < 14) 665 if (cellmask & (UINT64(1) << count)) { 666 struct CodeData cd = {0.0, 0}; 667 switch (sys) { 668 case 'J': 669 cd = qzss[RTCM3_MSM_NUMSIG - j - 1]; 670 break; 671 case 'C': 672 cd = bds[RTCM3_MSM_NUMSIG - j - 1]; 673 break; 674 case 'G': 675 case 'S': 676 cd = gps[RTCM3_MSM_NUMSIG - j - 1]; 677 break; 678 case 'R': 679 cd = glo[RTCM3_MSM_NUMSIG - j - 1]; 688 680 { 689 k = GLOFreq[RTCM3_MSM_NUMSAT-i-1] = 100+extsat[numsat]-7; 681 int k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1]; 682 if (extsat[numsat] < 14) { 683 k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1] = 100 + extsat[numsat] 684 - 7; 685 } 686 if (k) 687 cd.wl = ( 688 cd.wl == 0.0 ? 689 GLO_WAVELENGTH_L1(k - 100) : 690 GLO_WAVELENGTH_L2(k - 100)); 691 else 692 cd.code = 0; 690 693 } 691 if(k) 692 cd.wl = (cd.wl == 0.0 ? GLO_WAVELENGTH_L1(k-100) : GLO_WAVELENGTH_L2(k-100)); 693 else 694 cd.code = 0; 695 } 696 break; 697 case 'E': 698 cd = gal[RTCM3_MSM_NUMSIG-j-1]; 699 break; 694 break; 695 case 'E': 696 cd = gal[RTCM3_MSM_NUMSIG - j - 1]; 697 break; 700 698 } 701 if(cd.code) 702 { 699 if (cd.code) { 703 700 t_frqObs *frqObs = new t_frqObs; 704 701 frqObs->_rnxType2ch.assign(cd.code); 705 702 706 switch(type % 10) 707 { 708 case 1: 709 if(psr[count] > -1.0/(1<<10)) 710 { 711 frqObs->_code = psr[count]*LIGHTSPEED/1000.0 712 +(rrmod[numsat])*LIGHTSPEED/1000.0; 713 frqObs->_codeValid = true; 714 } 715 break; 716 case 2: 717 if(cp[count] > -1.0/(1<<8)) 718 { 719 frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl 720 +(rrmod[numsat])*LIGHTSPEED/1000.0/cd.wl; 721 frqObs->_phaseValid = true; 722 frqObs->_slipCounter = ll[count]; 723 } 724 break; 725 case 3: 726 if(psr[count] > -1.0/(1<<10)) 727 { 728 frqObs->_code = psr[count]*LIGHTSPEED/1000.0 729 +(rrmod[numsat])*LIGHTSPEED/1000.0; 730 frqObs->_codeValid = true; 731 } 732 733 if(cp[count] > -1.0/(1<<8)) 734 { 735 frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl 736 +rrmod[numsat]*LIGHTSPEED/1000.0/cd.wl; 737 frqObs->_phaseValid = true; 738 frqObs->_slipCounter = ll[count]; 739 } 740 break; 741 case 4: 742 if(psr[count] > -1.0/(1<<10)) 743 { 744 frqObs->_code = psr[count]*LIGHTSPEED/1000.0 745 +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0; 746 frqObs->_codeValid = true; 747 } 748 749 if(cp[count] > -1.0/(1<<8)) 750 { 751 frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl 752 +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl; 753 frqObs->_phaseValid = true; 754 frqObs->_slipCounter = ll[count]; 755 } 756 757 frqObs->_snr = cnr[count]; 758 frqObs->_snrValid = true; 759 break; 760 case 5: 761 if(psr[count] > -1.0/(1<<10)) 762 { 763 frqObs->_code = psr[count]*LIGHTSPEED/1000.0 764 +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0; 765 frqObs->_codeValid = true; 766 } 767 768 if(cp[count] > -1.0/(1<<8)) 769 { 770 frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl 771 +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl; 772 frqObs->_phaseValid = true; 773 frqObs->_slipCounter = ll[count]; 774 } 775 776 frqObs->_snr = cnr[count]; 777 frqObs->_snrValid = true; 778 779 if(dop[count] > -1.6384) 780 { 781 frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl; 782 frqObs->_dopplerValid = true; 783 } 784 break; 785 case 6: 786 if(psr[count] > -1.0/(1<<10)) 787 { 788 frqObs->_code = psr[count]*LIGHTSPEED/1000.0 789 +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0; 790 frqObs->_codeValid = true; 791 } 792 793 if(cp[count] > -1.0/(1<<8)) 794 { 795 frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl 796 +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl; 797 frqObs->_phaseValid = true; 798 frqObs->_slipCounter = ll[count]; 799 } 800 801 frqObs->_snr = cnr[count]; 802 frqObs->_snrValid = true; 803 break; 804 case 7: 805 if(psr[count] > -1.0/(1<<10)) 806 { 807 frqObs->_code = psr[count]*LIGHTSPEED/1000.0 808 +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0; 809 frqObs->_codeValid = true; 810 } 811 812 if(cp[count] > -1.0/(1<<8)) 813 { 814 frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl 815 +(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl; 816 frqObs->_phaseValid = true; 817 frqObs->_slipCounter = ll[count]; 818 } 819 820 frqObs->_snr = cnr[count]; 821 frqObs->_snrValid = true; 822 823 if(dop[count] > -1.6384) 824 { 825 frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl; 826 frqObs->_dopplerValid = true; 827 } 828 break; 703 switch (type % 10) { 704 case 1: 705 if (psr[count] > -1.0 / (1 << 10)) { 706 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 707 + (rrmod[numsat]) * LIGHTSPEED / 1000.0; 708 frqObs->_codeValid = true; 709 } 710 break; 711 case 2: 712 if (cp[count] > -1.0 / (1 << 8)) { 713 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 714 + (rrmod[numsat]) * LIGHTSPEED / 1000.0 / cd.wl; 715 frqObs->_phaseValid = true; 716 frqObs->_slipCounter = ll[count]; 717 } 718 break; 719 case 3: 720 if (psr[count] > -1.0 / (1 << 10)) { 721 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 722 + (rrmod[numsat]) * LIGHTSPEED / 1000.0; 723 frqObs->_codeValid = true; 724 } 725 726 if (cp[count] > -1.0 / (1 << 8)) { 727 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 728 + rrmod[numsat] * LIGHTSPEED / 1000.0 / cd.wl; 729 frqObs->_phaseValid = true; 730 frqObs->_slipCounter = ll[count]; 731 } 732 break; 733 case 4: 734 qDebug() << "MSM4"; 735 if (psr[count] > -1.0 / (1 << 10)) { 736 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 737 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; 738 frqObs->_codeValid = true; 739 qDebug() << "frqObs->_codeValid"; 740 } 741 742 if (cp[count] > -1.0 / (1 << 8)) { 743 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 744 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 745 / cd.wl; 746 frqObs->_phaseValid = true; 747 qDebug() << "frqObs->_phaseValid"; 748 frqObs->_slipCounter = ll[count]; 749 } 750 751 frqObs->_snr = cnr[count]; 752 frqObs->_snrValid = true; 753 qDebug() << "frqObs->_snrValid"; 754 break; 755 case 5: 756 if (psr[count] > -1.0 / (1 << 10)) { 757 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 758 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; 759 frqObs->_codeValid = true; 760 } 761 762 if (cp[count] > -1.0 / (1 << 8)) { 763 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 764 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 765 / cd.wl; 766 frqObs->_phaseValid = true; 767 frqObs->_slipCounter = ll[count]; 768 } 769 770 frqObs->_snr = cnr[count]; 771 frqObs->_snrValid = true; 772 773 if (dop[count] > -1.6384) { 774 frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl; 775 frqObs->_dopplerValid = true; 776 } 777 break; 778 case 6: 779 if (psr[count] > -1.0 / (1 << 10)) { 780 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 781 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; 782 frqObs->_codeValid = true; 783 } 784 785 if (cp[count] > -1.0 / (1 << 8)) { 786 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 787 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 788 / cd.wl; 789 frqObs->_phaseValid = true; 790 frqObs->_slipCounter = ll[count]; 791 } 792 793 frqObs->_snr = cnr[count]; 794 frqObs->_snrValid = true; 795 break; 796 case 7: 797 if (psr[count] > -1.0 / (1 << 10)) { 798 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 799 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; 800 frqObs->_codeValid = true; 801 } 802 803 if (cp[count] > -1.0 / (1 << 8)) { 804 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 805 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 806 / cd.wl; 807 frqObs->_phaseValid = true; 808 frqObs->_slipCounter = ll[count]; 809 } 810 811 frqObs->_snr = cnr[count]; 812 frqObs->_snrValid = true; 813 814 if (dop[count] > -1.6384) { 815 frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl; 816 frqObs->_dopplerValid = true; 817 } 818 break; 829 819 } 830 820 CurrentObs._obs.push_back(frqObs); … … 832 822 } 833 823 } 834 if(CurrentObs._obs.size() > 0) 824 qDebug() << CurrentObs._time.datestr().c_str() << " " 825 << CurrentObs._time.timestr().c_str(); 826 qDebug() << "CurrentObs._obs.size(): " << CurrentObs._obs.size(); 827 if (CurrentObs._obs.size() > 0) { 835 828 _CurrentObsList.push_back(CurrentObs); 829 qDebug() << "_CurrentObsList.push_back(CurrentObs)"; 830 } 836 831 } 837 832 } 838 else if((type % 10) < 3) 839 { 833 else if ((type % 10) < 3) { 840 834 emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!") 841 .arg(_staID).arg(type).toAscii(), true)); 842 } 843 if(!syncf) 844 { 835 .arg(_staID).arg(type).toAscii(), true)); 836 } 837 if (!syncf) { 845 838 decoded = true; 846 839 _obsList = _CurrentObsList; … … 853 846 // 854 847 //////////////////////////////////////////////////////////////////////////// 855 bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) 856 { 848 bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) { 857 849 bool decoded = false; 858 850 bncTime CurrentObsTime; … … 864 856 865 857 GETBITS(type, 12) 866 SKIPBITS(12) /* id */ 867 GETBITS(i,27) /* tk */ 858 SKIPBITS(12) 859 /* id */ 860 GETBITS(i, 27) 861 /* tk */ 868 862 869 863 CurrentObsTime.setTk(i); 870 if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime) 871 { 864 if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) { 872 865 decoded = true; 873 866 _obsList.append(_CurrentObsList); … … 876 869 _CurrentTime = CurrentObsTime; 877 870 878 GETBITS(syncf,1) /* sync */ 879 GETBITS(numsats,5) 880 SKIPBITS(4) /* smind, smint */ 881 882 while(numsats--) 883 { 884 int sv, code, l1range, amb=0, freq; 871 GETBITS(syncf, 1) 872 /* sync */ 873 GETBITS(numsats, 5) 874 SKIPBITS(4) 875 /* smind, smint */ 876 877 while (numsats--) { 878 int sv, code, l1range, amb = 0, freq; 885 879 t_satObs CurrentObs; 886 880 CurrentObs._time = CurrentObsTime; … … 890 884 GETBITS(code, 1) 891 885 GETBITS(freq, 5) 892 GLOFreq[sv -1] = 100+freq-7; /* store frequency for other users (MSM) */886 GLOFreq[sv - 1] = 100 + freq - 7; /* store frequency for other users (MSM) */ 893 887 894 888 t_frqObs *frqObs = new t_frqObs; 895 889 /* L1 */ 896 (code) ? frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C"); 890 (code) ? 891 frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C"); 897 892 GETBITS(l1range, 25); 898 893 GETBITSSIGN(i, 20); 899 if ((i&((1<<20)-1)) != 0x80000)900 {901 frqObs->_ code = l1range*0.02;902 frqObs->_phase = (l1range*0.02+i*0.0005)/GLO_WAVELENGTH_L1(freq-7);894 if ((i & ((1 << 20) - 1)) != 0x80000) { 895 frqObs->_code = l1range * 0.02; 896 frqObs->_phase = (l1range * 0.02 + i * 0.0005) 897 / GLO_WAVELENGTH_L1(freq - 7); 903 898 frqObs->_codeValid = frqObs->_phaseValid = true; 904 899 } 905 900 GETBITS(i, 7); 906 901 frqObs->_slipCounter = i; 907 if(type == 1010 || type == 1012) 908 { 909 GETBITS(amb,7); 910 if(amb) 911 { 912 frqObs->_code += amb*599584.916; 913 frqObs->_phase += (amb*599584.916)/GLO_WAVELENGTH_L1(freq-7); 902 if (type == 1010 || type == 1012) { 903 GETBITS(amb, 7); 904 if (amb) { 905 frqObs->_code += amb * 599584.916; 906 frqObs->_phase += (amb * 599584.916) / GLO_WAVELENGTH_L1(freq - 7); 914 907 } 915 908 GETBITS(i, 8); 916 if(i) 917 { 918 frqObs->_snr = i*0.25; 909 if (i) { 910 frqObs->_snr = i * 0.25; 919 911 frqObs->_snrValid = true; 920 912 } 921 913 } 922 914 CurrentObs._obs.push_back(frqObs); 923 if(type == 1011 || type == 1012) 924 { 915 if (type == 1011 || type == 1012) { 925 916 frqObs = new t_frqObs; 926 917 /* L2 */ 927 GETBITS(code,2); 928 switch(code) 929 { 930 case 3: frqObs->_rnxType2ch.assign("2P"); break; 931 case 2: frqObs->_rnxType2ch.assign("2P"); break; 932 case 1: frqObs->_rnxType2ch.assign("2P"); break; 933 case 0: frqObs->_rnxType2ch.assign("2C"); break; 934 } 935 GETBITSSIGN(i,14); 936 if((i&((1<<14)-1)) != 0x2000) 937 { 938 frqObs->_code = l1range*0.02+i*0.02+amb*599584.916; 918 GETBITS(code, 2); 919 switch (code) { 920 case 3: 921 frqObs->_rnxType2ch.assign("2P"); 922 break; 923 case 2: 924 frqObs->_rnxType2ch.assign("2P"); 925 break; 926 case 1: 927 frqObs->_rnxType2ch.assign("2P"); 928 break; 929 case 0: 930 frqObs->_rnxType2ch.assign("2C"); 931 break; 932 } 933 GETBITSSIGN(i, 14); 934 if ((i & ((1 << 14) - 1)) != 0x2000) { 935 frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 599584.916; 939 936 frqObs->_codeValid = true; 940 937 } 941 GETBITSSIGN(i, 20);942 if ((i&((1<<20)-1)) != 0x80000)943 {944 frqObs->_phase = (l1range*0.02+i*0.0005+amb*599584.916)/GLO_WAVELENGTH_L2(freq-7);938 GETBITSSIGN(i, 20); 939 if ((i & ((1 << 20) - 1)) != 0x80000) { 940 frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 599584.916) 941 / GLO_WAVELENGTH_L2(freq - 7); 945 942 frqObs->_phaseValid = true; 946 943 } 947 GETBITS(i, 7);944 GETBITS(i, 7); 948 945 frqObs->_slipCounter = i; 949 if(type == 1012) 950 { 946 if (type == 1012) { 951 947 GETBITS(i, 8); 952 if(i) 953 { 954 frqObs->_snr = i*0.25; 948 if (i) { 949 frqObs->_snr = i * 0.25; 955 950 frqObs->_snrValid = true; 956 951 } … … 960 955 _CurrentObsList.push_back(CurrentObs); 961 956 } 962 if(!syncf) 963 { 957 if (!syncf) { 964 958 decoded = true; 965 959 _obsList.append(_CurrentObsList); … … 972 966 // 973 967 //////////////////////////////////////////////////////////////////////////// 974 bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) 975 { 968 bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) { 976 969 bool decoded = false; 977 970 978 if(size == 67) 979 { 971 if (size == 67) { 980 972 t_ephGPS eph; 981 973 int i, week; … … 999 991 GETBITS(i, 16) 1000 992 i <<= 4; 1001 eph._TOC.set(i*1000); 1002 GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25)) 1003 GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13)) 1004 GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1)) 993 eph._TOC.set(i * 1000); 994 GETFLOATSIGN(eph._clock_driftrate, 8, 995 1.0 / (double )(1 << 30) / (double )(1 << 25)) 996 GETFLOATSIGN(eph._clock_drift, 16, 997 1.0 / (double )(1 << 30) / (double )(1 << 13)) 998 GETFLOATSIGN(eph._clock_bias, 22, 999 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1005 1000 GETBITS(eph._IODC, 10) 1006 GETFLOATSIGN(eph._Crs, 16, 1.0 /(double)(1<<5))1001 GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) 1007 1002 GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1008 1003 GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1009 GETFLOATSIGN(eph._Cuc, 16, 1.0 /(double)(1<<29))1010 GETFLOAT(eph._e, 32, 1.0 /(double)(1<<30)/(double)(1<<3))1011 GETFLOATSIGN(eph._Cus, 16, 1.0 /(double)(1<<29))1012 GETFLOAT(eph._sqrt_A, 32, 1.0 /(double)(1<<19))1004 GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) 1005 GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1006 GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) 1007 GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) 1013 1008 GETBITS(i, 16) 1014 1009 i <<= 4; 1015 1010 eph._TOEsec = i; 1016 1011 bncTime t; 1017 t.set(i *1000);1012 t.set(i * 1000); 1018 1013 eph._TOEweek = t.gpsw(); 1019 1014 /* week from HOW, differs from TOC, TOE week, we use adapted value instead */ 1020 if (eph._TOEweek > week + 1 || eph._TOEweek < week-1) /* invalid week */1015 if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */ 1021 1016 return false; 1022 GETFLOATSIGN(eph._Cic, 16, 1.0 /(double)(1<<29))1017 GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) 1023 1018 GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1024 GETFLOATSIGN(eph._Cis, 16, 1.0 /(double)(1<<29))1019 GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) 1025 1020 GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1026 GETFLOATSIGN(eph._Crc, 16, 1.0 /(double)(1<<5))1021 GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) 1027 1022 GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1028 1023 GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1029 GETFLOATSIGN(eph._TGD, 8, 1.0 /(double)(1<<30)/(double)(1<<1))1024 GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1030 1025 GETBITS(eph._health, 6) 1031 1026 GETBITS(eph._L2PFlag, 1) … … 1041 1036 // 1042 1037 //////////////////////////////////////////////////////////////////////////// 1043 bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) 1044 { 1038 bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) { 1045 1039 bool decoded = false; 1046 1040 1047 if(size == 51) 1048 { 1041 if (size == 51) { 1049 1042 t_ephGlo eph; 1050 1043 int sv, i, tk; … … 1061 1054 1062 1055 GETBITS(i, 5) 1063 eph._frequency_number = i-7; 1064 GLOFreq[sv-1] = 100+i-7; /* store frequency for other users (MSM) */ 1065 _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(eph._frequency_number,2,'f',0); 1066 SKIPBITS(4) /* almanac healthy, almanac health ok, P1 */ 1056 eph._frequency_number = i - 7; 1057 GLOFreq[sv - 1] = 100 + i - 7; /* store frequency for other users (MSM) */ 1058 _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg( 1059 eph._frequency_number, 2, 'f', 0); 1060 SKIPBITS(4) 1061 /* almanac healthy, almanac health ok, P1 */ 1067 1062 GETBITS(i, 5) 1068 tk = i *60*60;1063 tk = i * 60 * 60; 1069 1064 GETBITS(i, 6) 1070 tk += i *60;1065 tk += i * 60; 1071 1066 GETBITS(i, 1) 1072 tk += i *30;1073 eph._tki = tk < 3 *60*60 ? tk-3*60*60+86400 : tk-3*60*60;1067 tk += i * 30; 1068 eph._tki = tk < 3 * 60 * 60 ? tk - 3 * 60 * 60 + 86400 : tk - 3 * 60 * 60; 1074 1069 GETBITS(eph._health, 1) 1075 SKIPBITS(1) /* P2 */ 1070 SKIPBITS(1) 1071 /* P2 */ 1076 1072 GETBITS(i, 7) 1077 eph._TOC.setTk(i*15*60*1000); /* tb */ 1078 1079 GETFLOATSIGNM(eph._x_velocity, 24, 1.0/(double)(1<<20)) 1080 GETFLOATSIGNM(eph._x_pos, 27, 1.0/(double)(1<<11)) 1081 GETFLOATSIGNM(eph._x_acceleration, 5, 1.0/(double)(1<<30)) 1082 GETFLOATSIGNM(eph._y_velocity, 24, 1.0/(double)(1<<20)) 1083 GETFLOATSIGNM(eph._y_pos, 27, 1.0/(double)(1<<11)) 1084 GETFLOATSIGNM(eph._y_acceleration, 5, 1.0/(double)(1<<30)) 1085 GETFLOATSIGNM(eph._z_velocity, 24, 1.0/(double)(1<<20)) 1086 GETFLOATSIGNM(eph._z_pos, 27, 1.0/(double)(1<<11)) 1087 GETFLOATSIGNM(eph._z_acceleration, 5, 1.0/(double)(1<<30)) 1088 SKIPBITS(1) /* P3 */ 1089 GETFLOATSIGNM(eph._gamma, 11, 1.0/(double)(1<<30)/(double)(1<<10)) 1090 SKIPBITS(3) /* GLONASS-M P, GLONASS-M ln (third string) */ 1091 GETFLOATSIGNM(eph._tau, 22, 1.0/(double)(1<<30)) /* GLONASS tau n(tb) */ 1092 SKIPBITS(5) /* GLONASS-M delta tau n(tb) */ 1073 eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */ 1074 1075 GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20)) 1076 GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11)) 1077 GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30)) 1078 GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20)) 1079 GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11)) 1080 GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30)) 1081 GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20)) 1082 GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11)) 1083 GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30)) 1084 SKIPBITS(1) 1085 /* P3 */ 1086 GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10)) 1087 SKIPBITS(3) 1088 /* GLONASS-M P, GLONASS-M ln (third string) */ 1089 GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30)) 1090 /* GLONASS tau n(tb) */ 1091 SKIPBITS(5) 1092 /* GLONASS-M delta tau n(tb) */ 1093 1093 GETBITS(eph._E, 5) 1094 1094 /* GETBITS(i, 1) / * GLONASS-M P4 */ … … 1123 1123 // 1124 1124 //////////////////////////////////////////////////////////////////////////// 1125 bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) 1126 { 1125 bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) { 1127 1126 bool decoded = false; 1128 1127 1129 if(size == 67) 1130 { 1128 if (size == 67) { 1131 1129 t_ephGPS eph; 1132 1130 int i, week; … … 1144 1142 GETBITS(i, 16) 1145 1143 i <<= 4; 1146 eph._TOC.set(i*1000); 1147 1148 GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25)) 1149 GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13)) 1150 GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1)) 1144 eph._TOC.set(i * 1000); 1145 1146 GETFLOATSIGN(eph._clock_driftrate, 8, 1147 1.0 / (double )(1 << 30) / (double )(1 << 25)) 1148 GETFLOATSIGN(eph._clock_drift, 16, 1149 1.0 / (double )(1 << 30) / (double )(1 << 13)) 1150 GETFLOATSIGN(eph._clock_bias, 22, 1151 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1151 1152 GETBITS(eph._IODE, 8) 1152 GETFLOATSIGN(eph._Crs, 16, 1.0 /(double)(1<<5))1153 GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) 1153 1154 GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1154 1155 GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1155 GETFLOATSIGN(eph._Cuc, 16, 1.0 /(double)(1<<29))1156 GETFLOAT(eph._e, 32, 1.0 /(double)(1<<30)/(double)(1<<3))1157 GETFLOATSIGN(eph._Cus, 16, 1.0 /(double)(1<<29))1158 GETFLOAT(eph._sqrt_A, 32, 1.0 /(double)(1<<19))1156 GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) 1157 GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1158 GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) 1159 GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) 1159 1160 GETBITS(i, 16) 1160 1161 i <<= 4; … … 1163 1164 t.set(i); 1164 1165 1165 GETFLOATSIGN(eph._Cic, 16, 1.0 /(double)(1<<29))1166 GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) 1166 1167 GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1167 GETFLOATSIGN(eph._Cis, 16, 1.0 /(double)(1<<29))1168 GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) 1168 1169 GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1169 GETFLOATSIGN(eph._Crc, 16, 1.0 /(double)(1<<5))1170 GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) 1170 1171 GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1171 1172 GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) … … 1176 1177 eph._TOEweek = t.gpsw(); 1177 1178 /* week from HOW, differs from TOC, TOE week, we use adapted value instead */ 1178 if (eph._TOEweek > week + 1 || eph._TOEweek < week-1) /* invalid week */1179 if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */ 1179 1180 return false; 1180 1181 1181 1182 GETBITS(i, 4) 1182 if (i <= 6)1183 eph._ura = ceil(10.0 *pow(2.0, 1.0+i/2.0))/10.0;1183 if (i <= 6) 1184 eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0; 1184 1185 else 1185 eph._ura = ceil(10.0 *pow(2.0, i/2.0))/10.0;1186 eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0; 1186 1187 GETBITS(eph._health, 6) 1187 GETFLOATSIGN(eph._TGD, 8, 1.0 /(double)(1<<30)/(double)(1<<1))1188 GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1188 1189 GETBITS(eph._IODC, 10) 1189 1190 GETBITS(eph._fitInterval, 1) … … 1199 1200 // 1200 1201 //////////////////////////////////////////////////////////////////////////// 1201 bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) 1202 { 1202 bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) { 1203 1203 bool decoded = false; 1204 1204 1205 if(size == 35) 1206 { 1205 if (size == 35) { 1207 1206 t_ephSBAS eph; 1208 1207 int i; … … 1216 1215 1217 1216 GETBITS(i, 6) 1218 eph._prn.set('S', 20 +i);1217 eph._prn.set('S', 20 + i); 1219 1218 GETBITS(eph._IODN, 8) 1220 1219 GETBITS(i, 13) 1221 1220 i <<= 4; 1222 eph._TOC.setTOD(i *1000);1221 eph._TOC.setTOD(i * 1000); 1223 1222 GETBITS(i, 4) 1224 1223 eph._ura = accuracyFromIndex(i, eph.type()); … … 1232 1231 GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125) 1233 1232 GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625) 1234 GETFLOATSIGN(eph._agf0, 12, 1.0 /(1<<30)/(1<<1))1235 GETFLOATSIGN(eph._agf1, 8, 1.0 /(1<<30)/(1<<10))1233 GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1)) 1234 GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10)) 1236 1235 1237 1236 eph._TOW = 0.9999E9; … … 1246 1245 // 1247 1246 //////////////////////////////////////////////////////////////////////////// 1248 bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) 1249 { 1247 bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) { 1250 1248 bool decoded = false; 1251 1249 uint64_t numbits = 0, bitfield = 0; … … 1256 1254 GETBITS(i, 12) 1257 1255 1258 if((i == 1046 && size == 61) || (i == 1045 && size == 60)) 1259 { 1256 if ((i == 1046 && size == 61) || (i == 1045 && size == 60)) { 1260 1257 t_ephGal eph; 1261 1258 … … 1273 1270 GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1274 1271 GETBITSFACTOR(i, 14, 60) 1275 eph._TOC.set(1024+eph._TOEweek, i); 1276 GETFLOATSIGN(eph._clock_driftrate, 6, 1.0/(double)(1<<30)/(double)(1<<29)) 1277 GETFLOATSIGN(eph._clock_drift, 21, 1.0/(double)(1<<30)/(double)(1<<16)) 1278 GETFLOATSIGN(eph._clock_bias, 31, 1.0/(double)(1<<30)/(double)(1<<4)) 1279 GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5)) 1272 eph._TOC.set(1024 + eph._TOEweek, i); 1273 GETFLOATSIGN(eph._clock_driftrate, 6, 1274 1.0 / (double )(1 << 30) / (double )(1 << 29)) 1275 GETFLOATSIGN(eph._clock_drift, 21, 1276 1.0 / (double )(1 << 30) / (double )(1 << 16)) 1277 GETFLOATSIGN(eph._clock_bias, 31, 1278 1.0 / (double )(1 << 30) / (double )(1 << 4)) 1279 GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) 1280 1280 GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1281 1281 GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1282 GETFLOATSIGN(eph._Cuc, 16, 1.0 /(double)(1<<29))1283 GETFLOAT(eph._e, 32, 1.0 /(double)(1<<30)/(double)(1<<3))1284 GETFLOATSIGN(eph._Cus, 16, 1.0 /(double)(1<<29))1285 GETFLOAT(eph._sqrt_A, 32, 1.0 /(double)(1<<19))1282 GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) 1283 GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1284 GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) 1285 GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) 1286 1286 GETBITSFACTOR(eph._TOEsec, 14, 60) 1287 1287 /* FIXME: overwrite value, copied from old code */ 1288 1288 eph._TOEsec = eph._TOC.gpssec(); 1289 GETFLOATSIGN(eph._Cic, 16, 1.0 /(double)(1<<29))1289 GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) 1290 1290 GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1291 GETFLOATSIGN(eph._Cis, 16, 1.0 /(double)(1<<29))1291 GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) 1292 1292 GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1293 GETFLOATSIGN(eph._Crc, 16, 1.0 /(double)(1<<5))1293 GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) 1294 1294 GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1295 1295 GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1296 GETFLOATSIGN(eph._BGD_1_5A, 10, 1.0/(double)(1<<30)/(double)(1<<2))1297 if(eph._inav)1298 {1296 GETFLOATSIGN(eph._BGD_1_5A, 10, 1297 1.0 / (double )(1 << 30) / (double )(1 << 2)) 1298 if (eph._inav) { 1299 1299 /* set unused F/NAV values */ 1300 1300 eph._E5aHS = 0.0; 1301 1301 eph._e5aDataInValid = false; 1302 1302 1303 GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0/(double)(1<<30)/(double)(1<<2)) 1303 GETFLOATSIGN(eph._BGD_1_5B, 10, 1304 1.0 / (double )(1 << 30) / (double )(1 << 2)) 1304 1305 GETBITS(eph._E5bHS, 2) 1305 1306 GETBITS(eph._e5bDataInValid, 1) … … 1307 1308 GETBITS(eph._e1DataInValid, 1) 1308 1309 } 1309 else 1310 { 1310 else { 1311 1311 /* set unused I/NAV values */ 1312 1312 eph._BGD_1_5B = 0.0; … … 1329 1329 // 1330 1330 //////////////////////////////////////////////////////////////////////////// 1331 bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) 1332 { 1331 bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) { 1333 1332 bool decoded = false; 1334 1333 1335 if(size == 70) 1336 { 1334 if (size == 70) { 1337 1335 t_ephBDS eph; 1338 1336 int i; … … 1348 1346 eph._prn.set('C', i); 1349 1347 1350 SKIPBITS(13) /* week */ 1348 SKIPBITS(13) 1349 /* week */ 1351 1350 GETBITS(i, 4) 1352 1351 eph._URA = accuracyFromIndex(i, eph.type()); … … 1355 1354 GETBITS(i, 17) 1356 1355 i <<= 3; 1357 eph._TOC.setBDS(i*1000); 1358 GETFLOATSIGN(eph._clock_driftrate, 11, 1.0/(double)(1<<30)/(double)(1<<30)/(double)(1<<6)) 1359 GETFLOATSIGN(eph._clock_drift, 22, 1.0/(double)(1<<30)/(double)(1<<20)) 1360 GETFLOATSIGN(eph._clock_bias, 24, 1.0/(double)(1<<30)/(double)(1<<3)) 1356 eph._TOC.setBDS(i * 1000); 1357 GETFLOATSIGN(eph._clock_driftrate, 11, 1358 1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6)) 1359 GETFLOATSIGN(eph._clock_drift, 22, 1360 1.0 / (double )(1 << 30) / (double )(1 << 20)) 1361 GETFLOATSIGN(eph._clock_bias, 24, 1362 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1361 1363 GETBITS(eph._AODC, 5) 1362 GETFLOATSIGN(eph._Crs, 18, 1.0 /(double)(1<<6))1364 GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6)) 1363 1365 GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1364 1366 GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1365 GETFLOATSIGN(eph._Cuc, 18, 1.0 /(double)(1<<30)/(double)(1<<1))1366 GETFLOAT(eph._e, 32, 1.0 /(double)(1<<30)/(double)(1<<3))1367 GETFLOATSIGN(eph._Cus, 18, 1.0 /(double)(1<<30)/(double)(1<<1))1368 GETFLOAT(eph._sqrt_A, 32, 1.0 /(double)(1<<19))1367 GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1368 GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1369 GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1370 GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) 1369 1371 GETBITS(i, 17) 1370 1372 i <<= 3; 1371 1373 eph._TOEsec = i; 1372 eph._TOE.setBDS(i *1000);1373 GETFLOATSIGN(eph._Cic, 18, 1.0 /(double)(1<<30)/(double)(1<<1))1374 eph._TOE.setBDS(i * 1000); 1375 GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1374 1376 GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1375 GETFLOATSIGN(eph._Cis, 18, 1.0 /(double)(1<<30)/(double)(1<<1))1377 GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1376 1378 GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1377 GETFLOATSIGN(eph._Crc, 18, 1.0 /(double)(1<<6))1379 GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6)) 1378 1380 GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1379 1381 GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) … … 1391 1393 // 1392 1394 //////////////////////////////////////////////////////////////////////////// 1393 bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size) 1394 { 1395 bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size) { 1395 1396 char *antenna, type[256]; 1396 1397 int antnum = -1; … … 1414 1415 // 1415 1416 //////////////////////////////////////////////////////////////////////////// 1416 bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) 1417 { 1417 bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) { 1418 1418 int type; 1419 1419 uint64_t numbits = 0, bitfield = 0; … … 1435 1435 GETBITSSIGN(z, 38) 1436 1436 _antList.back().zz = z * 1e-4; 1437 if (type == 1006)1438 {1437 if (type == 1006) 1438 { 1439 1439 double h; 1440 1440 GETBITS(h, 16) … … 1442 1442 _antList.back().height_f = true; 1443 1443 } 1444 _antList.back().message 1444 _antList.back().message = type; 1445 1445 1446 1446 return true; … … 1449 1449 // 1450 1450 //////////////////////////////////////////////////////////////////////////// 1451 t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) 1452 { 1451 t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) { 1453 1452 bool decoded = false; 1454 1453 1455 1454 errmsg.clear(); 1456 1455 1457 while(bufLen && _MessageSize < sizeof(_Message)) 1458 { 1456 while (bufLen && _MessageSize < sizeof(_Message)) { 1459 1457 int l = sizeof(_Message) - _MessageSize; 1460 if (l > bufLen)1458 if (l > bufLen) 1461 1459 l = bufLen; 1462 memcpy(_Message +_MessageSize, buffer, l);1460 memcpy(_Message + _MessageSize, buffer, l); 1463 1461 _MessageSize += l; 1464 1462 bufLen -= l; 1465 1463 buffer += l; 1466 1464 int id; 1467 while((id = GetMessage())) 1468 { 1465 while ((id = GetMessage())) { 1469 1466 /* reset station ID for file loading as it can change */ 1470 if (_rawFile)1467 if (_rawFile) 1471 1468 _staID = _rawFile->staID(); 1472 1469 /* store the id into the list of loaded blocks */ … … 1476 1473 * extracted data block. That does no harm, as it anyway skip everything 1477 1474 * else. */ 1478 if((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) 1479 { 1475 if ((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) { 1480 1476 if (!_coDecoders.contains(_staID.toAscii())) 1481 1477 _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID); 1482 1478 RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()]; 1483 if(coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize, 1484 errmsg) == success) 1485 { 1479 if (coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize, 1480 errmsg) == success) { 1486 1481 decoded = true; 1487 1482 } 1488 1483 } 1489 else if(id >= 1070 && id <= 1229) /* MSM */ 1490 { 1491 if(DecodeRTCM3MSM(_Message, _BlockSize)) 1484 else if (id >= 1070 && id <= 1229) /* MSM */ { 1485 if (DecodeRTCM3MSM(_Message, _BlockSize)) 1492 1486 decoded = true; 1493 1487 } 1494 else 1495 { 1496 switch(id) 1497 { 1498 case 1001: case 1003: 1499 emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!") 1500 .arg(_staID).arg(id).toAscii(), true)); 1501 break; /* no use decoding partial data ATM, remove break when data can be used */ 1502 case 1002: case 1004: 1503 if(DecodeRTCM3GPS(_Message, _BlockSize)) 1504 decoded = true; 1505 break; 1506 case 1009: case 1011: 1507 emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!") 1508 .arg(_staID).arg(id).toAscii(), true)); 1509 break; /* no use decoding partial data ATM, remove break when data can be used */ 1510 case 1010: case 1012: 1511 if(DecodeRTCM3GLONASS(_Message, _BlockSize)) 1512 decoded = true; 1513 break; 1514 case 1019: 1515 if(DecodeGPSEphemeris(_Message, _BlockSize)) 1516 decoded = true; 1517 break; 1518 case 1020: 1519 if(DecodeGLONASSEphemeris(_Message, _BlockSize)) 1520 decoded = true; 1521 break; 1522 case 1043: 1523 if(DecodeSBASEphemeris(_Message, _BlockSize)) 1524 decoded = true; 1525 break; 1526 case 1044: 1527 if(DecodeQZSSEphemeris(_Message, _BlockSize)) 1528 decoded = true; 1529 break; 1530 case 1045: case 1046: 1531 if(DecodeGalileoEphemeris(_Message, _BlockSize)) 1532 decoded = true; 1533 break; 1534 case RTCM3ID_BDS: 1535 if(DecodeBDSEphemeris(_Message, _BlockSize)) 1536 decoded = true; 1537 break; 1538 case 1007: case 1008: case 1033: 1539 DecodeAntenna(_Message, _BlockSize); 1540 break; 1541 case 1005: case 1006: 1542 DecodeAntennaPosition(_Message, _BlockSize); 1543 break; 1488 else { 1489 switch (id) { 1490 case 1001: 1491 case 1003: 1492 emit(newMessage( 1493 QString("%1: Block %2 contain partial data! Ignored!") 1494 .arg(_staID).arg(id).toAscii(), true)); 1495 break; /* no use decoding partial data ATM, remove break when data can be used */ 1496 case 1002: 1497 case 1004: 1498 if (DecodeRTCM3GPS(_Message, _BlockSize)) 1499 decoded = true; 1500 break; 1501 case 1009: 1502 case 1011: 1503 emit(newMessage( 1504 QString("%1: Block %2 contain partial data! Ignored!") 1505 .arg(_staID).arg(id).toAscii(), true)); 1506 break; /* no use decoding partial data ATM, remove break when data can be used */ 1507 case 1010: 1508 case 1012: 1509 if (DecodeRTCM3GLONASS(_Message, _BlockSize)) 1510 decoded = true; 1511 break; 1512 case 1019: 1513 if (DecodeGPSEphemeris(_Message, _BlockSize)) 1514 decoded = true; 1515 break; 1516 case 1020: 1517 if (DecodeGLONASSEphemeris(_Message, _BlockSize)) 1518 decoded = true; 1519 break; 1520 case 1043: 1521 if (DecodeSBASEphemeris(_Message, _BlockSize)) 1522 decoded = true; 1523 break; 1524 case 1044: 1525 if (DecodeQZSSEphemeris(_Message, _BlockSize)) 1526 decoded = true; 1527 break; 1528 case 1045: 1529 case 1046: 1530 if (DecodeGalileoEphemeris(_Message, _BlockSize)) 1531 decoded = true; 1532 break; 1533 case RTCM3ID_BDS: 1534 if (DecodeBDSEphemeris(_Message, _BlockSize)) 1535 decoded = true; 1536 break; 1537 case 1007: 1538 case 1008: 1539 case 1033: 1540 DecodeAntenna(_Message, _BlockSize); 1541 break; 1542 case 1005: 1543 case 1006: 1544 DecodeAntennaPosition(_Message, _BlockSize); 1545 break; 1544 1546 } 1545 1547 } … … 1547 1549 } 1548 1550 return decoded ? success : failure; 1549 }; 1551 } 1552 ; 1550 1553 1551 1554 // 1552 1555 //////////////////////////////////////////////////////////////////////////// 1553 uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) 1554 { 1556 uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) { 1555 1557 uint32_t crc = 0; 1556 1558 int i; 1557 1559 1558 while(size--) 1559 { 1560 while (size--) { 1560 1561 crc ^= (*buf++) << (16); 1561 for (i = 0; i < 8; i++)1562 {1562 for (i = 0; i < 8; i++) 1563 { 1563 1564 crc <<= 1; 1564 if (crc & 0x1000000)1565 if (crc & 0x1000000) 1565 1566 crc ^= 0x01864cfb; 1566 1567 } … … 1571 1572 // 1572 1573 //////////////////////////////////////////////////////////////////////////// 1573 int RTCM3Decoder::GetMessage(void) 1574 { 1574 int RTCM3Decoder::GetMessage(void) { 1575 1575 unsigned char *m, *e; 1576 1576 int i; 1577 1577 1578 m = _Message +_SkipBytes;1579 e = _Message +_MessageSize;1578 m = _Message + _SkipBytes; 1579 e = _Message + _MessageSize; 1580 1580 _NeedBytes = _SkipBytes = 0; 1581 while(e-m >= 3) 1582 { 1583 if(m[0] == 0xD3) 1584 { 1585 _BlockSize = ((m[1]&3)<<8)|m[2]; 1586 if(e-m >= static_cast<int>(_BlockSize+6)) 1587 { 1588 if(static_cast<uint32_t>((m[3+_BlockSize]<<16)|(m[3+_BlockSize+1]<<8) 1589 |(m[3+_BlockSize+2])) == CRC24(_BlockSize+3, m)) 1590 { 1591 _BlockSize +=6; 1581 while (e - m >= 3) { 1582 if (m[0] == 0xD3) { 1583 _BlockSize = ((m[1] & 3) << 8) | m[2]; 1584 if (e - m >= static_cast<int>(_BlockSize + 6)) { 1585 if (static_cast<uint32_t>((m[3 + _BlockSize] << 16) 1586 | (m[3 + _BlockSize + 1] << 8) 1587 | (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) { 1588 _BlockSize += 6; 1592 1589 _SkipBytes = _BlockSize; 1593 1590 break; … … 1596 1593 ++m; 1597 1594 } 1598 else 1599 { 1595 else { 1600 1596 _NeedBytes = _BlockSize; 1601 1597 break; … … 1605 1601 ++m; 1606 1602 } 1607 if (e-m < 3)1603 if (e - m < 3) 1608 1604 _NeedBytes = 3; 1609 1605 1610 1606 /* copy buffer to front */ 1611 1607 i = m - _Message; 1612 if (i && m < e)1613 memmove(_Message, m, static_cast<size_t>(_MessageSize -i));1608 if (i && m < e) 1609 memmove(_Message, m, static_cast<size_t>(_MessageSize - i)); 1614 1610 _MessageSize -= i; 1615 1611 1616 return !_NeedBytes ? ((_Message[3] <<4)|(_Message[4]>>4)) : 0;1612 return !_NeedBytes ? ((_Message[3] << 4) | (_Message[4] >> 4)) : 0; 1617 1613 } 1618 1614 … … 1620 1616 ////////////////////////////////////////////////////////////////////////////// 1621 1617 int RTCM3Decoder::corrGPSEpochTime() const { 1622 return _coDecoders.size() > 0 ? _coDecoders.begin().value()->corrGPSEpochTime() : -1; 1623 } 1618 return 1619 _coDecoders.size() > 0 ? 1620 _coDecoders.begin().value()->corrGPSEpochTime() : -1; 1621 }
Note:
See TracChangeset
for help on using the changeset viewer.
