[297] | 1 | // Part of BNC, a utility for retrieving decoding and
|
---|
[464] | 2 | // converting GNSS data streams from NTRIP broadcasters.
|
---|
[297] | 3 | //
|
---|
[464] | 4 | // Copyright (C) 2007
|
---|
[297] | 5 | // German Federal Agency for Cartography and Geodesy (BKG)
|
---|
| 6 | // http://www.bkg.bund.de
|
---|
[464] | 7 | // Czech Technical University Prague, Department of Geodesy
|
---|
[297] | 8 | // http://www.fsv.cvut.cz
|
---|
| 9 | //
|
---|
| 10 | // Email: euref-ip@bkg.bund.de
|
---|
| 11 | //
|
---|
| 12 | // This program is free software; you can redistribute it and/or
|
---|
| 13 | // modify it under the terms of the GNU General Public License
|
---|
| 14 | // as published by the Free Software Foundation, version 2.
|
---|
| 15 | //
|
---|
| 16 | // This program is distributed in the hope that it will be useful,
|
---|
| 17 | // but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
| 18 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
---|
| 19 | // GNU General Public License for more details.
|
---|
| 20 | //
|
---|
| 21 | // You should have received a copy of the GNU General Public License
|
---|
| 22 | // along with this program; if not, write to the Free Software
|
---|
| 23 | // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
---|
[296] | 24 |
|
---|
| 25 | /* -------------------------------------------------------------------------
|
---|
| 26 | * BKG NTRIP Client
|
---|
| 27 | * -------------------------------------------------------------------------
|
---|
| 28 | *
|
---|
| 29 | * Class: RTCM3Decoder
|
---|
| 30 | *
|
---|
| 31 | * Purpose: RTCM3 Decoder
|
---|
| 32 | *
|
---|
| 33 | * Author: L. Mervart
|
---|
| 34 | *
|
---|
| 35 | * Created: 24-Aug-2006
|
---|
| 36 | *
|
---|
[7753] | 37 | * Changes:
|
---|
[296] | 38 | *
|
---|
| 39 | * -----------------------------------------------------------------------*/
|
---|
| 40 |
|
---|
| 41 | #include <iostream>
|
---|
[1807] | 42 | #include <iomanip>
|
---|
| 43 | #include <sstream>
|
---|
[296] | 44 | #include <math.h>
|
---|
[585] | 45 | #include <string.h>
|
---|
[296] | 46 |
|
---|
[6812] | 47 | #include "bits.h"
|
---|
| 48 | #include "gnss.h"
|
---|
[296] | 49 | #include "RTCM3Decoder.h"
|
---|
[6812] | 50 | #include "rtcm_utils.h"
|
---|
[296] | 51 | #include "bncconst.h"
|
---|
[5070] | 52 | #include "bnccore.h"
|
---|
[1535] | 53 | #include "bncutils.h"
|
---|
[7753] | 54 | #include "bncsettings.h"
|
---|
[296] | 55 |
|
---|
| 56 | using namespace std;
|
---|
| 57 |
|
---|
[320] | 58 | // Error Handling
|
---|
| 59 | ////////////////////////////////////////////////////////////////////////////
|
---|
[504] | 60 | void RTCM3Error(const char*, ...) {
|
---|
[505] | 61 | }
|
---|
[320] | 62 |
|
---|
[296] | 63 | // Constructor
|
---|
| 64 | ////////////////////////////////////////////////////////////////////////////
|
---|
[7753] | 65 | RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) :
|
---|
[8158] | 66 | GPSDecoder() {
|
---|
[505] | 67 |
|
---|
[8158] | 68 | _staID = staID;
|
---|
[2527] | 69 | _rawFile = rawFile;
|
---|
[2387] | 70 |
|
---|
[8158] | 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)));
|
---|
[939] | 81 |
|
---|
[6812] | 82 | _MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0;
|
---|
[296] | 83 | }
|
---|
| 84 |
|
---|
| 85 | // Destructor
|
---|
| 86 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 87 | RTCM3Decoder::~RTCM3Decoder() {
|
---|
[3001] | 88 | QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders);
|
---|
[8158] | 89 | while (it.hasNext())
|
---|
[6812] | 90 | {
|
---|
[3008] | 91 | it.next();
|
---|
| 92 | delete it.value();
|
---|
[3001] | 93 | }
|
---|
[296] | 94 | }
|
---|
| 95 |
|
---|
[7753] | 96 | //
|
---|
[296] | 97 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 98 | bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size) {
|
---|
[6812] | 99 | bool decoded = false;
|
---|
| 100 | bncTime CurrentObsTime;
|
---|
| 101 | int i, numsats, syncf, type;
|
---|
| 102 | uint64_t numbits = 0, bitfield = 0;
|
---|
[508] | 103 |
|
---|
[6812] | 104 | data += 3; /* header */
|
---|
| 105 | size -= 6; /* header + crc */
|
---|
[1218] | 106 |
|
---|
[6812] | 107 | GETBITS(type, 12)
|
---|
[8158] | 108 | SKIPBITS(12)
|
---|
| 109 | /* id */
|
---|
| 110 | GETBITS(i, 30)
|
---|
[8159] | 111 |
|
---|
[8158] | 112 | CurrentObsTime.set(i);
|
---|
| 113 | if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
|
---|
[6812] | 114 | decoded = true;
|
---|
[8164] | 115 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 116 | _CurrentObsList.clear();
|
---|
[2551] | 117 | }
|
---|
[7753] | 118 |
|
---|
[6812] | 119 | _CurrentTime = CurrentObsTime;
|
---|
[2551] | 120 |
|
---|
[8158] | 121 | GETBITS(syncf, 1)
|
---|
| 122 | /* sync */
|
---|
| 123 | GETBITS(numsats, 5)
|
---|
| 124 | SKIPBITS(4)
|
---|
| 125 | /* smind, smint */
|
---|
[3002] | 126 |
|
---|
[8158] | 127 | while (numsats--) {
|
---|
| 128 | int sv, code, l1range, amb = 0;
|
---|
[6812] | 129 | t_satObs CurrentObs;
|
---|
| 130 | CurrentObs._time = CurrentObsTime;
|
---|
| 131 |
|
---|
| 132 | GETBITS(sv, 6)
|
---|
[8158] | 133 | if (sv < 40)
|
---|
[6812] | 134 | CurrentObs._prn.set('G', sv);
|
---|
| 135 | else
|
---|
[8158] | 136 | CurrentObs._prn.set('S', sv - 20);
|
---|
[6812] | 137 |
|
---|
| 138 | t_frqObs *frqObs = new t_frqObs;
|
---|
| 139 | /* L1 */
|
---|
| 140 | GETBITS(code, 1);
|
---|
[8158] | 141 | (code) ?
|
---|
| 142 | frqObs->_rnxType2ch.assign("1W") : frqObs->_rnxType2ch.assign("1C");
|
---|
[6812] | 143 | GETBITS(l1range, 24);
|
---|
| 144 | GETBITSSIGN(i, 20);
|
---|
[8158] | 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;
|
---|
[6812] | 148 | frqObs->_codeValid = frqObs->_phaseValid = true;
|
---|
[3002] | 149 | }
|
---|
[6812] | 150 | GETBITS(i, 7);
|
---|
[8617] | 151 | frqObs->_lockTime = lti2sec(type,i);
|
---|
| 152 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
|
---|
[6812] | 153 | frqObs->_slipCounter = i;
|
---|
[8158] | 154 | if (type == 1002 || type == 1004) {
|
---|
| 155 | GETBITS(amb, 8);
|
---|
| 156 | if (amb) {
|
---|
| 157 | frqObs->_code += amb * 299792.458;
|
---|
| 158 | frqObs->_phase += (amb * 299792.458) / GPS_WAVELENGTH_L1;
|
---|
[1021] | 159 | }
|
---|
[6812] | 160 | GETBITS(i, 8);
|
---|
[8158] | 161 | if (i) {
|
---|
| 162 | frqObs->_snr = i * 0.25;
|
---|
[6812] | 163 | frqObs->_snrValid = true;
|
---|
| 164 | }
|
---|
[1021] | 165 | }
|
---|
[6812] | 166 | CurrentObs._obs.push_back(frqObs);
|
---|
[8158] | 167 | if (type == 1003 || type == 1004) {
|
---|
[6812] | 168 | frqObs = new t_frqObs;
|
---|
| 169 | /* L2 */
|
---|
[8158] | 170 | GETBITS(code, 2);
|
---|
| 171 | switch (code) {
|
---|
| 172 | case 3:
|
---|
| 173 | frqObs->_rnxType2ch.assign("2W"); /* or "2Y"? */
|
---|
| 174 | break;
|
---|
| 175 | case 2:
|
---|
| 176 | frqObs->_rnxType2ch.assign("2W");
|
---|
| 177 | break;
|
---|
| 178 | case 1:
|
---|
| 179 | frqObs->_rnxType2ch.assign("2P");
|
---|
| 180 | break;
|
---|
| 181 | case 0:
|
---|
| 182 | frqObs->_rnxType2ch.assign("2X"); /* or "2S" or "2L"? */
|
---|
| 183 | break;
|
---|
[6812] | 184 | }
|
---|
[8158] | 185 | GETBITSSIGN(i, 14);
|
---|
| 186 | if ((i & ((1 << 14) - 1)) != 0x2000) {
|
---|
| 187 | frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 299792.458;
|
---|
[6812] | 188 | frqObs->_codeValid = true;
|
---|
| 189 | }
|
---|
[8158] | 190 | GETBITSSIGN(i, 20);
|
---|
| 191 | if ((i & ((1 << 20) - 1)) != 0x80000) {
|
---|
| 192 | frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 299792.458)
|
---|
| 193 | / GPS_WAVELENGTH_L2;
|
---|
[6812] | 194 | frqObs->_phaseValid = true;
|
---|
| 195 | }
|
---|
[8158] | 196 | GETBITS(i, 7);
|
---|
[8617] | 197 | frqObs->_lockTime = lti2sec(type,i);
|
---|
[8682] | 198 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
|
---|
[6812] | 199 | frqObs->_slipCounter = i;
|
---|
[8158] | 200 | if (type == 1004) {
|
---|
[6812] | 201 | GETBITS(i, 8);
|
---|
[8158] | 202 | if (i) {
|
---|
| 203 | frqObs->_snr = i * 0.25;
|
---|
[6812] | 204 | frqObs->_snrValid = true;
|
---|
| 205 | }
|
---|
| 206 | }
|
---|
| 207 | CurrentObs._obs.push_back(frqObs);
|
---|
| 208 | }
|
---|
| 209 | _CurrentObsList.push_back(CurrentObs);
|
---|
[913] | 210 | }
|
---|
[7753] | 211 |
|
---|
[8158] | 212 | if (!syncf) {
|
---|
[6812] | 213 | decoded = true;
|
---|
[7753] | 214 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 215 | _CurrentTime.reset();
|
---|
| 216 | _CurrentObsList.clear();
|
---|
[2527] | 217 | }
|
---|
[6812] | 218 | return decoded;
|
---|
| 219 | }
|
---|
[2527] | 220 |
|
---|
[6812] | 221 | #define RTCM3_MSM_NUMSIG 32
|
---|
| 222 | #define RTCM3_MSM_NUMSAT 64
|
---|
| 223 | #define RTCM3_MSM_NUMCELLS 96 /* arbitrary limit */
|
---|
[1127] | 224 |
|
---|
[6812] | 225 | /**
|
---|
| 226 | * Frequency numbers of GLONASS with an offset of 100 to detect unset values.
|
---|
| 227 | * Gets filled by ephemeris and data blocks and shared between different streams.
|
---|
| 228 | */
|
---|
| 229 | static int GLOFreq[RTCM3_MSM_NUMSAT];
|
---|
[1127] | 230 |
|
---|
[6812] | 231 | /*
|
---|
| 232 | * Storage structure to store frequency and RINEX ID assignment for MSM
|
---|
| 233 | * message */
|
---|
| 234 | struct CodeData {
|
---|
| 235 | double wl;
|
---|
| 236 | const char *code; /* currently unused */
|
---|
| 237 | };
|
---|
[2677] | 238 |
|
---|
[6812] | 239 | /** MSM signal types for GPS and SBAS */
|
---|
[8158] | 240 | static struct CodeData gps[RTCM3_MSM_NUMSIG] = {
|
---|
| 241 | {0.0, 0},
|
---|
| 242 | {GPS_WAVELENGTH_L1, "1C"},
|
---|
| 243 | {GPS_WAVELENGTH_L1, "1P"},
|
---|
| 244 | {GPS_WAVELENGTH_L1, "1W"},
|
---|
| 245 | {0.0, 0}/*{GPS_WAVELENGTH_L1,"1Y"}*/,
|
---|
| 246 | {0.0, 0},
|
---|
| 247 | {0.0, 0},
|
---|
| 248 | {GPS_WAVELENGTH_L2, "2C"},
|
---|
| 249 | {GPS_WAVELENGTH_L2, "2P"},
|
---|
| 250 | {GPS_WAVELENGTH_L2, "2W"},
|
---|
| 251 | {0.0, 0}/*{GPS_WAVELENGTH_L2,"2Y"}*/,
|
---|
| 252 | {0.0, 0},
|
---|
| 253 | {0.0, 0},
|
---|
| 254 | {0.0, 0},
|
---|
| 255 | {GPS_WAVELENGTH_L2, "2S"},
|
---|
| 256 | {GPS_WAVELENGTH_L2, "2L"},
|
---|
| 257 | {GPS_WAVELENGTH_L2, "2X"},
|
---|
| 258 | {0.0, 0},
|
---|
| 259 | {0.0, 0},
|
---|
| 260 | {0.0, 0},
|
---|
| 261 | {0.0, 0},
|
---|
| 262 | {GPS_WAVELENGTH_L5, "5I"},
|
---|
| 263 | {GPS_WAVELENGTH_L5, "5Q"},
|
---|
| 264 | {GPS_WAVELENGTH_L5, "5X"},
|
---|
| 265 | {0.0, 0},
|
---|
| 266 | {0.0, 0},
|
---|
| 267 | {0.0, 0},
|
---|
| 268 | {0.0, 0},
|
---|
| 269 | {0.0, 0},
|
---|
| 270 | {GPS_WAVELENGTH_L1, "1S"},
|
---|
| 271 | {GPS_WAVELENGTH_L1, "1L"},
|
---|
| 272 | {GPS_WAVELENGTH_L1, "1X"}
|
---|
| 273 | };
|
---|
[1185] | 274 |
|
---|
[6812] | 275 | /**
|
---|
| 276 | * MSM signal types for GLONASS
|
---|
[7753] | 277 | *
|
---|
[6812] | 278 | * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!
|
---|
| 279 | */
|
---|
[8158] | 280 | static struct CodeData glo[RTCM3_MSM_NUMSIG] = {
|
---|
| 281 | {0.0, 0},
|
---|
| 282 | {0.0, "1C"},
|
---|
| 283 | {0.0, "1P"},
|
---|
| 284 | {0.0, 0},
|
---|
| 285 | {0.0, 0},
|
---|
| 286 | {0.0, 0},
|
---|
| 287 | {0.0, 0},
|
---|
| 288 | {1.0, "2C"},
|
---|
| 289 | {1.0, "2P"},
|
---|
| 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 | {0.0, 0},
|
---|
| 310 | {0.0, 0},
|
---|
| 311 | {0.0, 0},
|
---|
| 312 | {0.0, 0}
|
---|
| 313 | };
|
---|
[1130] | 314 |
|
---|
[6812] | 315 | /** MSM signal types for Galileo */
|
---|
[8158] | 316 | static struct CodeData gal[RTCM3_MSM_NUMSIG] = {
|
---|
| 317 | {0.0, 0},
|
---|
| 318 | {GAL_WAVELENGTH_E1, "1C"},
|
---|
| 319 | {GAL_WAVELENGTH_E1, "1A"},
|
---|
| 320 | {GAL_WAVELENGTH_E1, "1B"},
|
---|
| 321 | {GAL_WAVELENGTH_E1, "1X"},
|
---|
| 322 | {GAL_WAVELENGTH_E1, "1Z"},
|
---|
| 323 | {0.0, 0},
|
---|
| 324 | {GAL_WAVELENGTH_E6, "6C"},
|
---|
| 325 | {GAL_WAVELENGTH_E6, "6A"},
|
---|
| 326 | {GAL_WAVELENGTH_E6, "6B"},
|
---|
| 327 | {GAL_WAVELENGTH_E6, "6X"},
|
---|
| 328 | {GAL_WAVELENGTH_E6, "6Z"},
|
---|
| 329 | {0.0, 0},
|
---|
| 330 | {GAL_WAVELENGTH_E5B, "7I"},
|
---|
| 331 | {GAL_WAVELENGTH_E5B, "7Q"},
|
---|
| 332 | {GAL_WAVELENGTH_E5B, "7X"},
|
---|
| 333 | {0.0, 0},
|
---|
| 334 | {GAL_WAVELENGTH_E5AB, "8I"},
|
---|
| 335 | {GAL_WAVELENGTH_E5AB, "8Q"},
|
---|
| 336 | {GAL_WAVELENGTH_E5AB, "8X"},
|
---|
| 337 | {0.0, 0},
|
---|
| 338 | {GAL_WAVELENGTH_E5A, "5I"},
|
---|
| 339 | {GAL_WAVELENGTH_E5A, "5Q"},
|
---|
| 340 | {GAL_WAVELENGTH_E5A, "5X"},
|
---|
| 341 | {0.0, 0},
|
---|
| 342 | {0.0, 0},
|
---|
| 343 | {0.0, 0},
|
---|
| 344 | {0.0, 0},
|
---|
| 345 | {0.0, 0},
|
---|
| 346 | {0.0, 0},
|
---|
| 347 | {0.0, 0},
|
---|
| 348 | {0.0, 0},
|
---|
| 349 | };
|
---|
[1185] | 350 |
|
---|
[6812] | 351 | /** MSM signal types for QZSS */
|
---|
[8158] | 352 | static struct CodeData qzss[RTCM3_MSM_NUMSIG] = {
|
---|
| 353 | {0.0, 0},
|
---|
| 354 | {GPS_WAVELENGTH_L1, "1C"},
|
---|
| 355 | {0.0, 0},
|
---|
| 356 | {0.0, 0},
|
---|
| 357 | {0.0, 0},
|
---|
[8682] | 358 | {GPS_WAVELENGTH_L1, "1Z"},// not defined in RTCM3.3?
|
---|
[8158] | 359 | {0.0, 0},
|
---|
| 360 | {0.0, 0},
|
---|
[8682] | 361 | {QZSS_WAVELENGTH_L6, "6S"},
|
---|
| 362 | {QZSS_WAVELENGTH_L6, "6L"},
|
---|
| 363 | {QZSS_WAVELENGTH_L6, "6X"},
|
---|
[8158] | 364 | {0.0, 0},
|
---|
| 365 | {0.0, 0},
|
---|
| 366 | {0.0, 0},
|
---|
| 367 | {GPS_WAVELENGTH_L2, "2S"},
|
---|
| 368 | {GPS_WAVELENGTH_L2, "2L"},
|
---|
| 369 | {GPS_WAVELENGTH_L2, "2X"},
|
---|
| 370 | {0.0, 0},
|
---|
| 371 | {0.0, 0},
|
---|
| 372 | {0.0, 0},
|
---|
| 373 | {0.0, 0},
|
---|
| 374 | {GPS_WAVELENGTH_L5, "5I"},
|
---|
| 375 | {GPS_WAVELENGTH_L5, "5Q"},
|
---|
| 376 | {GPS_WAVELENGTH_L5, "5X"},
|
---|
| 377 | {0.0, 0},
|
---|
| 378 | {0.0, 0},
|
---|
| 379 | {0.0, 0},
|
---|
| 380 | {0.0, 0},
|
---|
| 381 | {0.0, 0},
|
---|
[8682] | 382 | {GPS_WAVELENGTH_L1, "1S"},
|
---|
| 383 | {GPS_WAVELENGTH_L1, "1L"},
|
---|
[8158] | 384 | {GPS_WAVELENGTH_L1, "1X"}
|
---|
| 385 | };
|
---|
[1185] | 386 |
|
---|
[6812] | 387 | /** MSM signal types for Beidou/BDS */
|
---|
[8158] | 388 | static struct CodeData bds[RTCM3_MSM_NUMSIG] = {
|
---|
| 389 | {0.0, 0},
|
---|
| 390 | {BDS_WAVELENGTH_B1, "2I"},
|
---|
[8682] | 391 | {BDS_WAVELENGTH_B1, "2Q"},
|
---|
| 392 | {BDS_WAVELENGTH_B1, "2X"},
|
---|
[8158] | 393 | {0.0, 0},
|
---|
| 394 | {0.0, 0},
|
---|
| 395 | {0.0, 0},
|
---|
| 396 | {BDS_WAVELENGTH_B3, "6I"},
|
---|
[8682] | 397 | {BDS_WAVELENGTH_B3, "6Q"},
|
---|
| 398 | {BDS_WAVELENGTH_B3, "6X"},
|
---|
[8158] | 399 | {0.0, 0},
|
---|
| 400 | {0.0, 0},
|
---|
| 401 | {0.0, 0},
|
---|
| 402 | {BDS_WAVELENGTH_B2, "7I"},
|
---|
[8682] | 403 | {BDS_WAVELENGTH_B2, "7Q"},
|
---|
| 404 | {BDS_WAVELENGTH_B2, "7X"},
|
---|
[8158] | 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 | {0.0, 0},
|
---|
| 419 | {0.0, 0},
|
---|
| 420 | {0.0, 0},
|
---|
| 421 | };
|
---|
[1807] | 422 |
|
---|
[8783] | 423 | /**
|
---|
| 424 | * MSM signal types for IRNSS
|
---|
| 425 | *
|
---|
| 426 | * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!
|
---|
| 427 | */
|
---|
| 428 | static struct CodeData irn[RTCM3_MSM_NUMSIG] = {
|
---|
| 429 | {0.0, 0},
|
---|
| 430 | {0.0, 0},
|
---|
| 431 | {0.0, 0},
|
---|
| 432 | {0.0, 0},
|
---|
| 433 | {0.0, 0},
|
---|
| 434 | {0.0, 0},
|
---|
| 435 | {0.0, 0},
|
---|
| 436 | {IRNSS_WAVELENGTH_S, "9A"},
|
---|
| 437 | {0.0, 0},
|
---|
| 438 | {0.0, 0},
|
---|
| 439 | {0.0, 0},
|
---|
| 440 | {0.0, 0},
|
---|
| 441 | {0.0, 0},
|
---|
| 442 | {0.0, 0},
|
---|
| 443 | {0.0, 0},
|
---|
| 444 | {0.0, 0},
|
---|
| 445 | {0.0, 0},
|
---|
| 446 | {0.0, 0},
|
---|
| 447 | {0.0, 0},
|
---|
| 448 | {0.0, 0},
|
---|
| 449 | {0.0, 0},
|
---|
| 450 | {IRNSS_WAVELENGTH_L5, "5A"},
|
---|
| 451 | {0.0, 0},
|
---|
| 452 | {0.0, 0},
|
---|
| 453 | {0.0, 0},
|
---|
| 454 | {0.0, 0},
|
---|
| 455 | {0.0, 0},
|
---|
| 456 | {0.0, 0},
|
---|
| 457 | {0.0, 0},
|
---|
| 458 | {0.0, 0},
|
---|
| 459 | {0.0, 0},
|
---|
| 460 | {0.0, 0}
|
---|
| 461 | };
|
---|
| 462 |
|
---|
[6812] | 463 | #define UINT64(c) c ## ULL
|
---|
[1033] | 464 |
|
---|
[7753] | 465 | //
|
---|
[6812] | 466 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 467 | bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size)
|
---|
[8158] | 468 | {
|
---|
[6812] | 469 | bool decoded = false;
|
---|
| 470 | int type, syncf, i;
|
---|
| 471 | uint64_t numbits = 0, bitfield = 0;
|
---|
[1807] | 472 |
|
---|
[6812] | 473 | data += 3; /* header */
|
---|
| 474 | size -= 6; /* header + crc */
|
---|
[1239] | 475 |
|
---|
[6812] | 476 | GETBITS(type, 12)
|
---|
[8158] | 477 | SKIPBITS(12)
|
---|
| 478 | /* id */
|
---|
[6812] | 479 | char sys;
|
---|
[8783] | 480 | if (type >= 1121)
|
---|
[6812] | 481 | sys = 'C';
|
---|
[8158] | 482 | else if (type >= 1111)
|
---|
[6812] | 483 | sys = 'J';
|
---|
[8158] | 484 | else if (type >= 1101)
|
---|
[6812] | 485 | sys = 'S';
|
---|
[8158] | 486 | else if (type >= 1091)
|
---|
[6812] | 487 | sys = 'E';
|
---|
[8158] | 488 | else if (type >= 1081)
|
---|
[6812] | 489 | sys = 'R';
|
---|
[8783] | 490 | else if (type >= 1071)
|
---|
[6812] | 491 | sys = 'G';
|
---|
[8783] | 492 | else if (type >= 21) // test
|
---|
| 493 | sys = 'I';
|
---|
[2683] | 494 |
|
---|
[6812] | 495 | bncTime CurrentObsTime;
|
---|
[8783] | 496 | if (sys == 'C') /* BDS */ {
|
---|
[8158] | 497 | GETBITS(i, 30)
|
---|
[6812] | 498 | CurrentObsTime.setBDS(i);
|
---|
| 499 | }
|
---|
[8158] | 500 | else if (sys == 'R') /* GLONASS */ {
|
---|
[6812] | 501 | SKIPBITS(3)
|
---|
[8158] | 502 | GETBITS(i, 27)
|
---|
| 503 | /* tk */
|
---|
[6812] | 504 | CurrentObsTime.setTk(i);
|
---|
| 505 | }
|
---|
[8158] | 506 | else /* GPS style date */ {
|
---|
| 507 | GETBITS(i, 30)
|
---|
[6812] | 508 | CurrentObsTime.set(i);
|
---|
| 509 | }
|
---|
[8158] | 510 | if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
|
---|
[6812] | 511 | decoded = true;
|
---|
[8160] | 512 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 513 | _CurrentObsList.clear();
|
---|
| 514 | }
|
---|
| 515 | _CurrentTime = CurrentObsTime;
|
---|
[2677] | 516 |
|
---|
[6812] | 517 | GETBITS(syncf, 1)
|
---|
| 518 | /**
|
---|
| 519 | * Ignore unknown types except for sync flag
|
---|
[7753] | 520 | *
|
---|
[6812] | 521 | * We actually support types 1-3 in following code, but as they are missing
|
---|
| 522 | * the full cycles and can't be used later we skip interpretation here already.
|
---|
| 523 | */
|
---|
[8158] | 524 | if (type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7) {
|
---|
[6812] | 525 | int sigmask, numsat = 0, numsig = 0;
|
---|
| 526 | uint64_t satmask, cellmask, ui;
|
---|
| 527 | double rrmod[RTCM3_MSM_NUMSAT];
|
---|
| 528 | int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT],
|
---|
[8158] | 529 | extsat[RTCM3_MSM_NUMSAT];
|
---|
[6812] | 530 | int ll[RTCM3_MSM_NUMCELLS]/*, hc[RTCM3_MSM_NUMCELLS]*/;
|
---|
| 531 | double cnr[RTCM3_MSM_NUMCELLS];
|
---|
| 532 | double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS],
|
---|
[8158] | 533 | dop[RTCM3_MSM_NUMCELLS];
|
---|
[2674] | 534 |
|
---|
[8158] | 535 | SKIPBITS(3 + 7 + 2 + 2 + 1 + 3)
|
---|
[6812] | 536 | GETBITS64(satmask, RTCM3_MSM_NUMSAT)
|
---|
[2674] | 537 |
|
---|
[6812] | 538 | /* http://gurmeetsingh.wordpress.com/2008/08/05/fast-bit-counting-routines/ */
|
---|
[8158] | 539 | for (ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */)
|
---|
[6812] | 540 | ++numsat;
|
---|
| 541 | GETBITS(sigmask, RTCM3_MSM_NUMSIG)
|
---|
[8158] | 542 | for (i = sigmask; i; i &= (i - 1) /* remove rightmost bit */)
|
---|
[6812] | 543 | ++numsig;
|
---|
[8158] | 544 | for (i = 0; i < RTCM3_MSM_NUMSAT; ++i)
|
---|
[6812] | 545 | extsat[i] = 15;
|
---|
[2674] | 546 |
|
---|
[8158] | 547 | i = numsat * numsig;
|
---|
| 548 | GETBITS64(cellmask, (unsigned )i)
|
---|
[6812] | 549 |
|
---|
[8158] | 550 | switch (type % 10) {
|
---|
[6812] | 551 | case 1:
|
---|
| 552 | case 2:
|
---|
| 553 | case 3:
|
---|
[8158] | 554 | /* partial data, already skipped above, but implemented for future expansion ! */
|
---|
| 555 | for (int j = numsat; j--;)
|
---|
| 556 | GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
|
---|
[6812] | 557 | break;
|
---|
| 558 | case 4:
|
---|
[8158] | 559 | case 6:
|
---|
| 560 | for (int j = numsat; j--;)
|
---|
| 561 | GETBITS(rrint[j], 8)
|
---|
| 562 | for (int j = numsat; j--;)
|
---|
| 563 | GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
|
---|
[6812] | 564 | break;
|
---|
| 565 | case 5:
|
---|
| 566 | case 7:
|
---|
[8158] | 567 | for (int j = numsat; j--;)
|
---|
| 568 | GETBITS(rrint[j], 8)
|
---|
| 569 | for (int j = numsat; j--;)
|
---|
| 570 | GETBITS(extsat[j], 4)
|
---|
| 571 | for (int j = numsat; j--;)
|
---|
| 572 | GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
|
---|
| 573 | for (int j = numsat; j--;)
|
---|
| 574 | GETBITSSIGN(rdop[j], 14)
|
---|
[6812] | 575 | break;
|
---|
[8158] | 576 | }
|
---|
| 577 |
|
---|
| 578 | int numcells = numsat * numsig;
|
---|
| 579 | /** Drop anything which exceeds our cell limit. Increase limit definition
|
---|
| 580 | * when that happens. */
|
---|
| 581 | if (numcells <= RTCM3_MSM_NUMCELLS) {
|
---|
| 582 | switch (type % 10) {
|
---|
| 583 | case 1:
|
---|
| 584 | for (int count = numcells; count--;)
|
---|
| 585 | if (cellmask & (UINT64(1) << count))
|
---|
| 586 | GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
|
---|
| 587 | break;
|
---|
| 588 | case 2:
|
---|
| 589 | for (int count = numcells; count--;)
|
---|
| 590 | if (cellmask & (UINT64(1) << count))
|
---|
| 591 | GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
|
---|
| 592 | for (int count = numcells; count--;)
|
---|
| 593 | if (cellmask & (UINT64(1) << count))
|
---|
| 594 | GETBITS(ll[count], 4)
|
---|
| 595 | for (int count = numcells; count--;)
|
---|
| 596 | if (cellmask & (UINT64(1) << count))
|
---|
| 597 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 598 | break;
|
---|
| 599 | case 3:
|
---|
| 600 | for (int count = numcells; count--;)
|
---|
| 601 | if (cellmask & (UINT64(1) << count))
|
---|
| 602 | GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
|
---|
| 603 | for (int count = numcells; count--;)
|
---|
| 604 | if (cellmask & (UINT64(1) << count))
|
---|
| 605 | GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
|
---|
| 606 | for (int count = numcells; count--;)
|
---|
| 607 | if (cellmask & (UINT64(1) << count))
|
---|
| 608 | GETBITS(ll[count], 4)
|
---|
| 609 | for (int count = numcells; count--;)
|
---|
| 610 | if (cellmask & (UINT64(1) << count))
|
---|
| 611 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 612 | break;
|
---|
| 613 | case 4:
|
---|
| 614 | for (int count = numcells; count--;)
|
---|
| 615 | if (cellmask & (UINT64(1) << count))
|
---|
| 616 | GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
|
---|
| 617 | for (int count = numcells; count--;)
|
---|
| 618 | if (cellmask & (UINT64(1) << count))
|
---|
| 619 | GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
|
---|
| 620 | for (int count = numcells; count--;)
|
---|
| 621 | if (cellmask & (UINT64(1) << count))
|
---|
| 622 | GETBITS(ll[count], 4)
|
---|
| 623 | for (int count = numcells; count--;)
|
---|
| 624 | if (cellmask & (UINT64(1) << count))
|
---|
| 625 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 626 | for (int count = numcells; count--;)
|
---|
| 627 | if (cellmask & (UINT64(1) << count))
|
---|
| 628 | GETBITS(cnr[count], 6)
|
---|
| 629 | break;
|
---|
| 630 | case 5:
|
---|
| 631 | for (int count = numcells; count--;)
|
---|
| 632 | if (cellmask & (UINT64(1) << count))
|
---|
| 633 | GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
|
---|
| 634 | for (int count = numcells; count--;)
|
---|
| 635 | if (cellmask & (UINT64(1) << count))
|
---|
| 636 | GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
|
---|
| 637 | for (int count = numcells; count--;)
|
---|
| 638 | if (cellmask & (UINT64(1) << count))
|
---|
| 639 | GETBITS(ll[count], 4)
|
---|
| 640 | for (int count = numcells; count--;)
|
---|
| 641 | if (cellmask & (UINT64(1) << count))
|
---|
| 642 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 643 | for (int count = numcells; count--;)
|
---|
| 644 | if (cellmask & (UINT64(1) << count))
|
---|
| 645 | GETFLOAT(cnr[count], 6, 1.0)
|
---|
| 646 | for (int count = numcells; count--;)
|
---|
| 647 | if (cellmask & (UINT64(1) << count))
|
---|
| 648 | GETFLOATSIGN(dop[count], 15, 0.0001)
|
---|
| 649 | break;
|
---|
| 650 | case 6:
|
---|
| 651 | for (int count = numcells; count--;)
|
---|
| 652 | if (cellmask & (UINT64(1) << count))
|
---|
| 653 | GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29))
|
---|
| 654 | for (int count = numcells; count--;)
|
---|
| 655 | if (cellmask & (UINT64(1) << count))
|
---|
| 656 | GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31))
|
---|
| 657 | for (int count = numcells; count--;)
|
---|
| 658 | if (cellmask & (UINT64(1) << count))
|
---|
| 659 | GETBITS(ll[count], 10)
|
---|
| 660 | for (int count = numcells; count--;)
|
---|
| 661 | if (cellmask & (UINT64(1) << count))
|
---|
| 662 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 663 | for (int count = numcells; count--;)
|
---|
| 664 | if (cellmask & (UINT64(1) << count))
|
---|
| 665 | GETFLOAT(cnr[count], 10, 1.0 / (1 << 4))
|
---|
| 666 | break;
|
---|
| 667 | case 7:
|
---|
| 668 | for (int count = numcells; count--;)
|
---|
| 669 | if (cellmask & (UINT64(1) << count))
|
---|
| 670 | GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29))
|
---|
| 671 | for (int count = numcells; count--;)
|
---|
| 672 | if (cellmask & (UINT64(1) << count))
|
---|
| 673 | GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31))
|
---|
| 674 | for (int count = numcells; count--;)
|
---|
| 675 | if (cellmask & (UINT64(1) << count))
|
---|
| 676 | GETBITS(ll[count], 10)
|
---|
| 677 | for (int count = numcells; count--;)
|
---|
| 678 | if (cellmask & (UINT64(1) << count))
|
---|
| 679 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 680 | for (int count = numcells; count--;)
|
---|
| 681 | if (cellmask & (UINT64(1) << count))
|
---|
| 682 | GETFLOAT(cnr[count], 10, 1.0 / (1 << 4))
|
---|
| 683 | for (int count = numcells; count--;)
|
---|
| 684 | if (cellmask & (UINT64(1) << count))
|
---|
| 685 | GETFLOATSIGN(dop[count], 15, 0.0001)
|
---|
| 686 | break;
|
---|
[6812] | 687 | }
|
---|
| 688 | i = RTCM3_MSM_NUMSAT;
|
---|
| 689 | int j = -1;
|
---|
| 690 | t_satObs CurrentObs;
|
---|
[8158] | 691 | for (int count = numcells; count--;) {
|
---|
| 692 | while (j >= 0 && !(sigmask & (1 << --j)))
|
---|
[6812] | 693 | ;
|
---|
[8158] | 694 | if (j < 0) {
|
---|
| 695 | while (!(satmask & (UINT64(1) << (--i))))
|
---|
| 696 | /* next satellite */
|
---|
[6812] | 697 | ;
|
---|
[8158] | 698 | if (CurrentObs._obs.size() > 0)
|
---|
[6812] | 699 | _CurrentObsList.push_back(CurrentObs);
|
---|
| 700 | CurrentObs.clear();
|
---|
| 701 | CurrentObs._time = CurrentObsTime;
|
---|
[8158] | 702 | if (sys == 'S')
|
---|
| 703 | CurrentObs._prn.set(sys, 20 - 1 + RTCM3_MSM_NUMSAT - i);
|
---|
[6812] | 704 | else
|
---|
[8158] | 705 | CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT - i);
|
---|
[6812] | 706 | j = RTCM3_MSM_NUMSIG;
|
---|
[8158] | 707 | while (!(sigmask & (1 << --j)))
|
---|
[6812] | 708 | ;
|
---|
| 709 | --numsat;
|
---|
| 710 | }
|
---|
[8158] | 711 | if (cellmask & (UINT64(1) << count)) {
|
---|
| 712 | struct CodeData cd = {0.0, 0};
|
---|
| 713 | switch (sys) {
|
---|
| 714 | case 'J':
|
---|
| 715 | cd = qzss[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 716 | break;
|
---|
| 717 | case 'C':
|
---|
| 718 | cd = bds[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 719 | break;
|
---|
| 720 | case 'G':
|
---|
| 721 | case 'S':
|
---|
| 722 | cd = gps[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 723 | break;
|
---|
| 724 | case 'R':
|
---|
| 725 | cd = glo[RTCM3_MSM_NUMSIG - j - 1];
|
---|
[6812] | 726 | {
|
---|
[8158] | 727 | int k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1];
|
---|
| 728 | if (extsat[numsat] < 14) {
|
---|
| 729 | k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1] = 100 + extsat[numsat]
|
---|
| 730 | - 7;
|
---|
| 731 | }
|
---|
| 732 | if (k)
|
---|
| 733 | cd.wl = (
|
---|
| 734 | cd.wl == 0.0 ?
|
---|
| 735 | GLO_WAVELENGTH_L1(k - 100) :
|
---|
| 736 | GLO_WAVELENGTH_L2(k - 100));
|
---|
| 737 | else
|
---|
| 738 | cd.code = 0;
|
---|
[2676] | 739 | }
|
---|
[8158] | 740 | break;
|
---|
| 741 | case 'E':
|
---|
| 742 | cd = gal[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 743 | break;
|
---|
[8783] | 744 | case 'I':
|
---|
| 745 | cd = irn[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 746 | break;
|
---|
[6812] | 747 | }
|
---|
[8158] | 748 | if (cd.code) {
|
---|
[6812] | 749 | t_frqObs *frqObs = new t_frqObs;
|
---|
[7850] | 750 | frqObs->_rnxType2ch.assign(cd.code);
|
---|
[2674] | 751 |
|
---|
[8158] | 752 | switch (type % 10) {
|
---|
| 753 | case 1:
|
---|
| 754 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 755 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
| 756 | + (rrmod[numsat]) * LIGHTSPEED / 1000.0;
|
---|
| 757 | frqObs->_codeValid = true;
|
---|
| 758 | }
|
---|
| 759 | break;
|
---|
| 760 | case 2:
|
---|
| 761 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 762 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
| 763 | + (rrmod[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
|
---|
| 764 | frqObs->_phaseValid = true;
|
---|
[8617] | 765 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 766 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[8158] | 767 | frqObs->_slipCounter = ll[count];
|
---|
| 768 | }
|
---|
| 769 | break;
|
---|
| 770 | case 3:
|
---|
| 771 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 772 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
| 773 | + (rrmod[numsat]) * LIGHTSPEED / 1000.0;
|
---|
| 774 | frqObs->_codeValid = true;
|
---|
| 775 | }
|
---|
[2669] | 776 |
|
---|
[8158] | 777 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 778 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
| 779 | + rrmod[numsat] * LIGHTSPEED / 1000.0 / cd.wl;
|
---|
| 780 | frqObs->_phaseValid = true;
|
---|
[8617] | 781 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 782 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[8158] | 783 | frqObs->_slipCounter = ll[count];
|
---|
| 784 | }
|
---|
| 785 | break;
|
---|
| 786 | case 4:
|
---|
| 787 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 788 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
| 789 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
|
---|
| 790 | frqObs->_codeValid = true;
|
---|
| 791 | }
|
---|
[2674] | 792 |
|
---|
[8158] | 793 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 794 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
| 795 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
|
---|
| 796 | / cd.wl;
|
---|
| 797 | frqObs->_phaseValid = true;
|
---|
[8617] | 798 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 799 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[8158] | 800 | frqObs->_slipCounter = ll[count];
|
---|
| 801 | }
|
---|
[4368] | 802 |
|
---|
[8158] | 803 | frqObs->_snr = cnr[count];
|
---|
| 804 | frqObs->_snrValid = true;
|
---|
| 805 | break;
|
---|
| 806 | case 5:
|
---|
| 807 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 808 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
| 809 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
|
---|
| 810 | frqObs->_codeValid = true;
|
---|
| 811 | }
|
---|
[4368] | 812 |
|
---|
[8158] | 813 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 814 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
| 815 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
|
---|
| 816 | / cd.wl;
|
---|
| 817 | frqObs->_phaseValid = true;
|
---|
[8617] | 818 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 819 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[8158] | 820 | frqObs->_slipCounter = ll[count];
|
---|
| 821 | }
|
---|
[2669] | 822 |
|
---|
[8158] | 823 | frqObs->_snr = cnr[count];
|
---|
| 824 | frqObs->_snrValid = true;
|
---|
[1807] | 825 |
|
---|
[8158] | 826 | if (dop[count] > -1.6384) {
|
---|
| 827 | frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl;
|
---|
| 828 | frqObs->_dopplerValid = true;
|
---|
| 829 | }
|
---|
| 830 | break;
|
---|
| 831 | case 6:
|
---|
| 832 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 833 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
| 834 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
|
---|
| 835 | frqObs->_codeValid = true;
|
---|
| 836 | }
|
---|
[6812] | 837 |
|
---|
[8158] | 838 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 839 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
| 840 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
|
---|
| 841 | / cd.wl;
|
---|
| 842 | frqObs->_phaseValid = true;
|
---|
[8617] | 843 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 844 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[8158] | 845 | frqObs->_slipCounter = ll[count];
|
---|
| 846 | }
|
---|
[6812] | 847 |
|
---|
[8158] | 848 | frqObs->_snr = cnr[count];
|
---|
| 849 | frqObs->_snrValid = true;
|
---|
| 850 | break;
|
---|
| 851 | case 7:
|
---|
| 852 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 853 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
| 854 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
|
---|
| 855 | frqObs->_codeValid = true;
|
---|
| 856 | }
|
---|
[2687] | 857 |
|
---|
[8158] | 858 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 859 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
| 860 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
|
---|
| 861 | / cd.wl;
|
---|
| 862 | frqObs->_phaseValid = true;
|
---|
[8617] | 863 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 864 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[8158] | 865 | frqObs->_slipCounter = ll[count];
|
---|
| 866 | }
|
---|
[6137] | 867 |
|
---|
[8158] | 868 | frqObs->_snr = cnr[count];
|
---|
| 869 | frqObs->_snrValid = true;
|
---|
[6137] | 870 |
|
---|
[8158] | 871 | if (dop[count] > -1.6384) {
|
---|
| 872 | frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl;
|
---|
| 873 | frqObs->_dopplerValid = true;
|
---|
| 874 | }
|
---|
| 875 | break;
|
---|
[366] | 876 | }
|
---|
[6812] | 877 | CurrentObs._obs.push_back(frqObs);
|
---|
[296] | 878 | }
|
---|
[6812] | 879 | }
|
---|
| 880 | }
|
---|
[8159] | 881 | if (CurrentObs._obs.size() > 0)
|
---|
[6812] | 882 | _CurrentObsList.push_back(CurrentObs);
|
---|
| 883 | }
|
---|
| 884 | }
|
---|
[8158] | 885 | else if ((type % 10) < 3) {
|
---|
[6812] | 886 | emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
|
---|
[8204] | 887 | .arg(_staID).arg(type).toLatin1(), true));
|
---|
[6812] | 888 | }
|
---|
[8158] | 889 | if (!syncf) {
|
---|
[6812] | 890 | decoded = true;
|
---|
[8166] | 891 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 892 | _CurrentTime.reset();
|
---|
| 893 | _CurrentObsList.clear();
|
---|
| 894 | }
|
---|
| 895 | return decoded;
|
---|
| 896 | }
|
---|
| 897 |
|
---|
[7753] | 898 | //
|
---|
[6812] | 899 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 900 | bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) {
|
---|
[6812] | 901 | bool decoded = false;
|
---|
| 902 | bncTime CurrentObsTime;
|
---|
| 903 | int i, numsats, syncf, type;
|
---|
| 904 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 905 |
|
---|
| 906 | data += 3; /* header */
|
---|
| 907 | size -= 6; /* header + crc */
|
---|
| 908 |
|
---|
| 909 | GETBITS(type, 12)
|
---|
[8158] | 910 | SKIPBITS(12)
|
---|
| 911 | /* id */
|
---|
| 912 | GETBITS(i, 27)
|
---|
| 913 | /* tk */
|
---|
[6812] | 914 |
|
---|
| 915 | CurrentObsTime.setTk(i);
|
---|
[8158] | 916 | if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
|
---|
[6812] | 917 | decoded = true;
|
---|
[7753] | 918 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 919 | _CurrentObsList.clear();
|
---|
| 920 | }
|
---|
| 921 | _CurrentTime = CurrentObsTime;
|
---|
| 922 |
|
---|
[8158] | 923 | GETBITS(syncf, 1)
|
---|
| 924 | /* sync */
|
---|
| 925 | GETBITS(numsats, 5)
|
---|
| 926 | SKIPBITS(4)
|
---|
| 927 | /* smind, smint */
|
---|
[6812] | 928 |
|
---|
[8158] | 929 | while (numsats--) {
|
---|
| 930 | int sv, code, l1range, amb = 0, freq;
|
---|
[6812] | 931 | t_satObs CurrentObs;
|
---|
| 932 | CurrentObs._time = CurrentObsTime;
|
---|
| 933 |
|
---|
| 934 | GETBITS(sv, 6)
|
---|
| 935 | CurrentObs._prn.set('R', sv);
|
---|
| 936 | GETBITS(code, 1)
|
---|
| 937 | GETBITS(freq, 5)
|
---|
[8158] | 938 | GLOFreq[sv - 1] = 100 + freq - 7; /* store frequency for other users (MSM) */
|
---|
[6812] | 939 |
|
---|
| 940 | t_frqObs *frqObs = new t_frqObs;
|
---|
| 941 | /* L1 */
|
---|
[8158] | 942 | (code) ?
|
---|
| 943 | frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C");
|
---|
[6812] | 944 | GETBITS(l1range, 25);
|
---|
| 945 | GETBITSSIGN(i, 20);
|
---|
[8158] | 946 | if ((i & ((1 << 20) - 1)) != 0x80000) {
|
---|
| 947 | frqObs->_code = l1range * 0.02;
|
---|
| 948 | frqObs->_phase = (l1range * 0.02 + i * 0.0005)
|
---|
| 949 | / GLO_WAVELENGTH_L1(freq - 7);
|
---|
[6812] | 950 | frqObs->_codeValid = frqObs->_phaseValid = true;
|
---|
| 951 | }
|
---|
| 952 | GETBITS(i, 7);
|
---|
[8617] | 953 | frqObs->_lockTime = lti2sec(type,i);
|
---|
[8682] | 954 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
|
---|
[6812] | 955 | frqObs->_slipCounter = i;
|
---|
[8158] | 956 | if (type == 1010 || type == 1012) {
|
---|
| 957 | GETBITS(amb, 7);
|
---|
| 958 | if (amb) {
|
---|
| 959 | frqObs->_code += amb * 599584.916;
|
---|
| 960 | frqObs->_phase += (amb * 599584.916) / GLO_WAVELENGTH_L1(freq - 7);
|
---|
[6812] | 961 | }
|
---|
| 962 | GETBITS(i, 8);
|
---|
[8158] | 963 | if (i) {
|
---|
| 964 | frqObs->_snr = i * 0.25;
|
---|
[6812] | 965 | frqObs->_snrValid = true;
|
---|
| 966 | }
|
---|
| 967 | }
|
---|
| 968 | CurrentObs._obs.push_back(frqObs);
|
---|
[8158] | 969 | if (type == 1011 || type == 1012) {
|
---|
[6812] | 970 | frqObs = new t_frqObs;
|
---|
| 971 | /* L2 */
|
---|
[8158] | 972 | GETBITS(code, 2);
|
---|
| 973 | switch (code) {
|
---|
| 974 | case 3:
|
---|
| 975 | frqObs->_rnxType2ch.assign("2P");
|
---|
| 976 | break;
|
---|
| 977 | case 2:
|
---|
| 978 | frqObs->_rnxType2ch.assign("2P");
|
---|
| 979 | break;
|
---|
| 980 | case 1:
|
---|
| 981 | frqObs->_rnxType2ch.assign("2P");
|
---|
| 982 | break;
|
---|
| 983 | case 0:
|
---|
| 984 | frqObs->_rnxType2ch.assign("2C");
|
---|
| 985 | break;
|
---|
[6812] | 986 | }
|
---|
[8158] | 987 | GETBITSSIGN(i, 14);
|
---|
| 988 | if ((i & ((1 << 14) - 1)) != 0x2000) {
|
---|
| 989 | frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 599584.916;
|
---|
[6812] | 990 | frqObs->_codeValid = true;
|
---|
| 991 | }
|
---|
[8158] | 992 | GETBITSSIGN(i, 20);
|
---|
| 993 | if ((i & ((1 << 20) - 1)) != 0x80000) {
|
---|
| 994 | frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 599584.916)
|
---|
| 995 | / GLO_WAVELENGTH_L2(freq - 7);
|
---|
[6812] | 996 | frqObs->_phaseValid = true;
|
---|
| 997 | }
|
---|
[8158] | 998 | GETBITS(i, 7);
|
---|
[8617] | 999 | frqObs->_lockTime = lti2sec(type,i);
|
---|
[8682] | 1000 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
|
---|
[6812] | 1001 | frqObs->_slipCounter = i;
|
---|
[8158] | 1002 | if (type == 1012) {
|
---|
[6812] | 1003 | GETBITS(i, 8);
|
---|
[8158] | 1004 | if (i) {
|
---|
| 1005 | frqObs->_snr = i * 0.25;
|
---|
[6812] | 1006 | frqObs->_snrValid = true;
|
---|
| 1007 | }
|
---|
| 1008 | }
|
---|
| 1009 | CurrentObs._obs.push_back(frqObs);
|
---|
| 1010 | }
|
---|
| 1011 | _CurrentObsList.push_back(CurrentObs);
|
---|
| 1012 | }
|
---|
[8158] | 1013 | if (!syncf) {
|
---|
[6812] | 1014 | decoded = true;
|
---|
[7753] | 1015 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 1016 | _CurrentTime.reset();
|
---|
| 1017 | _CurrentObsList.clear();
|
---|
| 1018 | }
|
---|
| 1019 | return decoded;
|
---|
| 1020 | }
|
---|
| 1021 |
|
---|
| 1022 | //
|
---|
| 1023 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1024 | bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1025 | bool decoded = false;
|
---|
| 1026 |
|
---|
[8158] | 1027 | if (size == 67) {
|
---|
[6812] | 1028 | t_ephGPS eph;
|
---|
| 1029 | int i, week;
|
---|
| 1030 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1031 |
|
---|
| 1032 | data += 3; /* header */
|
---|
| 1033 | size -= 6; /* header + crc */
|
---|
| 1034 | SKIPBITS(12)
|
---|
| 1035 |
|
---|
| 1036 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
| 1037 |
|
---|
| 1038 | GETBITS(i, 6)
|
---|
| 1039 | eph._prn.set('G', i);
|
---|
| 1040 | GETBITS(week, 10)
|
---|
| 1041 | GETBITS(i, 4)
|
---|
| 1042 | eph._ura = accuracyFromIndex(i, eph.type());
|
---|
| 1043 | GETBITS(eph._L2Codes, 2)
|
---|
| 1044 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1045 | GETBITS(eph._IODE, 8)
|
---|
| 1046 | GETBITS(i, 16)
|
---|
| 1047 | i <<= 4;
|
---|
[8158] | 1048 | eph._TOC.set(i * 1000);
|
---|
[8783] | 1049 | GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
|
---|
| 1050 | GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
|
---|
| 1051 | GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
[6812] | 1052 | GETBITS(eph._IODC, 10)
|
---|
[8783] | 1053 | GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
|
---|
| 1054 | GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1055 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1056 | GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
|
---|
| 1057 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1058 | GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
|
---|
| 1059 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[6812] | 1060 | GETBITS(i, 16)
|
---|
| 1061 | i <<= 4;
|
---|
| 1062 | eph._TOEsec = i;
|
---|
| 1063 | bncTime t;
|
---|
[8158] | 1064 | t.set(i * 1000);
|
---|
[6812] | 1065 | eph._TOEweek = t.gpsw();
|
---|
[8472] | 1066 | int numOfRollOvers = int(floor(t.gpsw()/1024.0));
|
---|
| 1067 | week += (numOfRollOvers * 1024);
|
---|
[6812] | 1068 | /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
|
---|
[8158] | 1069 | if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
|
---|
[6812] | 1070 | return false;
|
---|
[8783] | 1071 | GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
|
---|
| 1072 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1073 | GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
|
---|
| 1074 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1075 | GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
|
---|
| 1076 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
[6812] | 1077 | GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1078 | GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
[6812] | 1079 | GETBITS(eph._health, 6)
|
---|
| 1080 | GETBITS(eph._L2PFlag, 1)
|
---|
| 1081 | GETBITS(eph._fitInterval, 1)
|
---|
| 1082 | eph._TOT = 0.9999e9;
|
---|
| 1083 |
|
---|
| 1084 | emit newGPSEph(eph);
|
---|
| 1085 | decoded = true;
|
---|
| 1086 | }
|
---|
| 1087 | return decoded;
|
---|
| 1088 | }
|
---|
| 1089 |
|
---|
| 1090 | //
|
---|
| 1091 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1092 | bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1093 | bool decoded = false;
|
---|
| 1094 |
|
---|
[8158] | 1095 | if (size == 51) {
|
---|
[6812] | 1096 | t_ephGlo eph;
|
---|
| 1097 | int sv, i, tk;
|
---|
| 1098 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1099 |
|
---|
| 1100 | data += 3; /* header */
|
---|
| 1101 | size -= 6; /* header + crc */
|
---|
| 1102 | SKIPBITS(12)
|
---|
| 1103 |
|
---|
| 1104 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
| 1105 |
|
---|
| 1106 | GETBITS(sv, 6)
|
---|
| 1107 | eph._prn.set('R', sv);
|
---|
| 1108 |
|
---|
| 1109 | GETBITS(i, 5)
|
---|
[8158] | 1110 | eph._frequency_number = i - 7;
|
---|
[8182] | 1111 | GETBITS(eph._almanac_health, 1) /* almanac healthy */
|
---|
| 1112 | GETBITS(eph._almanac_health_availablility_indicator, 1) /* almanac health ok */
|
---|
| 1113 | GETBITS(eph._P1, 2) /* P1 */
|
---|
[6812] | 1114 | GETBITS(i, 5)
|
---|
[8158] | 1115 | tk = i * 60 * 60;
|
---|
[6812] | 1116 | GETBITS(i, 6)
|
---|
[8158] | 1117 | tk += i * 60;
|
---|
[6812] | 1118 | GETBITS(i, 1)
|
---|
[8158] | 1119 | tk += i * 30;
|
---|
| 1120 | eph._tki = tk < 3 * 60 * 60 ? tk - 3 * 60 * 60 + 86400 : tk - 3 * 60 * 60;
|
---|
[8182] | 1121 | GETBITS(eph._health, 1) /* MSB of Bn*/
|
---|
| 1122 | GETBITS(eph._P2, 1) /* P2 */
|
---|
[6812] | 1123 | GETBITS(i, 7)
|
---|
[8158] | 1124 | eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */
|
---|
[6812] | 1125 |
|
---|
[8783] | 1126 | GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20))
|
---|
| 1127 | GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11))
|
---|
[8158] | 1128 | GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30))
|
---|
[8783] | 1129 | GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20))
|
---|
| 1130 | GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11))
|
---|
[8158] | 1131 | GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30))
|
---|
[8783] | 1132 | GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20))
|
---|
| 1133 | GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11))
|
---|
[8158] | 1134 | GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30))
|
---|
[8476] | 1135 |
|
---|
[8182] | 1136 | GETBITS(eph._P3, 1) /* P3 */
|
---|
[8783] | 1137 | GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10))
|
---|
| 1138 | GETBITS(eph._M_P, 2) /* GLONASS-M P, */
|
---|
[8182] | 1139 | GETBITS(eph._M_l3, 1) /*GLONASS-M ln (third string) */
|
---|
[8783] | 1140 | GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30)) /* GLONASS tau n(tb) */
|
---|
[8183] | 1141 | GETFLOATSIGNM(eph._M_delta_tau, 5, 1.0 / (double )(1 << 30)) /* GLONASS-M delta tau n(tb) */
|
---|
[6812] | 1142 | GETBITS(eph._E, 5)
|
---|
[8783] | 1143 | GETBITS(eph._M_P4, 1) /* GLONASS-M P4 */
|
---|
| 1144 | GETBITS(eph._M_FT, 4) /* GLONASS-M Ft */
|
---|
[8182] | 1145 | GETBITS(eph._M_NT, 11) /* GLONASS-M Nt */
|
---|
[8783] | 1146 | GETBITS(eph._M_M, 2) /* GLONASS-M M */
|
---|
[8182] | 1147 | GETBITS(eph._additional_data_availability, 1) /* GLONASS-M The Availability of Additional Data */
|
---|
[8783] | 1148 | GETBITS(eph._NA, 11) /* GLONASS-M Na */
|
---|
| 1149 | GETFLOATSIGNM(eph._tauC, 32, 1.0/(double)(1<<30)/(double)(1<<1)) /* GLONASS tau c */
|
---|
[8182] | 1150 | GETBITS(eph._M_N4, 5) /* GLONASS-M N4 */
|
---|
[8783] | 1151 | GETFLOATSIGNM(eph._M_tau_GPS, 22, 1.0/(double)(1<<30)) /* GLONASS-M tau GPS */
|
---|
[8182] | 1152 | GETBITS(eph._M_l5, 1) /* GLONASS-M ln (fifth string) */
|
---|
[6812] | 1153 |
|
---|
| 1154 | unsigned year, month, day;
|
---|
| 1155 | eph._TOC.civil_date(year, month, day);
|
---|
| 1156 | eph._gps_utc = gnumleap(year, month, day);
|
---|
| 1157 | eph._tt = eph._TOC;
|
---|
[8783] | 1158 |
|
---|
[8708] | 1159 | eph._xv(1) = eph._x_pos * 1.e3;
|
---|
| 1160 | eph._xv(2) = eph._y_pos * 1.e3;
|
---|
| 1161 | eph._xv(3) = eph._z_pos * 1.e3;
|
---|
| 1162 | eph._xv(4) = eph._x_velocity * 1.e3;
|
---|
| 1163 | eph._xv(5) = eph._y_velocity * 1.e3;
|
---|
| 1164 | eph._xv(6) = eph._z_velocity * 1.e3;
|
---|
[6812] | 1165 |
|
---|
[8186] | 1166 | GLOFreq[sv - 1] = 100 + eph._frequency_number ; /* store frequency for other users (MSM) */
|
---|
| 1167 | _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(eph._frequency_number, 2, 'f', 0);
|
---|
| 1168 |
|
---|
[8708] | 1169 | emit newGlonassEph(eph);
|
---|
| 1170 | decoded = true;
|
---|
[6812] | 1171 | }
|
---|
| 1172 | return decoded;
|
---|
| 1173 | }
|
---|
| 1174 |
|
---|
| 1175 | //
|
---|
| 1176 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1177 | bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1178 | bool decoded = false;
|
---|
| 1179 |
|
---|
[8158] | 1180 | if (size == 67) {
|
---|
[6812] | 1181 | t_ephGPS eph;
|
---|
| 1182 | int i, week;
|
---|
| 1183 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1184 |
|
---|
| 1185 | data += 3; /* header */
|
---|
| 1186 | size -= 6; /* header + crc */
|
---|
| 1187 | SKIPBITS(12)
|
---|
| 1188 |
|
---|
| 1189 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
| 1190 |
|
---|
| 1191 | GETBITS(i, 4)
|
---|
| 1192 | eph._prn.set('J', i);
|
---|
[8476] | 1193 |
|
---|
[6812] | 1194 | GETBITS(i, 16)
|
---|
| 1195 | i <<= 4;
|
---|
[8158] | 1196 | eph._TOC.set(i * 1000);
|
---|
[6812] | 1197 |
|
---|
[8783] | 1198 | GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
|
---|
| 1199 | GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
|
---|
| 1200 | GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
[6812] | 1201 | GETBITS(eph._IODE, 8)
|
---|
[8783] | 1202 | GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
|
---|
[6812] | 1203 | GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1204 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1205 | GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
|
---|
| 1206 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1207 | GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
|
---|
| 1208 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[6812] | 1209 | GETBITS(i, 16)
|
---|
| 1210 | i <<= 4;
|
---|
| 1211 | eph._TOEsec = i;
|
---|
| 1212 | bncTime t;
|
---|
[8470] | 1213 | t.set(i*1000);
|
---|
| 1214 | eph._TOEweek = t.gpsw();
|
---|
[8783] | 1215 | GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
|
---|
| 1216 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1217 | GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
|
---|
| 1218 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1219 | GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
|
---|
| 1220 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
[6812] | 1221 | GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1222 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[6812] | 1223 | GETBITS(eph._L2Codes, 2)
|
---|
| 1224 | GETBITS(week, 10)
|
---|
[8472] | 1225 | int numOfRollOvers = int(floor(t.gpsw()/1024.0));
|
---|
| 1226 | week += (numOfRollOvers * 1024);
|
---|
[6812] | 1227 | /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
|
---|
[8158] | 1228 | if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
|
---|
[6812] | 1229 | return false;
|
---|
| 1230 |
|
---|
| 1231 | GETBITS(i, 4)
|
---|
[8158] | 1232 | if (i <= 6)
|
---|
| 1233 | eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0;
|
---|
[6812] | 1234 | else
|
---|
[8158] | 1235 | eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0;
|
---|
[6812] | 1236 | GETBITS(eph._health, 6)
|
---|
[8783] | 1237 | GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
[6812] | 1238 | GETBITS(eph._IODC, 10)
|
---|
| 1239 | GETBITS(eph._fitInterval, 1)
|
---|
| 1240 | eph._TOT = 0.9999e9;
|
---|
| 1241 |
|
---|
| 1242 | emit newGPSEph(eph);
|
---|
| 1243 | decoded = true;
|
---|
| 1244 | }
|
---|
| 1245 | return decoded;
|
---|
| 1246 | }
|
---|
| 1247 |
|
---|
| 1248 | //
|
---|
| 1249 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8783] | 1250 | bool RTCM3Decoder::DecodeIRNSSEphemeris(unsigned char* data, int size) {
|
---|
| 1251 | bool decoded = false;
|
---|
| 1252 |
|
---|
| 1253 | if (size == 67) {
|
---|
| 1254 | t_ephGPS eph;
|
---|
| 1255 | int i, week, L5Flag, SFlag;
|
---|
| 1256 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1257 |
|
---|
| 1258 | data += 3; /* header */
|
---|
| 1259 | size -= 6; /* header + crc */
|
---|
| 1260 | SKIPBITS(12)
|
---|
| 1261 |
|
---|
| 1262 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
| 1263 |
|
---|
| 1264 | GETBITS(i, 6)
|
---|
| 1265 | eph._prn.set('I', i);
|
---|
| 1266 | GETBITS(week, 10)
|
---|
| 1267 | GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1268 | GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
|
---|
| 1269 | GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
|
---|
| 1270 | GETBITS(i, 4)
|
---|
| 1271 | eph._ura = accuracyFromIndex(i, eph.type());
|
---|
| 1272 | GETBITS(i, 16)
|
---|
| 1273 | i <<= 4;
|
---|
| 1274 | eph._TOC.set(i * 1000);
|
---|
| 1275 | GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1276 | GETFLOATSIGN(eph._Delta_n, 22, R2R_PI/(double)(1<<30)/(double)(1 << 11))
|
---|
| 1277 | // IODCE
|
---|
| 1278 | GETBITS(eph._IODE, 8)
|
---|
| 1279 | eph._IODC = eph._IODE;
|
---|
| 1280 | SKIPBITS(10)
|
---|
| 1281 | GETBITS(L5Flag, 1)
|
---|
| 1282 | GETBITS(SFlag, 1)
|
---|
| 1283 | if (L5Flag == 0 && SFlag == 0) {
|
---|
| 1284 | eph._health = 0.0;
|
---|
| 1285 | }
|
---|
| 1286 | else if (L5Flag == 0 && SFlag == 1) {
|
---|
| 1287 | eph._health = 1.0;
|
---|
| 1288 | }
|
---|
| 1289 | else if (L5Flag == 1 && SFlag == 0) {
|
---|
| 1290 | eph._health = 2.0;
|
---|
| 1291 | }
|
---|
| 1292 | else if (L5Flag == 1 && SFlag == 1) {
|
---|
| 1293 | eph._health = 3.0;
|
---|
| 1294 | }
|
---|
| 1295 | GETFLOATSIGN(eph._Cuc, 15, 1.0 / (double )(1 << 28))
|
---|
| 1296 | GETFLOATSIGN(eph._Cus, 15, 1.0 / (double )(1 << 28))
|
---|
| 1297 | GETFLOATSIGN(eph._Cic, 15, 1.0 / (double )(1 << 28))
|
---|
| 1298 | GETFLOATSIGN(eph._Cis, 15, 1.0 / (double )(1 << 28))
|
---|
| 1299 | GETFLOATSIGN(eph._Crc, 15, 1.0 / (double )(1 << 4))
|
---|
| 1300 | GETFLOATSIGN(eph._Crs, 15, 1.0 / (double )(1 << 4))
|
---|
| 1301 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1302 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
|
---|
| 1303 | GETBITS(i, 16)
|
---|
| 1304 | i <<= 4;
|
---|
| 1305 | eph._TOEsec = i;
|
---|
| 1306 | bncTime t;
|
---|
| 1307 | t.set(i * 1000);
|
---|
| 1308 | eph._TOEweek = t.gpsw();
|
---|
| 1309 | int numOfRollOvers = int(floor(t.gpsw()/1024.0));
|
---|
| 1310 | week += (numOfRollOvers * 1024);
|
---|
| 1311 | /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
|
---|
| 1312 | if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
|
---|
| 1313 | return false;
|
---|
| 1314 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1315 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
| 1316 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
|
---|
| 1317 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
|
---|
| 1318 | GETFLOATSIGN(eph._OMEGADOT, 22, R2R_PI/(double)(1<<30)/(double)(1<<11))
|
---|
| 1319 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
|
---|
| 1320 | SKIPBITS(2)
|
---|
| 1321 | SKIPBITS(2)
|
---|
| 1322 | eph._TOT = 0.9999e9;
|
---|
| 1323 |
|
---|
| 1324 | emit newGPSEph(eph);
|
---|
| 1325 | decoded = true;
|
---|
| 1326 | }
|
---|
| 1327 | return decoded;
|
---|
| 1328 | }
|
---|
| 1329 |
|
---|
| 1330 | //
|
---|
| 1331 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1332 | bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1333 | bool decoded = false;
|
---|
| 1334 |
|
---|
[8158] | 1335 | if (size == 35) {
|
---|
[6812] | 1336 | t_ephSBAS eph;
|
---|
| 1337 | int i;
|
---|
| 1338 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1339 |
|
---|
| 1340 | data += 3; /* header */
|
---|
| 1341 | size -= 6; /* header + crc */
|
---|
| 1342 | SKIPBITS(12)
|
---|
| 1343 |
|
---|
| 1344 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
| 1345 |
|
---|
| 1346 | GETBITS(i, 6)
|
---|
[8158] | 1347 | eph._prn.set('S', 20 + i);
|
---|
[6812] | 1348 | GETBITS(eph._IODN, 8)
|
---|
| 1349 | GETBITS(i, 13)
|
---|
| 1350 | i <<= 4;
|
---|
[8158] | 1351 | eph._TOC.setTOD(i * 1000);
|
---|
[6812] | 1352 | GETBITS(i, 4)
|
---|
| 1353 | eph._ura = accuracyFromIndex(i, eph.type());
|
---|
| 1354 | GETFLOATSIGN(eph._x_pos, 30, 0.08)
|
---|
| 1355 | GETFLOATSIGN(eph._y_pos, 30, 0.08)
|
---|
| 1356 | GETFLOATSIGN(eph._z_pos, 25, 0.4)
|
---|
| 1357 | GETFLOATSIGN(eph._x_velocity, 17, 0.000625)
|
---|
| 1358 | GETFLOATSIGN(eph._y_velocity, 17, 0.000625)
|
---|
| 1359 | GETFLOATSIGN(eph._z_velocity, 18, 0.004)
|
---|
| 1360 | GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)
|
---|
| 1361 | GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)
|
---|
| 1362 | GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)
|
---|
[8158] | 1363 | GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1))
|
---|
| 1364 | GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10))
|
---|
[6812] | 1365 |
|
---|
[8456] | 1366 | eph._TOT = 0.9999E9;
|
---|
[6812] | 1367 | eph._health = 0;
|
---|
| 1368 |
|
---|
| 1369 | emit newSBASEph(eph);
|
---|
| 1370 | decoded = true;
|
---|
| 1371 | }
|
---|
| 1372 | return decoded;
|
---|
| 1373 | }
|
---|
| 1374 |
|
---|
| 1375 | //
|
---|
| 1376 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1377 | bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1378 | bool decoded = false;
|
---|
| 1379 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1380 | int i;
|
---|
| 1381 |
|
---|
| 1382 | data += 3; /* header */
|
---|
| 1383 | size -= 6; /* header + crc */
|
---|
| 1384 | GETBITS(i, 12)
|
---|
| 1385 |
|
---|
[8158] | 1386 | if ((i == 1046 && size == 61) || (i == 1045 && size == 60)) {
|
---|
[6812] | 1387 | t_ephGal eph;
|
---|
| 1388 |
|
---|
| 1389 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
| 1390 |
|
---|
| 1391 | eph._inav = (i == 1046);
|
---|
| 1392 | eph._fnav = (i == 1045);
|
---|
| 1393 | GETBITS(i, 6)
|
---|
| 1394 | eph._prn.set('E', i, eph._inav ? 1 : 0);
|
---|
| 1395 |
|
---|
[8783] | 1396 | GETBITS(eph._TOEweek, 12) //FIXME: roll-over after week 4095!!
|
---|
[6812] | 1397 | GETBITS(eph._IODnav, 10)
|
---|
| 1398 | GETBITS(i, 8)
|
---|
| 1399 | eph._SISA = accuracyFromIndex(i, eph.type());
|
---|
| 1400 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1401 | GETBITSFACTOR(i, 14, 60)
|
---|
[8158] | 1402 | eph._TOC.set(1024 + eph._TOEweek, i);
|
---|
[8783] | 1403 | GETFLOATSIGN(eph._clock_driftrate, 6, 1.0 / (double )(1 << 30) / (double )(1 << 29))
|
---|
| 1404 | GETFLOATSIGN(eph._clock_drift, 21, 1.0 / (double )(1 << 30) / (double )(1 << 16))
|
---|
| 1405 | GETFLOATSIGN(eph._clock_bias, 31, 1.0 / (double )(1 << 30) / (double )(1 << 4))
|
---|
| 1406 | GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
|
---|
| 1407 | GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1408 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1409 | GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
|
---|
| 1410 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1411 | GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
|
---|
| 1412 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[6812] | 1413 | GETBITSFACTOR(eph._TOEsec, 14, 60)
|
---|
| 1414 | /* FIXME: overwrite value, copied from old code */
|
---|
| 1415 | eph._TOEsec = eph._TOC.gpssec();
|
---|
[8783] | 1416 | GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
|
---|
| 1417 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1418 | GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
|
---|
| 1419 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1420 | GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
|
---|
| 1421 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
[6812] | 1422 | GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8158] | 1423 | GETFLOATSIGN(eph._BGD_1_5A, 10,
|
---|
| 1424 | 1.0 / (double )(1 << 30) / (double )(1 << 2))
|
---|
| 1425 | if (eph._inav) {
|
---|
[7006] | 1426 | /* set unused F/NAV values */
|
---|
[6812] | 1427 | eph._E5aHS = 0.0;
|
---|
| 1428 | eph._e5aDataInValid = false;
|
---|
| 1429 |
|
---|
[8783] | 1430 | GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0 / (double )(1 << 30) / (double )(1 << 2))
|
---|
[6812] | 1431 | GETBITS(eph._E5bHS, 2)
|
---|
| 1432 | GETBITS(eph._e5bDataInValid, 1)
|
---|
| 1433 | GETBITS(eph._E1_bHS, 2)
|
---|
| 1434 | GETBITS(eph._e1DataInValid, 1)
|
---|
| 1435 | }
|
---|
[8158] | 1436 | else {
|
---|
[7006] | 1437 | /* set unused I/NAV values */
|
---|
[6812] | 1438 | eph._BGD_1_5B = 0.0;
|
---|
| 1439 | eph._E5bHS = 0.0;
|
---|
| 1440 | eph._E1_bHS = 0.0;
|
---|
| 1441 | eph._e1DataInValid = false;
|
---|
| 1442 | eph._e5bDataInValid = false;
|
---|
| 1443 |
|
---|
| 1444 | GETBITS(eph._E5aHS, 2)
|
---|
| 1445 | GETBITS(eph._e5aDataInValid, 1)
|
---|
| 1446 | }
|
---|
| 1447 | eph._TOT = 0.9999e9;
|
---|
| 1448 |
|
---|
| 1449 | emit newGalileoEph(eph);
|
---|
| 1450 | decoded = true;
|
---|
| 1451 | }
|
---|
| 1452 | return decoded;
|
---|
| 1453 | }
|
---|
| 1454 |
|
---|
| 1455 | //
|
---|
| 1456 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1457 | bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1458 | bool decoded = false;
|
---|
| 1459 |
|
---|
[8158] | 1460 | if (size == 70) {
|
---|
[6812] | 1461 | t_ephBDS eph;
|
---|
| 1462 | int i;
|
---|
| 1463 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1464 |
|
---|
| 1465 | data += 3; /* header */
|
---|
| 1466 | size -= 6; /* header + crc */
|
---|
| 1467 | SKIPBITS(12)
|
---|
| 1468 |
|
---|
| 1469 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
| 1470 |
|
---|
| 1471 | GETBITS(i, 6)
|
---|
| 1472 | eph._prn.set('C', i);
|
---|
| 1473 |
|
---|
[8158] | 1474 | SKIPBITS(13)
|
---|
| 1475 | /* week */
|
---|
[6812] | 1476 | GETBITS(i, 4)
|
---|
| 1477 | eph._URA = accuracyFromIndex(i, eph.type());
|
---|
| 1478 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1479 | GETBITS(eph._AODE, 5)
|
---|
| 1480 | GETBITS(i, 17)
|
---|
| 1481 | i <<= 3;
|
---|
[8158] | 1482 | eph._TOC.setBDS(i * 1000);
|
---|
[8783] | 1483 | GETFLOATSIGN(eph._clock_driftrate, 11, 1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6))
|
---|
| 1484 | GETFLOATSIGN(eph._clock_drift, 22, 1.0 / (double )(1 << 30) / (double )(1 << 20))
|
---|
| 1485 | GETFLOATSIGN(eph._clock_bias, 24, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
[6812] | 1486 | GETBITS(eph._AODC, 5)
|
---|
[8783] | 1487 | GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6))
|
---|
[6812] | 1488 | GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1489 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1490 | GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1491 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1492 | GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1493 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[6812] | 1494 | GETBITS(i, 17)
|
---|
| 1495 | i <<= 3;
|
---|
| 1496 | eph._TOEsec = i;
|
---|
[8158] | 1497 | eph._TOE.setBDS(i * 1000);
|
---|
[8783] | 1498 | GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1499 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1500 | GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1501 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1502 | GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6))
|
---|
| 1503 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
[6812] | 1504 | GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1505 | GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)
|
---|
| 1506 | GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)
|
---|
[6812] | 1507 | GETBITS(eph._SatH1, 1)
|
---|
| 1508 |
|
---|
[8456] | 1509 | eph._TOT = 0.9999E9;
|
---|
[6812] | 1510 | emit newBDSEph(eph);
|
---|
| 1511 | decoded = true;
|
---|
| 1512 | }
|
---|
| 1513 | return decoded;
|
---|
| 1514 | }
|
---|
| 1515 |
|
---|
[8197] | 1516 | //
|
---|
| 1517 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 1518 | bool RTCM3Decoder::DecodeAntennaReceiver(unsigned char* data, int size) {
|
---|
[8234] | 1519 | char *antenna;
|
---|
| 1520 | char *antserialnum;
|
---|
| 1521 | char *receiver;
|
---|
| 1522 | char *recfirmware;
|
---|
| 1523 | char *recserialnum;
|
---|
[8197] | 1524 | int type;
|
---|
[8234] | 1525 | int antsernum = -1;
|
---|
[8197] | 1526 | int antnum = -1;
|
---|
| 1527 | int recnum = -1;
|
---|
[8234] | 1528 | int recsernum = -1;
|
---|
| 1529 | int recfirnum = -1;
|
---|
[8197] | 1530 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1531 |
|
---|
| 1532 | data += 3; /* header*/
|
---|
| 1533 | size -= 6; /* header + crc */
|
---|
| 1534 |
|
---|
[8198] | 1535 | GETBITS(type, 12)
|
---|
[8234] | 1536 | SKIPBITS(12) /* reference station ID */
|
---|
[8197] | 1537 | GETSTRING(antnum, antenna)
|
---|
[8234] | 1538 | if ((antnum > -1 && antnum < 265) &&
|
---|
| 1539 | (_antType.empty() || strncmp(_antType.back().descriptor, antenna, recnum) != 0)) {
|
---|
| 1540 | _antType.push_back(t_antInfo());
|
---|
| 1541 | memcpy(_antType.back().descriptor, antenna, antnum);
|
---|
| 1542 | _antType.back().descriptor[antnum] = 0;
|
---|
| 1543 | }
|
---|
| 1544 | SKIPBITS(8) /* antenna setup ID */
|
---|
| 1545 | if (type == 1008 || type == 1033 ) {
|
---|
| 1546 | GETSTRING(antsernum, antserialnum)
|
---|
| 1547 | if ((antsernum > -1 && antsernum < 265)) {
|
---|
| 1548 | memcpy(_antType.back().serialnumber, antserialnum, antsernum);
|
---|
| 1549 | _antType.back().serialnumber[antsernum] = 0;
|
---|
[8197] | 1550 | }
|
---|
| 1551 | }
|
---|
[8234] | 1552 |
|
---|
[8197] | 1553 | if (type == 1033) {
|
---|
| 1554 | GETSTRING(recnum, receiver)
|
---|
[8234] | 1555 | GETSTRING(recfirnum, recfirmware)
|
---|
| 1556 | GETSTRING(recsernum, recserialnum)
|
---|
| 1557 | if ((recnum > -1 && recnum < 265) &&
|
---|
| 1558 | (_recType.empty() || strncmp(_recType.back().descriptor, receiver, recnum) != 0)) {
|
---|
| 1559 | _recType.push_back(t_recInfo());
|
---|
| 1560 | memcpy(_recType.back().descriptor, receiver, recnum);
|
---|
| 1561 | _recType.back().descriptor[recnum] = 0;
|
---|
| 1562 | if (recfirnum > -1 && recfirnum < 265) {
|
---|
| 1563 | memcpy(_recType.back().firmware, recfirmware, recfirnum);
|
---|
| 1564 | _recType.back().firmware[recfirnum] = 0;
|
---|
[8197] | 1565 | }
|
---|
[8234] | 1566 | if (recsernum > -1 && recsernum < 265) {
|
---|
| 1567 | memcpy(_recType.back().serialnumber, recserialnum, recsernum);
|
---|
| 1568 | _recType.back().serialnumber[recsernum] = 0;
|
---|
| 1569 | }
|
---|
[8197] | 1570 | }
|
---|
| 1571 | }
|
---|
| 1572 | return true;
|
---|
[6812] | 1573 | }
|
---|
| 1574 |
|
---|
| 1575 | //
|
---|
| 1576 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1577 | bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) {
|
---|
[6812] | 1578 | int type;
|
---|
| 1579 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1580 | double x, y, z;
|
---|
| 1581 |
|
---|
| 1582 | data += 3; /* header */
|
---|
| 1583 | size -= 6; /* header + crc */
|
---|
| 1584 |
|
---|
| 1585 | GETBITS(type, 12)
|
---|
[8234] | 1586 | _antList.push_back(t_antRefPoint());
|
---|
| 1587 | _antList.back().type = t_antRefPoint::ARP;
|
---|
[6812] | 1588 | SKIPBITS(22)
|
---|
| 1589 | GETBITSSIGN(x, 38)
|
---|
| 1590 | _antList.back().xx = x * 1e-4;
|
---|
| 1591 | SKIPBITS(2)
|
---|
| 1592 | GETBITSSIGN(y, 38)
|
---|
| 1593 | _antList.back().yy = y * 1e-4;
|
---|
| 1594 | SKIPBITS(2)
|
---|
| 1595 | GETBITSSIGN(z, 38)
|
---|
| 1596 | _antList.back().zz = z * 1e-4;
|
---|
[8158] | 1597 | if (type == 1006)
|
---|
| 1598 | {
|
---|
[6812] | 1599 | double h;
|
---|
| 1600 | GETBITS(h, 16)
|
---|
| 1601 | _antList.back().height = h * 1e-4;
|
---|
| 1602 | _antList.back().height_f = true;
|
---|
| 1603 | }
|
---|
[8158] | 1604 | _antList.back().message = type;
|
---|
[6812] | 1605 |
|
---|
| 1606 | return true;
|
---|
| 1607 | }
|
---|
| 1608 |
|
---|
[7753] | 1609 | //
|
---|
[6812] | 1610 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1611 | t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
|
---|
[6812] | 1612 | bool decoded = false;
|
---|
| 1613 |
|
---|
| 1614 | errmsg.clear();
|
---|
| 1615 |
|
---|
[8158] | 1616 | while (bufLen && _MessageSize < sizeof(_Message)) {
|
---|
[6812] | 1617 | int l = sizeof(_Message) - _MessageSize;
|
---|
[8158] | 1618 | if (l > bufLen)
|
---|
[6812] | 1619 | l = bufLen;
|
---|
[8158] | 1620 | memcpy(_Message + _MessageSize, buffer, l);
|
---|
[6812] | 1621 | _MessageSize += l;
|
---|
| 1622 | bufLen -= l;
|
---|
| 1623 | buffer += l;
|
---|
| 1624 | int id;
|
---|
[8158] | 1625 | while ((id = GetMessage())) {
|
---|
[6812] | 1626 | /* reset station ID for file loading as it can change */
|
---|
[8158] | 1627 | if (_rawFile)
|
---|
[6812] | 1628 | _staID = _rawFile->staID();
|
---|
| 1629 | /* store the id into the list of loaded blocks */
|
---|
| 1630 | _typeList.push_back(id);
|
---|
| 1631 |
|
---|
[6856] | 1632 | /* SSR I+II data handled in another function, already pass the
|
---|
[6812] | 1633 | * extracted data block. That does no harm, as it anyway skip everything
|
---|
| 1634 | * else. */
|
---|
[8158] | 1635 | if ((id >= 1057 && id <= 1068) || (id >= 1240 && id <= 1270)) {
|
---|
[8204] | 1636 | if (!_coDecoders.contains(_staID.toLatin1()))
|
---|
| 1637 | _coDecoders[_staID.toLatin1()] = new RTCM3coDecoder(_staID);
|
---|
| 1638 | RTCM3coDecoder* coDecoder = _coDecoders[_staID.toLatin1()];
|
---|
[8158] | 1639 | if (coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize,
|
---|
| 1640 | errmsg) == success) {
|
---|
[6812] | 1641 | decoded = true;
|
---|
| 1642 | }
|
---|
| 1643 | }
|
---|
[8783] | 1644 | else if ((id >= 1070 && id <= 1229) ||
|
---|
| 1645 | (id >= 21 && id <= 27)) /* MSM */ {
|
---|
[8158] | 1646 | if (DecodeRTCM3MSM(_Message, _BlockSize))
|
---|
[6812] | 1647 | decoded = true;
|
---|
| 1648 | }
|
---|
[8158] | 1649 | else {
|
---|
| 1650 | switch (id) {
|
---|
| 1651 | case 1001:
|
---|
| 1652 | case 1003:
|
---|
| 1653 | emit(newMessage(
|
---|
| 1654 | QString("%1: Block %2 contain partial data! Ignored!")
|
---|
[8204] | 1655 | .arg(_staID).arg(id).toLatin1(), true));
|
---|
[8158] | 1656 | break; /* no use decoding partial data ATM, remove break when data can be used */
|
---|
| 1657 | case 1002:
|
---|
| 1658 | case 1004:
|
---|
| 1659 | if (DecodeRTCM3GPS(_Message, _BlockSize))
|
---|
| 1660 | decoded = true;
|
---|
| 1661 | break;
|
---|
| 1662 | case 1009:
|
---|
| 1663 | case 1011:
|
---|
| 1664 | emit(newMessage(
|
---|
| 1665 | QString("%1: Block %2 contain partial data! Ignored!")
|
---|
[8204] | 1666 | .arg(_staID).arg(id).toLatin1(), true));
|
---|
[8158] | 1667 | break; /* no use decoding partial data ATM, remove break when data can be used */
|
---|
| 1668 | case 1010:
|
---|
| 1669 | case 1012:
|
---|
| 1670 | if (DecodeRTCM3GLONASS(_Message, _BlockSize))
|
---|
| 1671 | decoded = true;
|
---|
| 1672 | break;
|
---|
| 1673 | case 1019:
|
---|
| 1674 | if (DecodeGPSEphemeris(_Message, _BlockSize))
|
---|
| 1675 | decoded = true;
|
---|
| 1676 | break;
|
---|
| 1677 | case 1020:
|
---|
| 1678 | if (DecodeGLONASSEphemeris(_Message, _BlockSize))
|
---|
| 1679 | decoded = true;
|
---|
| 1680 | break;
|
---|
| 1681 | case 1043:
|
---|
| 1682 | if (DecodeSBASEphemeris(_Message, _BlockSize))
|
---|
| 1683 | decoded = true;
|
---|
| 1684 | break;
|
---|
| 1685 | case 1044:
|
---|
| 1686 | if (DecodeQZSSEphemeris(_Message, _BlockSize))
|
---|
| 1687 | decoded = true;
|
---|
| 1688 | break;
|
---|
[8783] | 1689 | case 29:
|
---|
| 1690 | if (DecodeIRNSSEphemeris(_Message, _BlockSize))
|
---|
| 1691 | decoded = true;
|
---|
| 1692 | break;
|
---|
[8158] | 1693 | case 1045:
|
---|
| 1694 | case 1046:
|
---|
| 1695 | if (DecodeGalileoEphemeris(_Message, _BlockSize))
|
---|
| 1696 | decoded = true;
|
---|
| 1697 | break;
|
---|
| 1698 | case RTCM3ID_BDS:
|
---|
| 1699 | if (DecodeBDSEphemeris(_Message, _BlockSize))
|
---|
| 1700 | decoded = true;
|
---|
| 1701 | break;
|
---|
| 1702 | case 1007:
|
---|
| 1703 | case 1008:
|
---|
| 1704 | case 1033:
|
---|
[8197] | 1705 | DecodeAntennaReceiver(_Message, _BlockSize);
|
---|
[8158] | 1706 | break;
|
---|
| 1707 | case 1005:
|
---|
| 1708 | case 1006:
|
---|
| 1709 | DecodeAntennaPosition(_Message, _BlockSize);
|
---|
| 1710 | break;
|
---|
[296] | 1711 | }
|
---|
| 1712 | }
|
---|
| 1713 | }
|
---|
[6812] | 1714 | }
|
---|
| 1715 | return decoded ? success : failure;
|
---|
[8158] | 1716 | }
|
---|
| 1717 | ;
|
---|
[6812] | 1718 |
|
---|
[7753] | 1719 | //
|
---|
[6812] | 1720 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1721 | uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) {
|
---|
[6812] | 1722 | uint32_t crc = 0;
|
---|
| 1723 | int i;
|
---|
| 1724 |
|
---|
[8158] | 1725 | while (size--) {
|
---|
[6812] | 1726 | crc ^= (*buf++) << (16);
|
---|
[8158] | 1727 | for (i = 0; i < 8; i++)
|
---|
| 1728 | {
|
---|
[6812] | 1729 | crc <<= 1;
|
---|
[8158] | 1730 | if (crc & 0x1000000)
|
---|
[6812] | 1731 | crc ^= 0x01864cfb;
|
---|
[1021] | 1732 | }
|
---|
[296] | 1733 | }
|
---|
[6812] | 1734 | return crc;
|
---|
| 1735 | }
|
---|
[1021] | 1736 |
|
---|
[7753] | 1737 | //
|
---|
[6812] | 1738 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1739 | int RTCM3Decoder::GetMessage(void) {
|
---|
[6812] | 1740 | unsigned char *m, *e;
|
---|
| 1741 | int i;
|
---|
| 1742 |
|
---|
[8158] | 1743 | m = _Message + _SkipBytes;
|
---|
| 1744 | e = _Message + _MessageSize;
|
---|
[6812] | 1745 | _NeedBytes = _SkipBytes = 0;
|
---|
[8158] | 1746 | while (e - m >= 3) {
|
---|
| 1747 | if (m[0] == 0xD3) {
|
---|
| 1748 | _BlockSize = ((m[1] & 3) << 8) | m[2];
|
---|
| 1749 | if (e - m >= static_cast<int>(_BlockSize + 6)) {
|
---|
| 1750 | if (static_cast<uint32_t>((m[3 + _BlockSize] << 16)
|
---|
| 1751 | | (m[3 + _BlockSize + 1] << 8)
|
---|
| 1752 | | (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) {
|
---|
| 1753 | _BlockSize += 6;
|
---|
[6812] | 1754 | _SkipBytes = _BlockSize;
|
---|
| 1755 | break;
|
---|
| 1756 | }
|
---|
| 1757 | else
|
---|
| 1758 | ++m;
|
---|
| 1759 | }
|
---|
[8158] | 1760 | else {
|
---|
[6812] | 1761 | _NeedBytes = _BlockSize;
|
---|
| 1762 | break;
|
---|
| 1763 | }
|
---|
| 1764 | }
|
---|
| 1765 | else
|
---|
| 1766 | ++m;
|
---|
[658] | 1767 | }
|
---|
[8158] | 1768 | if (e - m < 3)
|
---|
[6812] | 1769 | _NeedBytes = 3;
|
---|
| 1770 |
|
---|
| 1771 | /* copy buffer to front */
|
---|
| 1772 | i = m - _Message;
|
---|
[8158] | 1773 | if (i && m < e)
|
---|
| 1774 | memmove(_Message, m, static_cast<size_t>(_MessageSize - i));
|
---|
[6812] | 1775 | _MessageSize -= i;
|
---|
| 1776 |
|
---|
[8158] | 1777 | return !_NeedBytes ? ((_Message[3] << 4) | (_Message[4] >> 4)) : 0;
|
---|
[296] | 1778 | }
|
---|
[1807] | 1779 |
|
---|
[3001] | 1780 | // Time of Corrections
|
---|
| 1781 | //////////////////////////////////////////////////////////////////////////////
|
---|
| 1782 | int RTCM3Decoder::corrGPSEpochTime() const {
|
---|
[8158] | 1783 | return
|
---|
| 1784 | _coDecoders.size() > 0 ?
|
---|
| 1785 | _coDecoders.begin().value()->corrGPSEpochTime() : -1;
|
---|
[3001] | 1786 | }
|
---|