Changeset 8161 in ntrip for branches/BNC_2.12/src/RTCM3/RTCM3Decoder.cpp
- Timestamp:
- Oct 23, 2017, 10:24:24 AM (7 years ago)
- File:
-
- 1 edited
-
branches/BNC_2.12/src/RTCM3/RTCM3Decoder.cpp (modified) (47 diffs)
Legend:
- Unmodified
- Added
- Removed
-
branches/BNC_2.12/src/RTCM3/RTCM3Decoder.cpp
r7875 r8161 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) 108 SKIPBITS(12) 109 /* id */ 110 GETBITS(i, 30) 106 111 107 112 CurrentObsTime.set(i); 108 if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime) 109 { 113 if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) { 110 114 decoded = true; 111 _obsList .append(_CurrentObsList);115 _obsList = _CurrentObsList; 112 116 _CurrentObsList.clear(); 113 117 } … … 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 _obsList = _CurrentObsList;466 _obsList.append(_CurrentObsList); 470 467 _CurrentObsList.clear(); 471 468 } … … 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 if (psr[count] > -1.0 / (1 << 10)) { 735 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 736 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; 737 frqObs->_codeValid = true; 738 } 739 740 if (cp[count] > -1.0 / (1 << 8)) { 741 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 742 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 743 / cd.wl; 744 frqObs->_phaseValid = true; 745 frqObs->_slipCounter = ll[count]; 746 } 747 748 frqObs->_snr = cnr[count]; 749 frqObs->_snrValid = true; 750 break; 751 case 5: 752 if (psr[count] > -1.0 / (1 << 10)) { 753 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 754 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; 755 frqObs->_codeValid = true; 756 } 757 758 if (cp[count] > -1.0 / (1 << 8)) { 759 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 760 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 761 / cd.wl; 762 frqObs->_phaseValid = true; 763 frqObs->_slipCounter = ll[count]; 764 } 765 766 frqObs->_snr = cnr[count]; 767 frqObs->_snrValid = true; 768 769 if (dop[count] > -1.6384) { 770 frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl; 771 frqObs->_dopplerValid = true; 772 } 773 break; 774 case 6: 775 if (psr[count] > -1.0 / (1 << 10)) { 776 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 777 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; 778 frqObs->_codeValid = true; 779 } 780 781 if (cp[count] > -1.0 / (1 << 8)) { 782 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 783 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 784 / cd.wl; 785 frqObs->_phaseValid = true; 786 frqObs->_slipCounter = ll[count]; 787 } 788 789 frqObs->_snr = cnr[count]; 790 frqObs->_snrValid = true; 791 break; 792 case 7: 793 if (psr[count] > -1.0 / (1 << 10)) { 794 frqObs->_code = psr[count] * LIGHTSPEED / 1000.0 795 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0; 796 frqObs->_codeValid = true; 797 } 798 799 if (cp[count] > -1.0 / (1 << 8)) { 800 frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl 801 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 802 / cd.wl; 803 frqObs->_phaseValid = true; 804 frqObs->_slipCounter = ll[count]; 805 } 806 807 frqObs->_snr = cnr[count]; 808 frqObs->_snrValid = true; 809 810 if (dop[count] > -1.6384) { 811 frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl; 812 frqObs->_dopplerValid = true; 813 } 814 break; 829 815 } 830 816 CurrentObs._obs.push_back(frqObs); … … 832 818 } 833 819 } 834 if (CurrentObs._obs.size() > 0)820 if (CurrentObs._obs.size() > 0) 835 821 _CurrentObsList.push_back(CurrentObs); 836 822 } 837 823 } 838 else if((type % 10) < 3) 839 { 824 else if ((type % 10) < 3) { 840 825 emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!") 841 .arg(_staID).arg(type).toAscii(), true)); 842 } 843 if(!syncf) 844 { 826 .arg(_staID).arg(type).toAscii(), true)); 827 } 828 if (!syncf) { 845 829 decoded = true; 846 830 _obsList = _CurrentObsList; … … 853 837 // 854 838 //////////////////////////////////////////////////////////////////////////// 855 bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) 856 { 839 bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) { 857 840 bool decoded = false; 858 841 bncTime CurrentObsTime; … … 864 847 865 848 GETBITS(type, 12) 866 SKIPBITS(12) /* id */ 867 GETBITS(i,27) /* tk */ 849 SKIPBITS(12) 850 /* id */ 851 GETBITS(i, 27) 852 /* tk */ 868 853 869 854 CurrentObsTime.setTk(i); 870 if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime) 871 { 855 if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) { 872 856 decoded = true; 873 857 _obsList.append(_CurrentObsList); … … 876 860 _CurrentTime = CurrentObsTime; 877 861 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; 862 GETBITS(syncf, 1) 863 /* sync */ 864 GETBITS(numsats, 5) 865 SKIPBITS(4) 866 /* smind, smint */ 867 868 while (numsats--) { 869 int sv, code, l1range, amb = 0, freq; 885 870 t_satObs CurrentObs; 886 871 CurrentObs._time = CurrentObsTime; … … 890 875 GETBITS(code, 1) 891 876 GETBITS(freq, 5) 892 GLOFreq[sv -1] = 100+freq-7; /* store frequency for other users (MSM) */877 GLOFreq[sv - 1] = 100 + freq - 7; /* store frequency for other users (MSM) */ 893 878 894 879 t_frqObs *frqObs = new t_frqObs; 895 880 /* L1 */ 896 (code) ? frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C"); 881 (code) ? 882 frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C"); 897 883 GETBITS(l1range, 25); 898 884 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);885 if ((i & ((1 << 20) - 1)) != 0x80000) { 886 frqObs->_code = l1range * 0.02; 887 frqObs->_phase = (l1range * 0.02 + i * 0.0005) 888 / GLO_WAVELENGTH_L1(freq - 7); 903 889 frqObs->_codeValid = frqObs->_phaseValid = true; 904 890 } 905 891 GETBITS(i, 7); 906 892 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); 893 if (type == 1010 || type == 1012) { 894 GETBITS(amb, 7); 895 if (amb) { 896 frqObs->_code += amb * 599584.916; 897 frqObs->_phase += (amb * 599584.916) / GLO_WAVELENGTH_L1(freq - 7); 914 898 } 915 899 GETBITS(i, 8); 916 if(i) 917 { 918 frqObs->_snr = i*0.25; 900 if (i) { 901 frqObs->_snr = i * 0.25; 919 902 frqObs->_snrValid = true; 920 903 } 921 904 } 922 905 CurrentObs._obs.push_back(frqObs); 923 if(type == 1011 || type == 1012) 924 { 906 if (type == 1011 || type == 1012) { 925 907 frqObs = new t_frqObs; 926 908 /* 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; 909 GETBITS(code, 2); 910 switch (code) { 911 case 3: 912 frqObs->_rnxType2ch.assign("2P"); 913 break; 914 case 2: 915 frqObs->_rnxType2ch.assign("2P"); 916 break; 917 case 1: 918 frqObs->_rnxType2ch.assign("2P"); 919 break; 920 case 0: 921 frqObs->_rnxType2ch.assign("2C"); 922 break; 923 } 924 GETBITSSIGN(i, 14); 925 if ((i & ((1 << 14) - 1)) != 0x2000) { 926 frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 599584.916; 939 927 frqObs->_codeValid = true; 940 928 } 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);929 GETBITSSIGN(i, 20); 930 if ((i & ((1 << 20) - 1)) != 0x80000) { 931 frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 599584.916) 932 / GLO_WAVELENGTH_L2(freq - 7); 945 933 frqObs->_phaseValid = true; 946 934 } 947 GETBITS(i, 7);935 GETBITS(i, 7); 948 936 frqObs->_slipCounter = i; 949 if(type == 1012) 950 { 937 if (type == 1012) { 951 938 GETBITS(i, 8); 952 if(i) 953 { 954 frqObs->_snr = i*0.25; 939 if (i) { 940 frqObs->_snr = i * 0.25; 955 941 frqObs->_snrValid = true; 956 942 } … … 960 946 _CurrentObsList.push_back(CurrentObs); 961 947 } 962 if(!syncf) 963 { 948 if (!syncf) { 964 949 decoded = true; 965 950 _obsList.append(_CurrentObsList); … … 972 957 // 973 958 //////////////////////////////////////////////////////////////////////////// 974 bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) 975 { 959 bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) { 976 960 bool decoded = false; 977 961 978 if(size == 67) 979 { 962 if (size == 67) { 980 963 t_ephGPS eph; 981 964 int i, week; … … 999 982 GETBITS(i, 16) 1000 983 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)) 984 eph._TOC.set(i * 1000); 985 GETFLOATSIGN(eph._clock_driftrate, 8, 986 1.0 / (double )(1 << 30) / (double )(1 << 25)) 987 GETFLOATSIGN(eph._clock_drift, 16, 988 1.0 / (double )(1 << 30) / (double )(1 << 13)) 989 GETFLOATSIGN(eph._clock_bias, 22, 990 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1005 991 GETBITS(eph._IODC, 10) 1006 GETFLOATSIGN(eph._Crs, 16, 1.0 /(double)(1<<5))992 GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) 1007 993 GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1008 994 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))995 GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) 996 GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) 997 GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) 998 GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) 1013 999 GETBITS(i, 16) 1014 1000 i <<= 4; 1015 1001 eph._TOEsec = i; 1016 1002 bncTime t; 1017 t.set(i *1000);1003 t.set(i * 1000); 1018 1004 eph._TOEweek = t.gpsw(); 1019 1005 /* 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 */1006 if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */ 1021 1007 return false; 1022 GETFLOATSIGN(eph._Cic, 16, 1.0 /(double)(1<<29))1008 GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) 1023 1009 GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1024 GETFLOATSIGN(eph._Cis, 16, 1.0 /(double)(1<<29))1010 GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) 1025 1011 GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1026 GETFLOATSIGN(eph._Crc, 16, 1.0 /(double)(1<<5))1012 GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) 1027 1013 GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1028 1014 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))1015 GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1030 1016 GETBITS(eph._health, 6) 1031 1017 GETBITS(eph._L2PFlag, 1) … … 1041 1027 // 1042 1028 //////////////////////////////////////////////////////////////////////////// 1043 bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) 1044 { 1029 bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) { 1045 1030 bool decoded = false; 1046 1031 1047 if(size == 51) 1048 { 1032 if (size == 51) { 1049 1033 t_ephGlo eph; 1050 1034 int sv, i, tk; … … 1061 1045 1062 1046 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 */ 1047 eph._frequency_number = i - 7; 1048 GLOFreq[sv - 1] = 100 + i - 7; /* store frequency for other users (MSM) */ 1049 _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg( 1050 eph._frequency_number, 2, 'f', 0); 1051 SKIPBITS(4) 1052 /* almanac healthy, almanac health ok, P1 */ 1067 1053 GETBITS(i, 5) 1068 tk = i *60*60;1054 tk = i * 60 * 60; 1069 1055 GETBITS(i, 6) 1070 tk += i *60;1056 tk += i * 60; 1071 1057 GETBITS(i, 1) 1072 tk += i *30;1073 eph._tki = tk < 3 *60*60 ? tk-3*60*60+86400 : tk-3*60*60;1058 tk += i * 30; 1059 eph._tki = tk < 3 * 60 * 60 ? tk - 3 * 60 * 60 + 86400 : tk - 3 * 60 * 60; 1074 1060 GETBITS(eph._health, 1) 1075 SKIPBITS(1) /* P2 */ 1061 SKIPBITS(1) 1062 /* P2 */ 1076 1063 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) */ 1064 eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */ 1065 1066 GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20)) 1067 GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11)) 1068 GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30)) 1069 GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20)) 1070 GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11)) 1071 GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30)) 1072 GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20)) 1073 GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11)) 1074 GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30)) 1075 SKIPBITS(1) 1076 /* P3 */ 1077 GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10)) 1078 SKIPBITS(3) 1079 /* GLONASS-M P, GLONASS-M ln (third string) */ 1080 GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30)) 1081 /* GLONASS tau n(tb) */ 1082 SKIPBITS(5) 1083 /* GLONASS-M delta tau n(tb) */ 1093 1084 GETBITS(eph._E, 5) 1094 1085 /* GETBITS(i, 1) / * GLONASS-M P4 */ … … 1123 1114 // 1124 1115 //////////////////////////////////////////////////////////////////////////// 1125 bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) 1126 { 1116 bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) { 1127 1117 bool decoded = false; 1128 1118 1129 if(size == 67) 1130 { 1119 if (size == 67) { 1131 1120 t_ephGPS eph; 1132 1121 int i, week; … … 1144 1133 GETBITS(i, 16) 1145 1134 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)) 1135 eph._TOC.set(i * 1000); 1136 1137 GETFLOATSIGN(eph._clock_driftrate, 8, 1138 1.0 / (double )(1 << 30) / (double )(1 << 25)) 1139 GETFLOATSIGN(eph._clock_drift, 16, 1140 1.0 / (double )(1 << 30) / (double )(1 << 13)) 1141 GETFLOATSIGN(eph._clock_bias, 22, 1142 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1151 1143 GETBITS(eph._IODE, 8) 1152 GETFLOATSIGN(eph._Crs, 16, 1.0 /(double)(1<<5))1144 GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) 1153 1145 GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1154 1146 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))1147 GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) 1148 GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1149 GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) 1150 GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) 1159 1151 GETBITS(i, 16) 1160 1152 i <<= 4; … … 1163 1155 t.set(i); 1164 1156 1165 GETFLOATSIGN(eph._Cic, 16, 1.0 /(double)(1<<29))1157 GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) 1166 1158 GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1167 GETFLOATSIGN(eph._Cis, 16, 1.0 /(double)(1<<29))1159 GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) 1168 1160 GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1169 GETFLOATSIGN(eph._Crc, 16, 1.0 /(double)(1<<5))1161 GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) 1170 1162 GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1171 1163 GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) … … 1176 1168 eph._TOEweek = t.gpsw(); 1177 1169 /* 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 */1170 if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */ 1179 1171 return false; 1180 1172 1181 1173 GETBITS(i, 4) 1182 if (i <= 6)1183 eph._ura = ceil(10.0 *pow(2.0, 1.0+i/2.0))/10.0;1174 if (i <= 6) 1175 eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0; 1184 1176 else 1185 eph._ura = ceil(10.0 *pow(2.0, i/2.0))/10.0;1177 eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0; 1186 1178 GETBITS(eph._health, 6) 1187 GETFLOATSIGN(eph._TGD, 8, 1.0 /(double)(1<<30)/(double)(1<<1))1179 GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1188 1180 GETBITS(eph._IODC, 10) 1189 1181 GETBITS(eph._fitInterval, 1) … … 1199 1191 // 1200 1192 //////////////////////////////////////////////////////////////////////////// 1201 bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) 1202 { 1193 bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) { 1203 1194 bool decoded = false; 1204 1195 1205 if(size == 35) 1206 { 1196 if (size == 35) { 1207 1197 t_ephSBAS eph; 1208 1198 int i; … … 1216 1206 1217 1207 GETBITS(i, 6) 1218 eph._prn.set('S', 20 +i);1208 eph._prn.set('S', 20 + i); 1219 1209 GETBITS(eph._IODN, 8) 1220 1210 GETBITS(i, 13) 1221 1211 i <<= 4; 1222 eph._TOC.setTOD(i *1000);1212 eph._TOC.setTOD(i * 1000); 1223 1213 GETBITS(i, 4) 1224 1214 eph._ura = accuracyFromIndex(i, eph.type()); … … 1232 1222 GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125) 1233 1223 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))1224 GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1)) 1225 GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10)) 1236 1226 1237 1227 eph._TOW = 0.9999E9; … … 1246 1236 // 1247 1237 //////////////////////////////////////////////////////////////////////////// 1248 bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) 1249 { 1238 bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) { 1250 1239 bool decoded = false; 1251 1240 uint64_t numbits = 0, bitfield = 0; … … 1256 1245 GETBITS(i, 12) 1257 1246 1258 if((i == 1046 && size == 61) || (i == 1045 && size == 60)) 1259 { 1247 if ((i == 1046 && size == 61) || (i == 1045 && size == 60)) { 1260 1248 t_ephGal eph; 1261 1249 … … 1273 1261 GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1274 1262 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)) 1263 eph._TOC.set(1024 + eph._TOEweek, i); 1264 GETFLOATSIGN(eph._clock_driftrate, 6, 1265 1.0 / (double )(1 << 30) / (double )(1 << 29)) 1266 GETFLOATSIGN(eph._clock_drift, 21, 1267 1.0 / (double )(1 << 30) / (double )(1 << 16)) 1268 GETFLOATSIGN(eph._clock_bias, 31, 1269 1.0 / (double )(1 << 30) / (double )(1 << 4)) 1270 GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) 1280 1271 GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1281 1272 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))1273 GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29)) 1274 GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1275 GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29)) 1276 GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) 1286 1277 GETBITSFACTOR(eph._TOEsec, 14, 60) 1287 1278 /* FIXME: overwrite value, copied from old code */ 1288 1279 eph._TOEsec = eph._TOC.gpssec(); 1289 GETFLOATSIGN(eph._Cic, 16, 1.0 /(double)(1<<29))1280 GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29)) 1290 1281 GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1291 GETFLOATSIGN(eph._Cis, 16, 1.0 /(double)(1<<29))1282 GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29)) 1292 1283 GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1293 GETFLOATSIGN(eph._Crc, 16, 1.0 /(double)(1<<5))1284 GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5)) 1294 1285 GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1295 1286 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 {1287 GETFLOATSIGN(eph._BGD_1_5A, 10, 1288 1.0 / (double )(1 << 30) / (double )(1 << 2)) 1289 if (eph._inav) { 1299 1290 /* set unused F/NAV values */ 1300 1291 eph._E5aHS = 0.0; 1301 1292 eph._e5aDataInValid = false; 1302 1293 1303 GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0/(double)(1<<30)/(double)(1<<2)) 1294 GETFLOATSIGN(eph._BGD_1_5B, 10, 1295 1.0 / (double )(1 << 30) / (double )(1 << 2)) 1304 1296 GETBITS(eph._E5bHS, 2) 1305 1297 GETBITS(eph._e5bDataInValid, 1) … … 1307 1299 GETBITS(eph._e1DataInValid, 1) 1308 1300 } 1309 else 1310 { 1301 else { 1311 1302 /* set unused I/NAV values */ 1312 1303 eph._BGD_1_5B = 0.0; … … 1329 1320 // 1330 1321 //////////////////////////////////////////////////////////////////////////// 1331 bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) 1332 { 1322 bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) { 1333 1323 bool decoded = false; 1334 1324 1335 if(size == 70) 1336 { 1325 if (size == 70) { 1337 1326 t_ephBDS eph; 1338 1327 int i; … … 1348 1337 eph._prn.set('C', i); 1349 1338 1350 SKIPBITS(13) /* week */ 1339 SKIPBITS(13) 1340 /* week */ 1351 1341 GETBITS(i, 4) 1352 1342 eph._URA = accuracyFromIndex(i, eph.type()); … … 1355 1345 GETBITS(i, 17) 1356 1346 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)) 1347 eph._TOC.setBDS(i * 1000); 1348 GETFLOATSIGN(eph._clock_driftrate, 11, 1349 1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6)) 1350 GETFLOATSIGN(eph._clock_drift, 22, 1351 1.0 / (double )(1 << 30) / (double )(1 << 20)) 1352 GETFLOATSIGN(eph._clock_bias, 24, 1353 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1361 1354 GETBITS(eph._AODC, 5) 1362 GETFLOATSIGN(eph._Crs, 18, 1.0 /(double)(1<<6))1355 GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6)) 1363 1356 GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) 1364 1357 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))1358 GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1359 GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) 1360 GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1361 GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) 1369 1362 GETBITS(i, 17) 1370 1363 i <<= 3; 1371 1364 eph._TOEsec = i; 1372 eph._TOE.setBDS(i *1000);1373 GETFLOATSIGN(eph._Cic, 18, 1.0 /(double)(1<<30)/(double)(1<<1))1365 eph._TOE.setBDS(i * 1000); 1366 GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1374 1367 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))1368 GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1)) 1376 1369 GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1377 GETFLOATSIGN(eph._Crc, 18, 1.0 /(double)(1<<6))1370 GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6)) 1378 1371 GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1)) 1379 1372 GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13)) … … 1391 1384 // 1392 1385 //////////////////////////////////////////////////////////////////////////// 1393 bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size) 1394 { 1386 bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size) { 1395 1387 char *antenna, type[256]; 1396 1388 int antnum = -1; … … 1414 1406 // 1415 1407 //////////////////////////////////////////////////////////////////////////// 1416 bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) 1417 { 1408 bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) { 1418 1409 int type; 1419 1410 uint64_t numbits = 0, bitfield = 0; … … 1435 1426 GETBITSSIGN(z, 38) 1436 1427 _antList.back().zz = z * 1e-4; 1437 if (type == 1006)1438 {1428 if (type == 1006) 1429 { 1439 1430 double h; 1440 1431 GETBITS(h, 16) … … 1442 1433 _antList.back().height_f = true; 1443 1434 } 1444 _antList.back().message 1435 _antList.back().message = type; 1445 1436 1446 1437 return true; … … 1449 1440 // 1450 1441 //////////////////////////////////////////////////////////////////////////// 1451 t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) 1452 { 1442 t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) { 1453 1443 bool decoded = false; 1454 1444 1455 1445 errmsg.clear(); 1456 1446 1457 while(bufLen && _MessageSize < sizeof(_Message)) 1458 { 1447 while (bufLen && _MessageSize < sizeof(_Message)) { 1459 1448 int l = sizeof(_Message) - _MessageSize; 1460 if (l > bufLen)1449 if (l > bufLen) 1461 1450 l = bufLen; 1462 memcpy(_Message +_MessageSize, buffer, l);1451 memcpy(_Message + _MessageSize, buffer, l); 1463 1452 _MessageSize += l; 1464 1453 bufLen -= l; 1465 1454 buffer += l; 1466 1455 int id; 1467 while((id = GetMessage())) 1468 { 1456 while ((id = GetMessage())) { 1469 1457 /* reset station ID for file loading as it can change */ 1470 if (_rawFile)1458 if (_rawFile) 1471 1459 _staID = _rawFile->staID(); 1472 1460 /* store the id into the list of loaded blocks */ … … 1476 1464 * extracted data block. That does no harm, as it anyway skip everything 1477 1465 * else. */ 1478 if((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) 1479 { 1466 if ((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) { 1480 1467 if (!_coDecoders.contains(_staID.toAscii())) 1481 1468 _coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID); 1482 1469 RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()]; 1483 if(coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize, 1484 errmsg) == success) 1485 { 1470 if (coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize, 1471 errmsg) == success) { 1486 1472 decoded = true; 1487 1473 } 1488 1474 } 1489 else if(id >= 1070 && id <= 1229) /* MSM */ 1490 { 1491 if(DecodeRTCM3MSM(_Message, _BlockSize)) 1475 else if (id >= 1070 && id <= 1229) /* MSM */ { 1476 if (DecodeRTCM3MSM(_Message, _BlockSize)) 1492 1477 decoded = true; 1493 1478 } 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; 1479 else { 1480 switch (id) { 1481 case 1001: 1482 case 1003: 1483 emit(newMessage( 1484 QString("%1: Block %2 contain partial data! Ignored!") 1485 .arg(_staID).arg(id).toAscii(), true)); 1486 break; /* no use decoding partial data ATM, remove break when data can be used */ 1487 case 1002: 1488 case 1004: 1489 if (DecodeRTCM3GPS(_Message, _BlockSize)) 1490 decoded = true; 1491 break; 1492 case 1009: 1493 case 1011: 1494 emit(newMessage( 1495 QString("%1: Block %2 contain partial data! Ignored!") 1496 .arg(_staID).arg(id).toAscii(), true)); 1497 break; /* no use decoding partial data ATM, remove break when data can be used */ 1498 case 1010: 1499 case 1012: 1500 if (DecodeRTCM3GLONASS(_Message, _BlockSize)) 1501 decoded = true; 1502 break; 1503 case 1019: 1504 if (DecodeGPSEphemeris(_Message, _BlockSize)) 1505 decoded = true; 1506 break; 1507 case 1020: 1508 if (DecodeGLONASSEphemeris(_Message, _BlockSize)) 1509 decoded = true; 1510 break; 1511 case 1043: 1512 if (DecodeSBASEphemeris(_Message, _BlockSize)) 1513 decoded = true; 1514 break; 1515 case 1044: 1516 if (DecodeQZSSEphemeris(_Message, _BlockSize)) 1517 decoded = true; 1518 break; 1519 case 1045: 1520 case 1046: 1521 if (DecodeGalileoEphemeris(_Message, _BlockSize)) 1522 decoded = true; 1523 break; 1524 case RTCM3ID_BDS: 1525 if (DecodeBDSEphemeris(_Message, _BlockSize)) 1526 decoded = true; 1527 break; 1528 case 1007: 1529 case 1008: 1530 case 1033: 1531 DecodeAntenna(_Message, _BlockSize); 1532 break; 1533 case 1005: 1534 case 1006: 1535 DecodeAntennaPosition(_Message, _BlockSize); 1536 break; 1544 1537 } 1545 1538 } … … 1547 1540 } 1548 1541 return decoded ? success : failure; 1549 }; 1542 } 1543 ; 1550 1544 1551 1545 // 1552 1546 //////////////////////////////////////////////////////////////////////////// 1553 uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) 1554 { 1547 uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) { 1555 1548 uint32_t crc = 0; 1556 1549 int i; 1557 1550 1558 while(size--) 1559 { 1551 while (size--) { 1560 1552 crc ^= (*buf++) << (16); 1561 for (i = 0; i < 8; i++)1562 {1553 for (i = 0; i < 8; i++) 1554 { 1563 1555 crc <<= 1; 1564 if (crc & 0x1000000)1556 if (crc & 0x1000000) 1565 1557 crc ^= 0x01864cfb; 1566 1558 } … … 1571 1563 // 1572 1564 //////////////////////////////////////////////////////////////////////////// 1573 int RTCM3Decoder::GetMessage(void) 1574 { 1565 int RTCM3Decoder::GetMessage(void) { 1575 1566 unsigned char *m, *e; 1576 1567 int i; 1577 1568 1578 m = _Message +_SkipBytes;1579 e = _Message +_MessageSize;1569 m = _Message + _SkipBytes; 1570 e = _Message + _MessageSize; 1580 1571 _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; 1572 while (e - m >= 3) { 1573 if (m[0] == 0xD3) { 1574 _BlockSize = ((m[1] & 3) << 8) | m[2]; 1575 if (e - m >= static_cast<int>(_BlockSize + 6)) { 1576 if (static_cast<uint32_t>((m[3 + _BlockSize] << 16) 1577 | (m[3 + _BlockSize + 1] << 8) 1578 | (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) { 1579 _BlockSize += 6; 1592 1580 _SkipBytes = _BlockSize; 1593 1581 break; … … 1596 1584 ++m; 1597 1585 } 1598 else 1599 { 1586 else { 1600 1587 _NeedBytes = _BlockSize; 1601 1588 break; … … 1605 1592 ++m; 1606 1593 } 1607 if (e-m < 3)1594 if (e - m < 3) 1608 1595 _NeedBytes = 3; 1609 1596 1610 1597 /* copy buffer to front */ 1611 1598 i = m - _Message; 1612 if (i && m < e)1613 memmove(_Message, m, static_cast<size_t>(_MessageSize -i));1599 if (i && m < e) 1600 memmove(_Message, m, static_cast<size_t>(_MessageSize - i)); 1614 1601 _MessageSize -= i; 1615 1602 1616 return !_NeedBytes ? ((_Message[3] <<4)|(_Message[4]>>4)) : 0;1603 return !_NeedBytes ? ((_Message[3] << 4) | (_Message[4] >> 4)) : 0; 1617 1604 } 1618 1605 … … 1620 1607 ////////////////////////////////////////////////////////////////////////////// 1621 1608 int RTCM3Decoder::corrGPSEpochTime() const { 1622 return _coDecoders.size() > 0 ? _coDecoders.begin().value()->corrGPSEpochTime() : -1; 1623 } 1609 return 1610 _coDecoders.size() > 0 ? 1611 _coDecoders.begin().value()->corrGPSEpochTime() : -1; 1612 }
Note:
See TracChangeset
for help on using the changeset viewer.
