[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);
|
---|
[10234] | 89 | while (it.hasNext()) {
|
---|
[3008] | 90 | it.next();
|
---|
| 91 | delete it.value();
|
---|
[3001] | 92 | }
|
---|
[10234] | 93 | _coDecoders.clear();
|
---|
[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;
|
---|
[9541] | 131 | CurrentObs._type = type;
|
---|
[6812] | 132 |
|
---|
| 133 | GETBITS(sv, 6)
|
---|
[8158] | 134 | if (sv < 40)
|
---|
[6812] | 135 | CurrentObs._prn.set('G', sv);
|
---|
| 136 | else
|
---|
[8158] | 137 | CurrentObs._prn.set('S', sv - 20);
|
---|
[6812] | 138 |
|
---|
| 139 | t_frqObs *frqObs = new t_frqObs;
|
---|
| 140 | /* L1 */
|
---|
| 141 | GETBITS(code, 1);
|
---|
[8158] | 142 | (code) ?
|
---|
| 143 | frqObs->_rnxType2ch.assign("1W") : frqObs->_rnxType2ch.assign("1C");
|
---|
[6812] | 144 | GETBITS(l1range, 24);
|
---|
| 145 | GETBITSSIGN(i, 20);
|
---|
[8158] | 146 | if ((i & ((1 << 20) - 1)) != 0x80000) {
|
---|
| 147 | frqObs->_code = l1range * 0.02;
|
---|
| 148 | frqObs->_phase = (l1range * 0.02 + i * 0.0005) / GPS_WAVELENGTH_L1;
|
---|
[6812] | 149 | frqObs->_codeValid = frqObs->_phaseValid = true;
|
---|
[3002] | 150 | }
|
---|
[9088] | 151 | GETBITS(frqObs->_lockTimeIndicator, 7);
|
---|
| 152 | frqObs->_lockTime = lti2sec(type, frqObs->_lockTimeIndicator);
|
---|
[8617] | 153 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
|
---|
[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 | }
|
---|
[9088] | 196 | GETBITS(frqObs->_lockTimeIndicator, 7);
|
---|
| 197 | frqObs->_lockTime = lti2sec(type, frqObs->_lockTimeIndicator);
|
---|
[8682] | 198 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
|
---|
[8158] | 199 | if (type == 1004) {
|
---|
[6812] | 200 | GETBITS(i, 8);
|
---|
[8158] | 201 | if (i) {
|
---|
| 202 | frqObs->_snr = i * 0.25;
|
---|
[6812] | 203 | frqObs->_snrValid = true;
|
---|
| 204 | }
|
---|
| 205 | }
|
---|
| 206 | CurrentObs._obs.push_back(frqObs);
|
---|
| 207 | }
|
---|
| 208 | _CurrentObsList.push_back(CurrentObs);
|
---|
[913] | 209 | }
|
---|
[7753] | 210 |
|
---|
[8158] | 211 | if (!syncf) {
|
---|
[6812] | 212 | decoded = true;
|
---|
[7753] | 213 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 214 | _CurrentTime.reset();
|
---|
| 215 | _CurrentObsList.clear();
|
---|
[2527] | 216 | }
|
---|
[6812] | 217 | return decoded;
|
---|
| 218 | }
|
---|
[2527] | 219 |
|
---|
[6812] | 220 | #define RTCM3_MSM_NUMSIG 32
|
---|
| 221 | #define RTCM3_MSM_NUMSAT 64
|
---|
| 222 | #define RTCM3_MSM_NUMCELLS 96 /* arbitrary limit */
|
---|
[1127] | 223 |
|
---|
[6812] | 224 | /**
|
---|
| 225 | * Frequency numbers of GLONASS with an offset of 100 to detect unset values.
|
---|
| 226 | * Gets filled by ephemeris and data blocks and shared between different streams.
|
---|
| 227 | */
|
---|
| 228 | static int GLOFreq[RTCM3_MSM_NUMSAT];
|
---|
[1127] | 229 |
|
---|
[6812] | 230 | /*
|
---|
| 231 | * Storage structure to store frequency and RINEX ID assignment for MSM
|
---|
| 232 | * message */
|
---|
| 233 | struct CodeData {
|
---|
| 234 | double wl;
|
---|
| 235 | const char *code; /* currently unused */
|
---|
| 236 | };
|
---|
[2677] | 237 |
|
---|
[6812] | 238 | /** MSM signal types for GPS and SBAS */
|
---|
[8158] | 239 | static struct CodeData gps[RTCM3_MSM_NUMSIG] = {
|
---|
| 240 | {0.0, 0},
|
---|
| 241 | {GPS_WAVELENGTH_L1, "1C"},
|
---|
| 242 | {GPS_WAVELENGTH_L1, "1P"},
|
---|
| 243 | {GPS_WAVELENGTH_L1, "1W"},
|
---|
| 244 | {0.0, 0},
|
---|
| 245 | {0.0, 0},
|
---|
[8801] | 246 | {0.0, 0},
|
---|
[8158] | 247 | {GPS_WAVELENGTH_L2, "2C"},
|
---|
| 248 | {GPS_WAVELENGTH_L2, "2P"},
|
---|
| 249 | {GPS_WAVELENGTH_L2, "2W"},
|
---|
| 250 | {0.0, 0},
|
---|
| 251 | {0.0, 0},
|
---|
| 252 | {0.0, 0},
|
---|
[8801] | 253 | {0.0, 0},
|
---|
[8158] | 254 | {GPS_WAVELENGTH_L2, "2S"},
|
---|
| 255 | {GPS_WAVELENGTH_L2, "2L"},
|
---|
| 256 | {GPS_WAVELENGTH_L2, "2X"},
|
---|
| 257 | {0.0, 0},
|
---|
| 258 | {0.0, 0},
|
---|
| 259 | {0.0, 0},
|
---|
| 260 | {0.0, 0},
|
---|
| 261 | {GPS_WAVELENGTH_L5, "5I"},
|
---|
| 262 | {GPS_WAVELENGTH_L5, "5Q"},
|
---|
| 263 | {GPS_WAVELENGTH_L5, "5X"},
|
---|
| 264 | {0.0, 0},
|
---|
| 265 | {0.0, 0},
|
---|
| 266 | {0.0, 0},
|
---|
| 267 | {0.0, 0},
|
---|
| 268 | {0.0, 0},
|
---|
| 269 | {GPS_WAVELENGTH_L1, "1S"},
|
---|
| 270 | {GPS_WAVELENGTH_L1, "1L"},
|
---|
| 271 | {GPS_WAVELENGTH_L1, "1X"}
|
---|
| 272 | };
|
---|
[1185] | 273 |
|
---|
[6812] | 274 | /**
|
---|
| 275 | * MSM signal types for GLONASS
|
---|
[7753] | 276 | *
|
---|
[6812] | 277 | * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!
|
---|
| 278 | */
|
---|
[8158] | 279 | static struct CodeData glo[RTCM3_MSM_NUMSIG] = {
|
---|
| 280 | {0.0, 0},
|
---|
| 281 | {0.0, "1C"},
|
---|
| 282 | {0.0, "1P"},
|
---|
| 283 | {0.0, 0},
|
---|
| 284 | {0.0, 0},
|
---|
| 285 | {0.0, 0},
|
---|
| 286 | {0.0, 0},
|
---|
| 287 | {1.0, "2C"},
|
---|
| 288 | {1.0, "2P"},
|
---|
[8801] | 289 | {GLO_WAVELENGTH_L1a, "4A"},
|
---|
| 290 | {GLO_WAVELENGTH_L1a, "4B"},
|
---|
| 291 | {GLO_WAVELENGTH_L1a, "4X"},
|
---|
| 292 | {GLO_WAVELENGTH_L2a, "6A"},
|
---|
| 293 | {GLO_WAVELENGTH_L2a, "6B"},
|
---|
| 294 | {GLO_WAVELENGTH_L2a, "6X"},
|
---|
| 295 | {GLO_WAVELENGTH_L3, "3I"},
|
---|
| 296 | {GLO_WAVELENGTH_L3, "3Q"},
|
---|
| 297 | {GLO_WAVELENGTH_L3, "3X"},
|
---|
[8158] | 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 | };
|
---|
[1130] | 313 |
|
---|
[6812] | 314 | /** MSM signal types for Galileo */
|
---|
[8158] | 315 | static struct CodeData gal[RTCM3_MSM_NUMSIG] = {
|
---|
| 316 | {0.0, 0},
|
---|
[8801] | 317 | {GAL_WAVELENGTH_E1, "1C"},
|
---|
| 318 | {GAL_WAVELENGTH_E1, "1A"},
|
---|
| 319 | {GAL_WAVELENGTH_E1, "1B"},
|
---|
| 320 | {GAL_WAVELENGTH_E1, "1X"},
|
---|
| 321 | {GAL_WAVELENGTH_E1, "1Z"},
|
---|
[8158] | 322 | {0.0, 0},
|
---|
[8801] | 323 | {GAL_WAVELENGTH_E6, "6C"},
|
---|
| 324 | {GAL_WAVELENGTH_E6, "6A"},
|
---|
| 325 | {GAL_WAVELENGTH_E6, "6B"},
|
---|
| 326 | {GAL_WAVELENGTH_E6, "6X"},
|
---|
| 327 | {GAL_WAVELENGTH_E6, "6Z"},
|
---|
[8158] | 328 | {0.0, 0},
|
---|
| 329 | {GAL_WAVELENGTH_E5B, "7I"},
|
---|
| 330 | {GAL_WAVELENGTH_E5B, "7Q"},
|
---|
| 331 | {GAL_WAVELENGTH_E5B, "7X"},
|
---|
| 332 | {0.0, 0},
|
---|
[8801] | 333 | {GAL_WAVELENGTH_E5AB,"8I"},
|
---|
| 334 | {GAL_WAVELENGTH_E5AB,"8Q"},
|
---|
| 335 | {GAL_WAVELENGTH_E5AB,"8X"},
|
---|
[8158] | 336 | {0.0, 0},
|
---|
| 337 | {GAL_WAVELENGTH_E5A, "5I"},
|
---|
| 338 | {GAL_WAVELENGTH_E5A, "5Q"},
|
---|
| 339 | {GAL_WAVELENGTH_E5A, "5X"},
|
---|
| 340 | {0.0, 0},
|
---|
| 341 | {0.0, 0},
|
---|
| 342 | {0.0, 0},
|
---|
| 343 | {0.0, 0},
|
---|
| 344 | {0.0, 0},
|
---|
| 345 | {0.0, 0},
|
---|
| 346 | {0.0, 0},
|
---|
[8801] | 347 | {0.0, 0}
|
---|
[8158] | 348 | };
|
---|
[1185] | 349 |
|
---|
[6812] | 350 | /** MSM signal types for QZSS */
|
---|
[8158] | 351 | static struct CodeData qzss[RTCM3_MSM_NUMSIG] = {
|
---|
| 352 | {0.0, 0},
|
---|
| 353 | {GPS_WAVELENGTH_L1, "1C"},
|
---|
| 354 | {0.0, 0},
|
---|
| 355 | {0.0, 0},
|
---|
| 356 | {0.0, 0},
|
---|
| 357 | {0.0, 0},
|
---|
| 358 | {0.0, 0},
|
---|
[8844] | 359 | {0.0, 0},
|
---|
[8682] | 360 | {QZSS_WAVELENGTH_L6, "6S"},
|
---|
| 361 | {QZSS_WAVELENGTH_L6, "6L"},
|
---|
| 362 | {QZSS_WAVELENGTH_L6, "6X"},
|
---|
[8158] | 363 | {0.0, 0},
|
---|
| 364 | {0.0, 0},
|
---|
| 365 | {0.0, 0},
|
---|
| 366 | {GPS_WAVELENGTH_L2, "2S"},
|
---|
| 367 | {GPS_WAVELENGTH_L2, "2L"},
|
---|
| 368 | {GPS_WAVELENGTH_L2, "2X"},
|
---|
| 369 | {0.0, 0},
|
---|
| 370 | {0.0, 0},
|
---|
| 371 | {0.0, 0},
|
---|
| 372 | {0.0, 0},
|
---|
| 373 | {GPS_WAVELENGTH_L5, "5I"},
|
---|
| 374 | {GPS_WAVELENGTH_L5, "5Q"},
|
---|
| 375 | {GPS_WAVELENGTH_L5, "5X"},
|
---|
| 376 | {0.0, 0},
|
---|
| 377 | {0.0, 0},
|
---|
| 378 | {0.0, 0},
|
---|
| 379 | {0.0, 0},
|
---|
| 380 | {0.0, 0},
|
---|
[8682] | 381 | {GPS_WAVELENGTH_L1, "1S"},
|
---|
| 382 | {GPS_WAVELENGTH_L1, "1L"},
|
---|
[8158] | 383 | {GPS_WAVELENGTH_L1, "1X"}
|
---|
| 384 | };
|
---|
[1185] | 385 |
|
---|
[6812] | 386 | /** MSM signal types for Beidou/BDS */
|
---|
[8158] | 387 | static struct CodeData bds[RTCM3_MSM_NUMSIG] = {
|
---|
| 388 | {0.0, 0},
|
---|
| 389 | {BDS_WAVELENGTH_B1, "2I"},
|
---|
[8682] | 390 | {BDS_WAVELENGTH_B1, "2Q"},
|
---|
| 391 | {BDS_WAVELENGTH_B1, "2X"},
|
---|
[8158] | 392 | {0.0, 0},
|
---|
| 393 | {0.0, 0},
|
---|
| 394 | {0.0, 0},
|
---|
| 395 | {BDS_WAVELENGTH_B3, "6I"},
|
---|
[8682] | 396 | {BDS_WAVELENGTH_B3, "6Q"},
|
---|
| 397 | {BDS_WAVELENGTH_B3, "6X"},
|
---|
[8158] | 398 | {0.0, 0},
|
---|
| 399 | {0.0, 0},
|
---|
| 400 | {0.0, 0},
|
---|
| 401 | {BDS_WAVELENGTH_B2, "7I"},
|
---|
[8682] | 402 | {BDS_WAVELENGTH_B2, "7Q"},
|
---|
| 403 | {BDS_WAVELENGTH_B2, "7X"},
|
---|
[8158] | 404 | {0.0, 0},
|
---|
| 405 | {0.0, 0},
|
---|
| 406 | {0.0, 0},
|
---|
| 407 | {0.0, 0},
|
---|
| 408 | {0.0, 0},
|
---|
[8801] | 409 | {BDS_WAVELENGTH_B2a, "5D"},
|
---|
| 410 | {BDS_WAVELENGTH_B2a, "5P"},
|
---|
| 411 | {BDS_WAVELENGTH_B2a, "5X"},
|
---|
[9000] | 412 | {BDS_WAVELENGTH_B2b, "7D"},
|
---|
[8158] | 413 | {0.0, 0},
|
---|
| 414 | {0.0, 0},
|
---|
| 415 | {0.0, 0},
|
---|
| 416 | {0.0, 0},
|
---|
[8818] | 417 | {BDS_WAVELENGTH_B1C, "1D"},
|
---|
| 418 | {BDS_WAVELENGTH_B1C, "1P"},
|
---|
| 419 | {BDS_WAVELENGTH_B1C, "1X"}
|
---|
[8158] | 420 | };
|
---|
[1807] | 421 |
|
---|
[8844] | 422 | /** MSM signal types for IRNSS */
|
---|
[8783] | 423 | static struct CodeData irn[RTCM3_MSM_NUMSIG] = {
|
---|
| 424 | {0.0, 0},
|
---|
| 425 | {0.0, 0},
|
---|
| 426 | {0.0, 0},
|
---|
| 427 | {0.0, 0},
|
---|
| 428 | {0.0, 0},
|
---|
| 429 | {0.0, 0},
|
---|
| 430 | {0.0, 0},
|
---|
| 431 | {IRNSS_WAVELENGTH_S, "9A"},
|
---|
| 432 | {0.0, 0},
|
---|
| 433 | {0.0, 0},
|
---|
| 434 | {0.0, 0},
|
---|
| 435 | {0.0, 0},
|
---|
| 436 | {0.0, 0},
|
---|
| 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 | {IRNSS_WAVELENGTH_L5, "5A"},
|
---|
| 446 | {0.0, 0},
|
---|
| 447 | {0.0, 0},
|
---|
| 448 | {0.0, 0},
|
---|
| 449 | {0.0, 0},
|
---|
| 450 | {0.0, 0},
|
---|
| 451 | {0.0, 0},
|
---|
| 452 | {0.0, 0},
|
---|
| 453 | {0.0, 0},
|
---|
| 454 | {0.0, 0},
|
---|
| 455 | {0.0, 0}
|
---|
| 456 | };
|
---|
| 457 |
|
---|
[6812] | 458 | #define UINT64(c) c ## ULL
|
---|
[1033] | 459 |
|
---|
[7753] | 460 | //
|
---|
[6812] | 461 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8801] | 462 | bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size) {
|
---|
[6812] | 463 | bool decoded = false;
|
---|
| 464 | int type, syncf, i;
|
---|
| 465 | uint64_t numbits = 0, bitfield = 0;
|
---|
[1807] | 466 |
|
---|
[6812] | 467 | data += 3; /* header */
|
---|
| 468 | size -= 6; /* header + crc */
|
---|
[1239] | 469 |
|
---|
[6812] | 470 | GETBITS(type, 12)
|
---|
[8158] | 471 | SKIPBITS(12)
|
---|
| 472 | /* id */
|
---|
[6812] | 473 | char sys;
|
---|
[9262] | 474 | if (type >= 1131 && type <= 1137) {
|
---|
[9007] | 475 | sys = 'I';
|
---|
[9262] | 476 | }
|
---|
| 477 | else if (type >= 1121 && type <= 1127) {
|
---|
[6812] | 478 | sys = 'C';
|
---|
[9262] | 479 | }
|
---|
| 480 | else if (type >= 1111 && type <= 1117) {
|
---|
[6812] | 481 | sys = 'J';
|
---|
[9262] | 482 | }
|
---|
| 483 | else if (type >= 1101 && type <= 1007) {
|
---|
[6812] | 484 | sys = 'S';
|
---|
[9262] | 485 | }
|
---|
| 486 | else if (type >= 1091 && type <= 1097) {
|
---|
[6812] | 487 | sys = 'E';
|
---|
[9262] | 488 | }
|
---|
| 489 | else if (type >= 1081 && type <= 1087) {
|
---|
[6812] | 490 | sys = 'R';
|
---|
[9262] | 491 | }
|
---|
| 492 | else if (type >= 1071 && type <= 1077) {
|
---|
[6812] | 493 | sys = 'G';
|
---|
[9262] | 494 | }
|
---|
| 495 | else {
|
---|
| 496 | return decoded; // false
|
---|
| 497 | }
|
---|
[6812] | 498 | bncTime CurrentObsTime;
|
---|
[8783] | 499 | if (sys == 'C') /* BDS */ {
|
---|
[8158] | 500 | GETBITS(i, 30)
|
---|
[6812] | 501 | CurrentObsTime.setBDS(i);
|
---|
| 502 | }
|
---|
[8158] | 503 | else if (sys == 'R') /* GLONASS */ {
|
---|
[6812] | 504 | SKIPBITS(3)
|
---|
[8158] | 505 | GETBITS(i, 27)
|
---|
| 506 | /* tk */
|
---|
[6812] | 507 | CurrentObsTime.setTk(i);
|
---|
| 508 | }
|
---|
[8158] | 509 | else /* GPS style date */ {
|
---|
| 510 | GETBITS(i, 30)
|
---|
[6812] | 511 | CurrentObsTime.set(i);
|
---|
| 512 | }
|
---|
[8158] | 513 | if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
|
---|
[6812] | 514 | decoded = true;
|
---|
[8160] | 515 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 516 | _CurrentObsList.clear();
|
---|
| 517 | }
|
---|
| 518 | _CurrentTime = CurrentObsTime;
|
---|
[2677] | 519 |
|
---|
[6812] | 520 | GETBITS(syncf, 1)
|
---|
| 521 | /**
|
---|
| 522 | * Ignore unknown types except for sync flag
|
---|
[7753] | 523 | *
|
---|
[6812] | 524 | * We actually support types 1-3 in following code, but as they are missing
|
---|
| 525 | * the full cycles and can't be used later we skip interpretation here already.
|
---|
| 526 | */
|
---|
[9007] | 527 | if (type <= 1137 && (type % 10) >= 4 && (type % 10) <= 7) {
|
---|
[6812] | 528 | int sigmask, numsat = 0, numsig = 0;
|
---|
| 529 | uint64_t satmask, cellmask, ui;
|
---|
[8801] | 530 | // satellite data
|
---|
| 531 | double rrmod[RTCM3_MSM_NUMSAT]; // GNSS sat rough ranges modulo 1 millisecond
|
---|
| 532 | int rrint[RTCM3_MSM_NUMSAT]; // number of integer msecs in GNSS sat rough ranges
|
---|
| 533 | int rdop[RTCM3_MSM_NUMSAT]; // GNSS sat rough phase range rates
|
---|
| 534 | int extsat[RTCM3_MSM_NUMSAT];// extended sat info
|
---|
| 535 | // signal data
|
---|
| 536 | int ll[RTCM3_MSM_NUMCELLS]; // lock time indicator
|
---|
| 537 | /*int hc[RTCM3_MSM_NUMCELLS];*/ // half cycle ambiguity indicator
|
---|
| 538 | double cnr[RTCM3_MSM_NUMCELLS]; // signal cnr
|
---|
| 539 | double cp[RTCM3_MSM_NUMCELLS]; // fine phase range data
|
---|
| 540 | double psr[RTCM3_MSM_NUMCELLS]; // fine psr
|
---|
| 541 | double dop[RTCM3_MSM_NUMCELLS]; // fine phase range rates
|
---|
[2674] | 542 |
|
---|
[8158] | 543 | SKIPBITS(3 + 7 + 2 + 2 + 1 + 3)
|
---|
[6812] | 544 | GETBITS64(satmask, RTCM3_MSM_NUMSAT)
|
---|
[2674] | 545 |
|
---|
[6812] | 546 | /* http://gurmeetsingh.wordpress.com/2008/08/05/fast-bit-counting-routines/ */
|
---|
[8158] | 547 | for (ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */)
|
---|
[6812] | 548 | ++numsat;
|
---|
| 549 | GETBITS(sigmask, RTCM3_MSM_NUMSIG)
|
---|
[8158] | 550 | for (i = sigmask; i; i &= (i - 1) /* remove rightmost bit */)
|
---|
[6812] | 551 | ++numsig;
|
---|
[8158] | 552 | for (i = 0; i < RTCM3_MSM_NUMSAT; ++i)
|
---|
[6812] | 553 | extsat[i] = 15;
|
---|
[2674] | 554 |
|
---|
[8158] | 555 | i = numsat * numsig;
|
---|
| 556 | GETBITS64(cellmask, (unsigned )i)
|
---|
[8801] | 557 | // satellite data
|
---|
[8158] | 558 | switch (type % 10) {
|
---|
[6812] | 559 | case 1:
|
---|
| 560 | case 2:
|
---|
| 561 | case 3:
|
---|
[8158] | 562 | /* partial data, already skipped above, but implemented for future expansion ! */
|
---|
| 563 | for (int j = numsat; j--;)
|
---|
| 564 | GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
|
---|
[6812] | 565 | break;
|
---|
| 566 | case 4:
|
---|
[8158] | 567 | case 6:
|
---|
| 568 | for (int j = numsat; j--;)
|
---|
| 569 | GETBITS(rrint[j], 8)
|
---|
| 570 | for (int j = numsat; j--;)
|
---|
| 571 | GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
|
---|
[6812] | 572 | break;
|
---|
| 573 | case 5:
|
---|
| 574 | case 7:
|
---|
[8158] | 575 | for (int j = numsat; j--;)
|
---|
| 576 | GETBITS(rrint[j], 8)
|
---|
| 577 | for (int j = numsat; j--;)
|
---|
| 578 | GETBITS(extsat[j], 4)
|
---|
| 579 | for (int j = numsat; j--;)
|
---|
| 580 | GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
|
---|
| 581 | for (int j = numsat; j--;)
|
---|
| 582 | GETBITSSIGN(rdop[j], 14)
|
---|
[6812] | 583 | break;
|
---|
[8158] | 584 | }
|
---|
[8801] | 585 | // signal data
|
---|
[8158] | 586 | int numcells = numsat * numsig;
|
---|
| 587 | /** Drop anything which exceeds our cell limit. Increase limit definition
|
---|
| 588 | * when that happens. */
|
---|
| 589 | if (numcells <= RTCM3_MSM_NUMCELLS) {
|
---|
| 590 | switch (type % 10) {
|
---|
| 591 | case 1:
|
---|
| 592 | for (int count = numcells; count--;)
|
---|
| 593 | if (cellmask & (UINT64(1) << count))
|
---|
| 594 | GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
|
---|
| 595 | break;
|
---|
| 596 | case 2:
|
---|
| 597 | for (int count = numcells; count--;)
|
---|
| 598 | if (cellmask & (UINT64(1) << count))
|
---|
| 599 | GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
|
---|
| 600 | for (int count = numcells; count--;)
|
---|
| 601 | if (cellmask & (UINT64(1) << count))
|
---|
| 602 | GETBITS(ll[count], 4)
|
---|
| 603 | for (int count = numcells; count--;)
|
---|
| 604 | if (cellmask & (UINT64(1) << count))
|
---|
| 605 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 606 | break;
|
---|
| 607 | case 3:
|
---|
| 608 | for (int count = numcells; count--;)
|
---|
| 609 | if (cellmask & (UINT64(1) << count))
|
---|
| 610 | GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
|
---|
| 611 | for (int count = numcells; count--;)
|
---|
| 612 | if (cellmask & (UINT64(1) << count))
|
---|
| 613 | GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
|
---|
| 614 | for (int count = numcells; count--;)
|
---|
| 615 | if (cellmask & (UINT64(1) << count))
|
---|
| 616 | GETBITS(ll[count], 4)
|
---|
| 617 | for (int count = numcells; count--;)
|
---|
| 618 | if (cellmask & (UINT64(1) << count))
|
---|
| 619 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 620 | break;
|
---|
| 621 | case 4:
|
---|
| 622 | for (int count = numcells; count--;)
|
---|
| 623 | if (cellmask & (UINT64(1) << count))
|
---|
| 624 | GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
|
---|
| 625 | for (int count = numcells; count--;)
|
---|
| 626 | if (cellmask & (UINT64(1) << count))
|
---|
| 627 | GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
|
---|
| 628 | for (int count = numcells; count--;)
|
---|
| 629 | if (cellmask & (UINT64(1) << count))
|
---|
| 630 | GETBITS(ll[count], 4)
|
---|
| 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 | GETBITS(cnr[count], 6)
|
---|
| 637 | break;
|
---|
| 638 | case 5:
|
---|
| 639 | for (int count = numcells; count--;)
|
---|
| 640 | if (cellmask & (UINT64(1) << count))
|
---|
| 641 | GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
|
---|
| 642 | for (int count = numcells; count--;)
|
---|
| 643 | if (cellmask & (UINT64(1) << count))
|
---|
| 644 | GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
|
---|
| 645 | for (int count = numcells; count--;)
|
---|
| 646 | if (cellmask & (UINT64(1) << count))
|
---|
| 647 | GETBITS(ll[count], 4)
|
---|
| 648 | for (int count = numcells; count--;)
|
---|
| 649 | if (cellmask & (UINT64(1) << count))
|
---|
| 650 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 651 | for (int count = numcells; count--;)
|
---|
| 652 | if (cellmask & (UINT64(1) << count))
|
---|
| 653 | GETFLOAT(cnr[count], 6, 1.0)
|
---|
| 654 | for (int count = numcells; count--;)
|
---|
| 655 | if (cellmask & (UINT64(1) << count))
|
---|
| 656 | GETFLOATSIGN(dop[count], 15, 0.0001)
|
---|
| 657 | break;
|
---|
| 658 | case 6:
|
---|
| 659 | for (int count = numcells; count--;)
|
---|
| 660 | if (cellmask & (UINT64(1) << count))
|
---|
| 661 | GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29))
|
---|
| 662 | for (int count = numcells; count--;)
|
---|
| 663 | if (cellmask & (UINT64(1) << count))
|
---|
| 664 | GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31))
|
---|
| 665 | for (int count = numcells; count--;)
|
---|
| 666 | if (cellmask & (UINT64(1) << count))
|
---|
| 667 | GETBITS(ll[count], 10)
|
---|
| 668 | for (int count = numcells; count--;)
|
---|
| 669 | if (cellmask & (UINT64(1) << count))
|
---|
| 670 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 671 | for (int count = numcells; count--;)
|
---|
| 672 | if (cellmask & (UINT64(1) << count))
|
---|
| 673 | GETFLOAT(cnr[count], 10, 1.0 / (1 << 4))
|
---|
| 674 | break;
|
---|
| 675 | case 7:
|
---|
| 676 | for (int count = numcells; count--;)
|
---|
| 677 | if (cellmask & (UINT64(1) << count))
|
---|
| 678 | GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29))
|
---|
| 679 | for (int count = numcells; count--;)
|
---|
| 680 | if (cellmask & (UINT64(1) << count))
|
---|
| 681 | GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31))
|
---|
| 682 | for (int count = numcells; count--;)
|
---|
| 683 | if (cellmask & (UINT64(1) << count))
|
---|
| 684 | GETBITS(ll[count], 10)
|
---|
| 685 | for (int count = numcells; count--;)
|
---|
| 686 | if (cellmask & (UINT64(1) << count))
|
---|
| 687 | SKIPBITS(1)/*GETBITS(hc[count], 1)*/
|
---|
| 688 | for (int count = numcells; count--;)
|
---|
| 689 | if (cellmask & (UINT64(1) << count))
|
---|
| 690 | GETFLOAT(cnr[count], 10, 1.0 / (1 << 4))
|
---|
| 691 | for (int count = numcells; count--;)
|
---|
| 692 | if (cellmask & (UINT64(1) << count))
|
---|
| 693 | GETFLOATSIGN(dop[count], 15, 0.0001)
|
---|
| 694 | break;
|
---|
[6812] | 695 | }
|
---|
| 696 | i = RTCM3_MSM_NUMSAT;
|
---|
| 697 | int j = -1;
|
---|
| 698 | t_satObs CurrentObs;
|
---|
[8158] | 699 | for (int count = numcells; count--;) {
|
---|
| 700 | while (j >= 0 && !(sigmask & (1 << --j)))
|
---|
[6812] | 701 | ;
|
---|
[8158] | 702 | if (j < 0) {
|
---|
| 703 | while (!(satmask & (UINT64(1) << (--i))))
|
---|
| 704 | /* next satellite */
|
---|
[6812] | 705 | ;
|
---|
[8158] | 706 | if (CurrentObs._obs.size() > 0)
|
---|
[6812] | 707 | _CurrentObsList.push_back(CurrentObs);
|
---|
| 708 | CurrentObs.clear();
|
---|
| 709 | CurrentObs._time = CurrentObsTime;
|
---|
[9541] | 710 | CurrentObs._type = type;
|
---|
[8158] | 711 | if (sys == 'S')
|
---|
| 712 | CurrentObs._prn.set(sys, 20 - 1 + RTCM3_MSM_NUMSAT - i);
|
---|
[6812] | 713 | else
|
---|
[8158] | 714 | CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT - i);
|
---|
[6812] | 715 | j = RTCM3_MSM_NUMSIG;
|
---|
[8158] | 716 | while (!(sigmask & (1 << --j)))
|
---|
[6812] | 717 | ;
|
---|
| 718 | --numsat;
|
---|
| 719 | }
|
---|
[8158] | 720 | if (cellmask & (UINT64(1) << count)) {
|
---|
| 721 | struct CodeData cd = {0.0, 0};
|
---|
| 722 | switch (sys) {
|
---|
| 723 | case 'J':
|
---|
| 724 | cd = qzss[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 725 | break;
|
---|
| 726 | case 'C':
|
---|
| 727 | cd = bds[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 728 | break;
|
---|
| 729 | case 'G':
|
---|
| 730 | case 'S':
|
---|
| 731 | cd = gps[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 732 | break;
|
---|
| 733 | case 'R':
|
---|
| 734 | cd = glo[RTCM3_MSM_NUMSIG - j - 1];
|
---|
[6812] | 735 | {
|
---|
[8158] | 736 | int k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1];
|
---|
[8801] | 737 | if (extsat[numsat] < 14) { // channel number is available as extended info for MSM5/7
|
---|
| 738 | k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1] = 100 + extsat[numsat] - 7;
|
---|
[8158] | 739 | }
|
---|
[8801] | 740 | if (k) {
|
---|
| 741 | if (cd.wl == 0.0) {
|
---|
| 742 | cd.wl = GLO_WAVELENGTH_L1(k - 100);
|
---|
| 743 | }
|
---|
| 744 | else if (cd.wl == 1.0) {
|
---|
| 745 | cd.wl = GLO_WAVELENGTH_L2(k - 100);
|
---|
| 746 | }
|
---|
| 747 | }
|
---|
| 748 | else if (!k && cd.wl <= 1) {
|
---|
[8158] | 749 | cd.code = 0;
|
---|
[8801] | 750 | }
|
---|
[2676] | 751 | }
|
---|
[8158] | 752 | break;
|
---|
| 753 | case 'E':
|
---|
| 754 | cd = gal[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 755 | break;
|
---|
[8783] | 756 | case 'I':
|
---|
| 757 | cd = irn[RTCM3_MSM_NUMSIG - j - 1];
|
---|
| 758 | break;
|
---|
[6812] | 759 | }
|
---|
[8158] | 760 | if (cd.code) {
|
---|
[6812] | 761 | t_frqObs *frqObs = new t_frqObs;
|
---|
[7850] | 762 | frqObs->_rnxType2ch.assign(cd.code);
|
---|
[2674] | 763 |
|
---|
[8158] | 764 | switch (type % 10) {
|
---|
| 765 | case 1:
|
---|
| 766 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 767 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
[8801] | 768 | + (rrmod[numsat]) * LIGHTSPEED / 1000.0;
|
---|
[8158] | 769 | frqObs->_codeValid = true;
|
---|
| 770 | }
|
---|
| 771 | break;
|
---|
| 772 | case 2:
|
---|
| 773 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 774 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
[8801] | 775 | + (rrmod[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
|
---|
[8158] | 776 | frqObs->_phaseValid = true;
|
---|
[8617] | 777 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 778 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[9088] | 779 | frqObs->_lockTimeIndicator = ll[count];
|
---|
[8158] | 780 | }
|
---|
| 781 | break;
|
---|
| 782 | case 3:
|
---|
| 783 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 784 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
[8801] | 785 | + (rrmod[numsat]) * LIGHTSPEED / 1000.0;
|
---|
[8158] | 786 | frqObs->_codeValid = true;
|
---|
| 787 | }
|
---|
| 788 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 789 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
[8801] | 790 | + rrmod[numsat] * LIGHTSPEED / 1000.0 / cd.wl;
|
---|
[8158] | 791 | frqObs->_phaseValid = true;
|
---|
[8617] | 792 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 793 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[9088] | 794 | frqObs->_lockTimeIndicator = ll[count];
|
---|
[8158] | 795 | }
|
---|
| 796 | break;
|
---|
| 797 | case 4:
|
---|
| 798 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 799 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
[8801] | 800 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
|
---|
[8158] | 801 | frqObs->_codeValid = true;
|
---|
| 802 | }
|
---|
| 803 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 804 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
[8801] | 805 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
|
---|
[8158] | 806 | frqObs->_phaseValid = true;
|
---|
[8617] | 807 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 808 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[9088] | 809 | frqObs->_lockTimeIndicator = ll[count];
|
---|
[8158] | 810 | }
|
---|
| 811 | frqObs->_snr = cnr[count];
|
---|
| 812 | frqObs->_snrValid = true;
|
---|
| 813 | break;
|
---|
| 814 | case 5:
|
---|
| 815 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 816 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
[8801] | 817 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
|
---|
[8158] | 818 | frqObs->_codeValid = true;
|
---|
| 819 | }
|
---|
| 820 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 821 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
[8801] | 822 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
|
---|
[8158] | 823 | frqObs->_phaseValid = true;
|
---|
[8617] | 824 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 825 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[9088] | 826 | frqObs->_lockTimeIndicator = ll[count];
|
---|
[8158] | 827 | }
|
---|
| 828 | frqObs->_snr = cnr[count];
|
---|
| 829 | frqObs->_snrValid = true;
|
---|
| 830 | if (dop[count] > -1.6384) {
|
---|
| 831 | frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl;
|
---|
| 832 | frqObs->_dopplerValid = true;
|
---|
| 833 | }
|
---|
| 834 | break;
|
---|
| 835 | case 6:
|
---|
| 836 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 837 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
[8801] | 838 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
|
---|
[8158] | 839 | frqObs->_codeValid = true;
|
---|
| 840 | }
|
---|
| 841 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 842 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
[8801] | 843 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
|
---|
[8158] | 844 | frqObs->_phaseValid = true;
|
---|
[8617] | 845 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 846 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[9088] | 847 | frqObs->_lockTimeIndicator = ll[count];
|
---|
[8158] | 848 | }
|
---|
[6812] | 849 |
|
---|
[8158] | 850 | frqObs->_snr = cnr[count];
|
---|
| 851 | frqObs->_snrValid = true;
|
---|
| 852 | break;
|
---|
| 853 | case 7:
|
---|
| 854 | if (psr[count] > -1.0 / (1 << 10)) {
|
---|
| 855 | frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
|
---|
[8801] | 856 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
|
---|
[8158] | 857 | frqObs->_codeValid = true;
|
---|
| 858 | }
|
---|
| 859 | if (cp[count] > -1.0 / (1 << 8)) {
|
---|
| 860 | frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
|
---|
[8801] | 861 | + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
|
---|
[8158] | 862 | frqObs->_phaseValid = true;
|
---|
[8617] | 863 | frqObs->_lockTime = lti2sec(type,ll[count]);
|
---|
[8682] | 864 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
|
---|
[9088] | 865 | frqObs->_lockTimeIndicator = ll[count];
|
---|
[8158] | 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 | }
|
---|
[8801] | 881 | if (CurrentObs._obs.size() > 0) {
|
---|
[6812] | 882 | _CurrentObsList.push_back(CurrentObs);
|
---|
[8801] | 883 | }
|
---|
[6812] | 884 | }
|
---|
| 885 | }
|
---|
[8937] | 886 | else if ((type % 10) < 4) {
|
---|
[6812] | 887 | emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
|
---|
[8204] | 888 | .arg(_staID).arg(type).toLatin1(), true));
|
---|
[6812] | 889 | }
|
---|
[8158] | 890 | if (!syncf) {
|
---|
[6812] | 891 | decoded = true;
|
---|
[8166] | 892 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 893 | _CurrentTime.reset();
|
---|
| 894 | _CurrentObsList.clear();
|
---|
| 895 | }
|
---|
| 896 | return decoded;
|
---|
| 897 | }
|
---|
| 898 |
|
---|
[7753] | 899 | //
|
---|
[6812] | 900 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 901 | bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) {
|
---|
[6812] | 902 | bool decoded = false;
|
---|
| 903 | bncTime CurrentObsTime;
|
---|
| 904 | int i, numsats, syncf, type;
|
---|
| 905 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 906 |
|
---|
| 907 | data += 3; /* header */
|
---|
| 908 | size -= 6; /* header + crc */
|
---|
| 909 |
|
---|
| 910 | GETBITS(type, 12)
|
---|
[8158] | 911 | SKIPBITS(12)
|
---|
| 912 | /* id */
|
---|
| 913 | GETBITS(i, 27)
|
---|
| 914 | /* tk */
|
---|
[6812] | 915 |
|
---|
| 916 | CurrentObsTime.setTk(i);
|
---|
[8158] | 917 | if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
|
---|
[6812] | 918 | decoded = true;
|
---|
[7753] | 919 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 920 | _CurrentObsList.clear();
|
---|
| 921 | }
|
---|
| 922 | _CurrentTime = CurrentObsTime;
|
---|
| 923 |
|
---|
[8158] | 924 | GETBITS(syncf, 1)
|
---|
| 925 | /* sync */
|
---|
| 926 | GETBITS(numsats, 5)
|
---|
| 927 | SKIPBITS(4)
|
---|
| 928 | /* smind, smint */
|
---|
[6812] | 929 |
|
---|
[8158] | 930 | while (numsats--) {
|
---|
| 931 | int sv, code, l1range, amb = 0, freq;
|
---|
[6812] | 932 | t_satObs CurrentObs;
|
---|
| 933 | CurrentObs._time = CurrentObsTime;
|
---|
[9541] | 934 | CurrentObs._type = type;
|
---|
[6812] | 935 |
|
---|
| 936 | GETBITS(sv, 6)
|
---|
| 937 | CurrentObs._prn.set('R', sv);
|
---|
| 938 | GETBITS(code, 1)
|
---|
| 939 | GETBITS(freq, 5)
|
---|
[8158] | 940 | GLOFreq[sv - 1] = 100 + freq - 7; /* store frequency for other users (MSM) */
|
---|
[6812] | 941 |
|
---|
| 942 | t_frqObs *frqObs = new t_frqObs;
|
---|
| 943 | /* L1 */
|
---|
[8158] | 944 | (code) ?
|
---|
| 945 | frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C");
|
---|
[6812] | 946 | GETBITS(l1range, 25);
|
---|
| 947 | GETBITSSIGN(i, 20);
|
---|
[8158] | 948 | if ((i & ((1 << 20) - 1)) != 0x80000) {
|
---|
| 949 | frqObs->_code = l1range * 0.02;
|
---|
[9088] | 950 | frqObs->_phase = (l1range * 0.02 + i * 0.0005) / GLO_WAVELENGTH_L1(freq - 7);
|
---|
[6812] | 951 | frqObs->_codeValid = frqObs->_phaseValid = true;
|
---|
| 952 | }
|
---|
[9088] | 953 | GETBITS(frqObs->_lockTimeIndicator, 7);
|
---|
| 954 | frqObs->_lockTime = lti2sec(type, frqObs->_lockTimeIndicator);
|
---|
[8682] | 955 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
|
---|
[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 | }
|
---|
[9088] | 998 | GETBITS(frqObs->_lockTimeIndicator, 7);
|
---|
| 999 | frqObs->_lockTime = lti2sec(type, frqObs->_lockTimeIndicator);
|
---|
[8682] | 1000 | frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
|
---|
[8158] | 1001 | if (type == 1012) {
|
---|
[6812] | 1002 | GETBITS(i, 8);
|
---|
[8158] | 1003 | if (i) {
|
---|
| 1004 | frqObs->_snr = i * 0.25;
|
---|
[6812] | 1005 | frqObs->_snrValid = true;
|
---|
| 1006 | }
|
---|
| 1007 | }
|
---|
| 1008 | CurrentObs._obs.push_back(frqObs);
|
---|
| 1009 | }
|
---|
| 1010 | _CurrentObsList.push_back(CurrentObs);
|
---|
| 1011 | }
|
---|
[8158] | 1012 | if (!syncf) {
|
---|
[6812] | 1013 | decoded = true;
|
---|
[7753] | 1014 | _obsList.append(_CurrentObsList);
|
---|
[6812] | 1015 | _CurrentTime.reset();
|
---|
| 1016 | _CurrentObsList.clear();
|
---|
| 1017 | }
|
---|
| 1018 | return decoded;
|
---|
| 1019 | }
|
---|
| 1020 |
|
---|
| 1021 | //
|
---|
| 1022 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1023 | bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1024 | bool decoded = false;
|
---|
| 1025 |
|
---|
[8158] | 1026 | if (size == 67) {
|
---|
[6812] | 1027 | t_ephGPS eph;
|
---|
| 1028 | int i, week;
|
---|
| 1029 | uint64_t numbits = 0, bitfield = 0;
|
---|
[8852] | 1030 | int fitIntervalFalg = 0;
|
---|
[6812] | 1031 |
|
---|
| 1032 | data += 3; /* header */
|
---|
| 1033 | size -= 6; /* header + crc */
|
---|
| 1034 | SKIPBITS(12)
|
---|
| 1035 |
|
---|
| 1036 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
[9212] | 1037 | eph._receptStaID = _staID;
|
---|
[6812] | 1038 |
|
---|
| 1039 | GETBITS(i, 6)
|
---|
| 1040 | eph._prn.set('G', i);
|
---|
| 1041 | GETBITS(week, 10)
|
---|
| 1042 | GETBITS(i, 4)
|
---|
| 1043 | eph._ura = accuracyFromIndex(i, eph.type());
|
---|
| 1044 | GETBITS(eph._L2Codes, 2)
|
---|
| 1045 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1046 | GETBITS(eph._IODE, 8)
|
---|
| 1047 | GETBITS(i, 16)
|
---|
| 1048 | i <<= 4;
|
---|
[8158] | 1049 | eph._TOC.set(i * 1000);
|
---|
[8783] | 1050 | GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
|
---|
| 1051 | GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
|
---|
| 1052 | GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
[6812] | 1053 | GETBITS(eph._IODC, 10)
|
---|
[8783] | 1054 | GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
|
---|
| 1055 | GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1056 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1057 | GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
|
---|
| 1058 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1059 | GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
|
---|
| 1060 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[9201] | 1061 | if (eph._sqrt_A < 1000.0) {
|
---|
[9210] | 1062 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1063 | emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
|
---|
[9315] | 1064 | .arg(_staID).arg(1019,4).arg(eph._prn.toString().c_str())
|
---|
[9201] | 1065 | .arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
|
---|
[9210] | 1066 | #endif
|
---|
[9201] | 1067 | return false;
|
---|
| 1068 | }
|
---|
[6812] | 1069 | GETBITS(i, 16)
|
---|
| 1070 | i <<= 4;
|
---|
| 1071 | eph._TOEsec = i;
|
---|
| 1072 | bncTime t;
|
---|
[8158] | 1073 | t.set(i * 1000);
|
---|
[6812] | 1074 | eph._TOEweek = t.gpsw();
|
---|
[8472] | 1075 | int numOfRollOvers = int(floor(t.gpsw()/1024.0));
|
---|
| 1076 | week += (numOfRollOvers * 1024);
|
---|
[6812] | 1077 | /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
|
---|
[8158] | 1078 | if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
|
---|
[6812] | 1079 | return false;
|
---|
[8783] | 1080 | GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
|
---|
| 1081 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1082 | GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
|
---|
| 1083 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1084 | GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
|
---|
| 1085 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
[6812] | 1086 | GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1087 | GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
[6812] | 1088 | GETBITS(eph._health, 6)
|
---|
| 1089 | GETBITS(eph._L2PFlag, 1)
|
---|
[8852] | 1090 | GETBITS(fitIntervalFalg, 1)
|
---|
[8903] | 1091 | eph._fitInterval = fitIntervalFromFlag(fitIntervalFalg, eph._IODC, eph.type());
|
---|
[6812] | 1092 | eph._TOT = 0.9999e9;
|
---|
[9765] | 1093 | eph._navType = t_eph::LNAV;
|
---|
[6812] | 1094 |
|
---|
| 1095 | emit newGPSEph(eph);
|
---|
| 1096 | decoded = true;
|
---|
| 1097 | }
|
---|
| 1098 | return decoded;
|
---|
| 1099 | }
|
---|
| 1100 |
|
---|
| 1101 | //
|
---|
| 1102 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1103 | bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1104 | bool decoded = false;
|
---|
| 1105 |
|
---|
[8158] | 1106 | if (size == 51) {
|
---|
[6812] | 1107 | t_ephGlo eph;
|
---|
| 1108 | int sv, i, tk;
|
---|
| 1109 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1110 |
|
---|
| 1111 | data += 3; /* header */
|
---|
| 1112 | size -= 6; /* header + crc */
|
---|
| 1113 | SKIPBITS(12)
|
---|
| 1114 |
|
---|
| 1115 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
[9212] | 1116 | eph._receptStaID = _staID;
|
---|
[6812] | 1117 |
|
---|
[9892] | 1118 | eph._flags_unknown = true;
|
---|
| 1119 |
|
---|
[6812] | 1120 | GETBITS(sv, 6)
|
---|
| 1121 | eph._prn.set('R', sv);
|
---|
| 1122 |
|
---|
| 1123 | GETBITS(i, 5)
|
---|
[8158] | 1124 | eph._frequency_number = i - 7;
|
---|
[8182] | 1125 | GETBITS(eph._almanac_health, 1) /* almanac healthy */
|
---|
| 1126 | GETBITS(eph._almanac_health_availablility_indicator, 1) /* almanac health ok */
|
---|
[9892] | 1127 | /*
|
---|
[9201] | 1128 | if (eph._almanac_health_availablility_indicator == 0.0) {
|
---|
[9210] | 1129 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1130 | emit(newMessage(QString("%1: Block %2 (%3): ALM = %4: missing data!")
|
---|
[9315] | 1131 | .arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str())
|
---|
[9201] | 1132 | .arg(eph._almanac_health_availablility_indicator).toLatin1(), true));
|
---|
[9210] | 1133 | #endif
|
---|
[9892] | 1134 | return false;
|
---|
[9318] | 1135 | }
|
---|
[9892] | 1136 | */
|
---|
[8182] | 1137 | GETBITS(eph._P1, 2) /* P1 */
|
---|
[6812] | 1138 | GETBITS(i, 5)
|
---|
[8158] | 1139 | tk = i * 60 * 60;
|
---|
[6812] | 1140 | GETBITS(i, 6)
|
---|
[8158] | 1141 | tk += i * 60;
|
---|
[6812] | 1142 | GETBITS(i, 1)
|
---|
[8158] | 1143 | tk += i * 30;
|
---|
[8801] | 1144 | eph._tki = tk - 3*60*60;
|
---|
| 1145 | if(eph._tki < 0.0) {
|
---|
| 1146 | eph._tki += 86400.0;
|
---|
| 1147 | }
|
---|
[8182] | 1148 | GETBITS(eph._health, 1) /* MSB of Bn*/
|
---|
| 1149 | GETBITS(eph._P2, 1) /* P2 */
|
---|
[6812] | 1150 | GETBITS(i, 7)
|
---|
[8158] | 1151 | eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */
|
---|
[6812] | 1152 |
|
---|
[8783] | 1153 | GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20))
|
---|
| 1154 | GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11))
|
---|
[8158] | 1155 | GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30))
|
---|
[8783] | 1156 | GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20))
|
---|
| 1157 | GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11))
|
---|
[8158] | 1158 | GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30))
|
---|
[8783] | 1159 | GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20))
|
---|
| 1160 | GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11))
|
---|
[8158] | 1161 | GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30))
|
---|
[8182] | 1162 | GETBITS(eph._P3, 1) /* P3 */
|
---|
[8783] | 1163 | GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10))
|
---|
| 1164 | GETBITS(eph._M_P, 2) /* GLONASS-M P, */
|
---|
[9201] | 1165 | GETBITS(eph._M_l3, 1) /* GLONASS-M ln (third string) */
|
---|
| 1166 | GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30)) /* GLONASS tau n(tb) */
|
---|
[8183] | 1167 | GETFLOATSIGNM(eph._M_delta_tau, 5, 1.0 / (double )(1 << 30)) /* GLONASS-M delta tau n(tb) */
|
---|
[6812] | 1168 | GETBITS(eph._E, 5)
|
---|
[8783] | 1169 | GETBITS(eph._M_P4, 1) /* GLONASS-M P4 */
|
---|
| 1170 | GETBITS(eph._M_FT, 4) /* GLONASS-M Ft */
|
---|
[8182] | 1171 | GETBITS(eph._M_NT, 11) /* GLONASS-M Nt */
|
---|
[9201] | 1172 | if (eph._M_NT == 0.0) {
|
---|
[9210] | 1173 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1174 | emit(newMessage(QString("%1: Block %2 (%3): NT = %4: missing data!")
|
---|
[9315] | 1175 | .arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str()).arg(eph._M_NT,4).toLatin1(), true));
|
---|
[9210] | 1176 | #endif
|
---|
[9891] | 1177 | return false;
|
---|
[9201] | 1178 | }
|
---|
[8783] | 1179 | GETBITS(eph._M_M, 2) /* GLONASS-M M */
|
---|
[8182] | 1180 | GETBITS(eph._additional_data_availability, 1) /* GLONASS-M The Availability of Additional Data */
|
---|
[9201] | 1181 | if (eph._additional_data_availability == 0.0) {
|
---|
[9210] | 1182 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1183 | emit(newMessage(QString("%1: Block %2 (%3): ADD = %4: missing data!")
|
---|
[9315] | 1184 | .arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str())
|
---|
[9201] | 1185 | .arg(eph._additional_data_availability).toLatin1(), true));
|
---|
[9210] | 1186 | #endif
|
---|
[9891] | 1187 | return false;
|
---|
[9201] | 1188 | }
|
---|
[8783] | 1189 | GETBITS(eph._NA, 11) /* GLONASS-M Na */
|
---|
| 1190 | GETFLOATSIGNM(eph._tauC, 32, 1.0/(double)(1<<30)/(double)(1<<1)) /* GLONASS tau c */
|
---|
[8182] | 1191 | GETBITS(eph._M_N4, 5) /* GLONASS-M N4 */
|
---|
[8783] | 1192 | GETFLOATSIGNM(eph._M_tau_GPS, 22, 1.0/(double)(1<<30)) /* GLONASS-M tau GPS */
|
---|
[8182] | 1193 | GETBITS(eph._M_l5, 1) /* GLONASS-M ln (fifth string) */
|
---|
[6812] | 1194 |
|
---|
| 1195 | unsigned year, month, day;
|
---|
| 1196 | eph._TOC.civil_date(year, month, day);
|
---|
| 1197 | eph._gps_utc = gnumleap(year, month, day);
|
---|
| 1198 | eph._tt = eph._TOC;
|
---|
[8783] | 1199 |
|
---|
[8708] | 1200 | eph._xv(1) = eph._x_pos * 1.e3;
|
---|
| 1201 | eph._xv(2) = eph._y_pos * 1.e3;
|
---|
| 1202 | eph._xv(3) = eph._z_pos * 1.e3;
|
---|
[9201] | 1203 | if (eph._xv.Rows(1,3).NormFrobenius() < 1.0) {
|
---|
[9210] | 1204 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1205 | emit(newMessage(QString("%1: Block %2 (%3): zero position!")
|
---|
[9315] | 1206 | .arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str()).toLatin1(), true));
|
---|
[9210] | 1207 | #endif
|
---|
[9201] | 1208 | return false;
|
---|
| 1209 | }
|
---|
[8708] | 1210 | eph._xv(4) = eph._x_velocity * 1.e3;
|
---|
| 1211 | eph._xv(5) = eph._y_velocity * 1.e3;
|
---|
| 1212 | eph._xv(6) = eph._z_velocity * 1.e3;
|
---|
[9201] | 1213 | if (eph._xv.Rows(4,6).NormFrobenius() < 1.0) {
|
---|
[9210] | 1214 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1215 | emit(newMessage(QString("%1: Block %2 (%3): zero velocity!")
|
---|
[9315] | 1216 | .arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str()).toLatin1(), true));
|
---|
[9210] | 1217 | #endif
|
---|
[9201] | 1218 | return false;
|
---|
| 1219 | }
|
---|
[8186] | 1220 | GLOFreq[sv - 1] = 100 + eph._frequency_number ; /* store frequency for other users (MSM) */
|
---|
| 1221 | _gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(eph._frequency_number, 2, 'f', 0);
|
---|
| 1222 |
|
---|
[9765] | 1223 | eph._navType = t_eph::FDMA;
|
---|
| 1224 |
|
---|
[8708] | 1225 | emit newGlonassEph(eph);
|
---|
| 1226 | decoded = true;
|
---|
[6812] | 1227 | }
|
---|
| 1228 | return decoded;
|
---|
| 1229 | }
|
---|
| 1230 |
|
---|
| 1231 | //
|
---|
| 1232 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1233 | bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1234 | bool decoded = false;
|
---|
| 1235 |
|
---|
[8158] | 1236 | if (size == 67) {
|
---|
[6812] | 1237 | t_ephGPS eph;
|
---|
| 1238 | int i, week;
|
---|
| 1239 | uint64_t numbits = 0, bitfield = 0;
|
---|
[8852] | 1240 | int fitIntervalFalg = 0;
|
---|
[6812] | 1241 |
|
---|
| 1242 | data += 3; /* header */
|
---|
| 1243 | size -= 6; /* header + crc */
|
---|
| 1244 | SKIPBITS(12)
|
---|
| 1245 |
|
---|
| 1246 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
[9212] | 1247 | eph._receptStaID = _staID;
|
---|
[6812] | 1248 |
|
---|
| 1249 | GETBITS(i, 4)
|
---|
| 1250 | eph._prn.set('J', i);
|
---|
[8476] | 1251 |
|
---|
[6812] | 1252 | GETBITS(i, 16)
|
---|
| 1253 | i <<= 4;
|
---|
[8158] | 1254 | eph._TOC.set(i * 1000);
|
---|
[6812] | 1255 |
|
---|
[8783] | 1256 | GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
|
---|
| 1257 | GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
|
---|
| 1258 | GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
[6812] | 1259 | GETBITS(eph._IODE, 8)
|
---|
[8783] | 1260 | GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
|
---|
[6812] | 1261 | GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1262 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1263 | GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
|
---|
| 1264 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1265 | GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
|
---|
| 1266 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[9201] | 1267 | if (eph._sqrt_A < 1000.0) {
|
---|
[9210] | 1268 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1269 | emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
|
---|
| 1270 | .arg(_staID).arg(1044,4).arg(eph._prn.toString().c_str())
|
---|
| 1271 | .arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
|
---|
[9210] | 1272 | #endif
|
---|
[9201] | 1273 | return false;
|
---|
| 1274 | }
|
---|
[6812] | 1275 | GETBITS(i, 16)
|
---|
| 1276 | i <<= 4;
|
---|
| 1277 | eph._TOEsec = i;
|
---|
| 1278 | bncTime t;
|
---|
[8470] | 1279 | t.set(i*1000);
|
---|
| 1280 | eph._TOEweek = t.gpsw();
|
---|
[8783] | 1281 | GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
|
---|
| 1282 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1283 | GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
|
---|
| 1284 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1285 | GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
|
---|
| 1286 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
[6812] | 1287 | GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1288 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[6812] | 1289 | GETBITS(eph._L2Codes, 2)
|
---|
| 1290 | GETBITS(week, 10)
|
---|
[8472] | 1291 | int numOfRollOvers = int(floor(t.gpsw()/1024.0));
|
---|
| 1292 | week += (numOfRollOvers * 1024);
|
---|
[6812] | 1293 | /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
|
---|
[8158] | 1294 | if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
|
---|
[6812] | 1295 | return false;
|
---|
| 1296 |
|
---|
| 1297 | GETBITS(i, 4)
|
---|
[8158] | 1298 | if (i <= 6)
|
---|
| 1299 | eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0;
|
---|
[6812] | 1300 | else
|
---|
[8158] | 1301 | eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0;
|
---|
[6812] | 1302 | GETBITS(eph._health, 6)
|
---|
[8783] | 1303 | GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
[6812] | 1304 | GETBITS(eph._IODC, 10)
|
---|
[8852] | 1305 | GETBITS(fitIntervalFalg, 1)
|
---|
[8903] | 1306 | eph._fitInterval = fitIntervalFromFlag(fitIntervalFalg, eph._IODC, eph.type());
|
---|
[6812] | 1307 | eph._TOT = 0.9999e9;
|
---|
[9765] | 1308 | eph._navType = t_eph::LNAV;
|
---|
[6812] | 1309 |
|
---|
| 1310 | emit newGPSEph(eph);
|
---|
| 1311 | decoded = true;
|
---|
| 1312 | }
|
---|
| 1313 | return decoded;
|
---|
| 1314 | }
|
---|
| 1315 |
|
---|
| 1316 | //
|
---|
| 1317 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8783] | 1318 | bool RTCM3Decoder::DecodeIRNSSEphemeris(unsigned char* data, int size) {
|
---|
| 1319 | bool decoded = false;
|
---|
| 1320 |
|
---|
| 1321 | if (size == 67) {
|
---|
| 1322 | t_ephGPS eph;
|
---|
| 1323 | int i, week, L5Flag, SFlag;
|
---|
| 1324 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1325 |
|
---|
| 1326 | data += 3; /* header */
|
---|
| 1327 | size -= 6; /* header + crc */
|
---|
| 1328 | SKIPBITS(12)
|
---|
| 1329 |
|
---|
| 1330 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
[9212] | 1331 | eph._receptStaID = _staID;
|
---|
[8783] | 1332 |
|
---|
| 1333 | GETBITS(i, 6)
|
---|
| 1334 | eph._prn.set('I', i);
|
---|
| 1335 | GETBITS(week, 10)
|
---|
| 1336 | GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1337 | GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
|
---|
| 1338 | GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
|
---|
| 1339 | GETBITS(i, 4)
|
---|
| 1340 | eph._ura = accuracyFromIndex(i, eph.type());
|
---|
| 1341 | GETBITS(i, 16)
|
---|
| 1342 | i <<= 4;
|
---|
| 1343 | eph._TOC.set(i * 1000);
|
---|
| 1344 | GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1345 | GETFLOATSIGN(eph._Delta_n, 22, R2R_PI/(double)(1<<30)/(double)(1 << 11))
|
---|
| 1346 | // IODCE
|
---|
| 1347 | GETBITS(eph._IODE, 8)
|
---|
| 1348 | eph._IODC = eph._IODE;
|
---|
| 1349 | SKIPBITS(10)
|
---|
| 1350 | GETBITS(L5Flag, 1)
|
---|
| 1351 | GETBITS(SFlag, 1)
|
---|
| 1352 | if (L5Flag == 0 && SFlag == 0) {
|
---|
| 1353 | eph._health = 0.0;
|
---|
| 1354 | }
|
---|
| 1355 | else if (L5Flag == 0 && SFlag == 1) {
|
---|
| 1356 | eph._health = 1.0;
|
---|
| 1357 | }
|
---|
| 1358 | else if (L5Flag == 1 && SFlag == 0) {
|
---|
| 1359 | eph._health = 2.0;
|
---|
| 1360 | }
|
---|
| 1361 | else if (L5Flag == 1 && SFlag == 1) {
|
---|
| 1362 | eph._health = 3.0;
|
---|
| 1363 | }
|
---|
| 1364 | GETFLOATSIGN(eph._Cuc, 15, 1.0 / (double )(1 << 28))
|
---|
| 1365 | GETFLOATSIGN(eph._Cus, 15, 1.0 / (double )(1 << 28))
|
---|
| 1366 | GETFLOATSIGN(eph._Cic, 15, 1.0 / (double )(1 << 28))
|
---|
| 1367 | GETFLOATSIGN(eph._Cis, 15, 1.0 / (double )(1 << 28))
|
---|
| 1368 | GETFLOATSIGN(eph._Crc, 15, 1.0 / (double )(1 << 4))
|
---|
| 1369 | GETFLOATSIGN(eph._Crs, 15, 1.0 / (double )(1 << 4))
|
---|
| 1370 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[9328] | 1371 | SKIPBITS(2)
|
---|
[8783] | 1372 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
|
---|
| 1373 | GETBITS(i, 16)
|
---|
| 1374 | i <<= 4;
|
---|
| 1375 | eph._TOEsec = i;
|
---|
| 1376 | bncTime t;
|
---|
| 1377 | t.set(i * 1000);
|
---|
| 1378 | eph._TOEweek = t.gpsw();
|
---|
| 1379 | int numOfRollOvers = int(floor(t.gpsw()/1024.0));
|
---|
| 1380 | week += (numOfRollOvers * 1024);
|
---|
| 1381 | /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
|
---|
| 1382 | if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
|
---|
| 1383 | return false;
|
---|
| 1384 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1385 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[9201] | 1386 | if (eph._sqrt_A < 1000.0) {
|
---|
[9210] | 1387 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1388 | emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
|
---|
| 1389 | .arg(_staID).arg(1041,4).arg(eph._prn.toString().c_str())
|
---|
| 1390 | .arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
|
---|
[9210] | 1391 | #endif
|
---|
[9201] | 1392 | return false;
|
---|
| 1393 | }
|
---|
[8783] | 1394 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
|
---|
| 1395 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
|
---|
| 1396 | GETFLOATSIGN(eph._OMEGADOT, 22, R2R_PI/(double)(1<<30)/(double)(1<<11))
|
---|
| 1397 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
|
---|
| 1398 | SKIPBITS(2)
|
---|
| 1399 | eph._TOT = 0.9999e9;
|
---|
[9765] | 1400 | eph._navType = t_eph::LNAV;
|
---|
[8783] | 1401 |
|
---|
| 1402 | emit newGPSEph(eph);
|
---|
| 1403 | decoded = true;
|
---|
| 1404 | }
|
---|
| 1405 | return decoded;
|
---|
| 1406 | }
|
---|
| 1407 |
|
---|
| 1408 | //
|
---|
| 1409 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1410 | bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1411 | bool decoded = false;
|
---|
| 1412 |
|
---|
[8158] | 1413 | if (size == 35) {
|
---|
[6812] | 1414 | t_ephSBAS eph;
|
---|
| 1415 | int i;
|
---|
| 1416 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1417 |
|
---|
| 1418 | data += 3; /* header */
|
---|
| 1419 | size -= 6; /* header + crc */
|
---|
| 1420 | SKIPBITS(12)
|
---|
| 1421 |
|
---|
| 1422 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
[9212] | 1423 | eph._receptStaID = _staID;
|
---|
[6812] | 1424 |
|
---|
| 1425 | GETBITS(i, 6)
|
---|
[8158] | 1426 | eph._prn.set('S', 20 + i);
|
---|
[6812] | 1427 | GETBITS(eph._IODN, 8)
|
---|
| 1428 | GETBITS(i, 13)
|
---|
| 1429 | i <<= 4;
|
---|
[8158] | 1430 | eph._TOC.setTOD(i * 1000);
|
---|
[6812] | 1431 | GETBITS(i, 4)
|
---|
| 1432 | eph._ura = accuracyFromIndex(i, eph.type());
|
---|
| 1433 | GETFLOATSIGN(eph._x_pos, 30, 0.08)
|
---|
| 1434 | GETFLOATSIGN(eph._y_pos, 30, 0.08)
|
---|
| 1435 | GETFLOATSIGN(eph._z_pos, 25, 0.4)
|
---|
[9201] | 1436 | ColumnVector pos(3);
|
---|
| 1437 | pos(1) = eph._x_pos; pos(2) = eph._y_pos; pos(3) = eph._z_pos;
|
---|
| 1438 | if (pos.NormFrobenius() < 1.0) {
|
---|
[9210] | 1439 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1440 | emit(newMessage(QString("%1: Block %2 (%3): zero position!")
|
---|
| 1441 | .arg(_staID).arg(1043,4).arg(eph._prn.toString().c_str()).toLatin1(), true));
|
---|
[9210] | 1442 | #endif
|
---|
[9201] | 1443 | return false;
|
---|
| 1444 | }
|
---|
[6812] | 1445 | GETFLOATSIGN(eph._x_velocity, 17, 0.000625)
|
---|
| 1446 | GETFLOATSIGN(eph._y_velocity, 17, 0.000625)
|
---|
| 1447 | GETFLOATSIGN(eph._z_velocity, 18, 0.004)
|
---|
| 1448 | GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)
|
---|
| 1449 | GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)
|
---|
| 1450 | GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)
|
---|
[8158] | 1451 | GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1))
|
---|
| 1452 | GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10))
|
---|
[6812] | 1453 |
|
---|
[8456] | 1454 | eph._TOT = 0.9999E9;
|
---|
[6812] | 1455 | eph._health = 0;
|
---|
[9765] | 1456 | eph._navType = t_eph::SBASL1;
|
---|
[6812] | 1457 |
|
---|
| 1458 | emit newSBASEph(eph);
|
---|
| 1459 | decoded = true;
|
---|
| 1460 | }
|
---|
| 1461 | return decoded;
|
---|
| 1462 | }
|
---|
| 1463 |
|
---|
| 1464 | //
|
---|
| 1465 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1466 | bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1467 | bool decoded = false;
|
---|
| 1468 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1469 | int i;
|
---|
| 1470 |
|
---|
| 1471 | data += 3; /* header */
|
---|
| 1472 | size -= 6; /* header + crc */
|
---|
| 1473 | GETBITS(i, 12)
|
---|
| 1474 |
|
---|
[8158] | 1475 | if ((i == 1046 && size == 61) || (i == 1045 && size == 60)) {
|
---|
[6812] | 1476 | t_ephGal eph;
|
---|
| 1477 |
|
---|
| 1478 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
[9212] | 1479 | eph._receptStaID = _staID;
|
---|
[6812] | 1480 |
|
---|
| 1481 | eph._inav = (i == 1046);
|
---|
| 1482 | eph._fnav = (i == 1045);
|
---|
| 1483 | GETBITS(i, 6)
|
---|
| 1484 | eph._prn.set('E', i, eph._inav ? 1 : 0);
|
---|
| 1485 |
|
---|
[8783] | 1486 | GETBITS(eph._TOEweek, 12) //FIXME: roll-over after week 4095!!
|
---|
[6812] | 1487 | GETBITS(eph._IODnav, 10)
|
---|
| 1488 | GETBITS(i, 8)
|
---|
| 1489 | eph._SISA = accuracyFromIndex(i, eph.type());
|
---|
| 1490 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1491 | GETBITSFACTOR(i, 14, 60)
|
---|
[8158] | 1492 | eph._TOC.set(1024 + eph._TOEweek, i);
|
---|
[8783] | 1493 | GETFLOATSIGN(eph._clock_driftrate, 6, 1.0 / (double )(1 << 30) / (double )(1 << 29))
|
---|
| 1494 | GETFLOATSIGN(eph._clock_drift, 21, 1.0 / (double )(1 << 30) / (double )(1 << 16))
|
---|
| 1495 | GETFLOATSIGN(eph._clock_bias, 31, 1.0 / (double )(1 << 30) / (double )(1 << 4))
|
---|
| 1496 | GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
|
---|
| 1497 | GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1498 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1499 | GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
|
---|
| 1500 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1501 | GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
|
---|
| 1502 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[6812] | 1503 | GETBITSFACTOR(eph._TOEsec, 14, 60)
|
---|
| 1504 | /* FIXME: overwrite value, copied from old code */
|
---|
| 1505 | eph._TOEsec = eph._TOC.gpssec();
|
---|
[8783] | 1506 | GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
|
---|
| 1507 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1508 | GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
|
---|
| 1509 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1510 | GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
|
---|
| 1511 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
[6812] | 1512 | GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8981] | 1513 | GETFLOATSIGN(eph._BGD_1_5A, 10, 1.0 / (double )(1 << 30) / (double )(1 << 2))
|
---|
[8158] | 1514 | if (eph._inav) {
|
---|
[7006] | 1515 | /* set unused F/NAV values */
|
---|
[6812] | 1516 | eph._E5aHS = 0.0;
|
---|
| 1517 | eph._e5aDataInValid = false;
|
---|
| 1518 |
|
---|
[8783] | 1519 | GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0 / (double )(1 << 30) / (double )(1 << 2))
|
---|
[6812] | 1520 | GETBITS(eph._E5bHS, 2)
|
---|
| 1521 | GETBITS(eph._e5bDataInValid, 1)
|
---|
| 1522 | GETBITS(eph._E1_bHS, 2)
|
---|
| 1523 | GETBITS(eph._e1DataInValid, 1)
|
---|
[9192] | 1524 | if (eph._E5bHS != eph._E1_bHS) {
|
---|
[9210] | 1525 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1526 | emit(newMessage(QString("%1: Block %2 (%3) SHS E5b %4 E1B %5: inconsistent health!")
|
---|
| 1527 | .arg(_staID).arg(1046,4).arg(eph._prn.toString().c_str())
|
---|
| 1528 | .arg(eph._E5bHS).arg(eph._E1_bHS).toLatin1(), true));
|
---|
[9210] | 1529 | #endif
|
---|
[9192] | 1530 | return false;
|
---|
| 1531 | }
|
---|
| 1532 | if ((eph._BGD_1_5A == 0.0 && fabs(eph._BGD_1_5B) > 1e-9) ||
|
---|
| 1533 | (eph._BGD_1_5B == 0.0 && fabs(eph._BGD_1_5A) > 1e-9)) {
|
---|
[9210] | 1534 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1535 | emit(newMessage(QString("%1: Block %2 (%3) BGD_15a = %4 BGD_15b = %5: inconsistent BGD!")
|
---|
| 1536 | .arg(_staID).arg(1046,4).arg(eph._prn.toString().c_str())
|
---|
| 1537 | .arg(eph._BGD_1_5A,10,'E',3).arg(eph._BGD_1_5B,10,'E',3).toLatin1(), true));
|
---|
[9210] | 1538 | #endif
|
---|
[9192] | 1539 | return false;
|
---|
| 1540 | }
|
---|
[9765] | 1541 | eph._navType = t_eph::INAF;
|
---|
[6812] | 1542 | }
|
---|
[8158] | 1543 | else {
|
---|
[7006] | 1544 | /* set unused I/NAV values */
|
---|
[6812] | 1545 | eph._BGD_1_5B = 0.0;
|
---|
| 1546 | eph._E5bHS = 0.0;
|
---|
| 1547 | eph._E1_bHS = 0.0;
|
---|
| 1548 | eph._e1DataInValid = false;
|
---|
| 1549 | eph._e5bDataInValid = false;
|
---|
| 1550 |
|
---|
| 1551 | GETBITS(eph._E5aHS, 2)
|
---|
| 1552 | GETBITS(eph._e5aDataInValid, 1)
|
---|
[9765] | 1553 | eph._navType = t_eph::FNAV;
|
---|
[6812] | 1554 | }
|
---|
| 1555 | eph._TOT = 0.9999e9;
|
---|
| 1556 |
|
---|
[9201] | 1557 | if (eph._sqrt_A < 1000.0) {
|
---|
[9210] | 1558 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1559 | emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
|
---|
| 1560 | .arg(_staID).arg(eph._inav? 1046 : 1045,4).arg(eph._prn.toString().c_str())
|
---|
| 1561 | .arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
|
---|
[9210] | 1562 | #endif
|
---|
[9201] | 1563 | return false;
|
---|
| 1564 | }
|
---|
| 1565 |
|
---|
[6812] | 1566 | emit newGalileoEph(eph);
|
---|
| 1567 | decoded = true;
|
---|
| 1568 | }
|
---|
| 1569 | return decoded;
|
---|
| 1570 | }
|
---|
| 1571 |
|
---|
| 1572 | //
|
---|
| 1573 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1574 | bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) {
|
---|
[6812] | 1575 | bool decoded = false;
|
---|
[9765] | 1576 | const double iMaxGEO = 10.0 / 180.0 * M_PI;
|
---|
[6812] | 1577 |
|
---|
[8158] | 1578 | if (size == 70) {
|
---|
[6812] | 1579 | t_ephBDS eph;
|
---|
| 1580 | int i;
|
---|
| 1581 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1582 |
|
---|
| 1583 | data += 3; /* header */
|
---|
| 1584 | size -= 6; /* header + crc */
|
---|
| 1585 | SKIPBITS(12)
|
---|
| 1586 |
|
---|
| 1587 | eph._receptDateTime = currentDateAndTimeGPS();
|
---|
[9212] | 1588 | eph._receptStaID = _staID;
|
---|
[6812] | 1589 |
|
---|
| 1590 | GETBITS(i, 6)
|
---|
| 1591 | eph._prn.set('C', i);
|
---|
| 1592 |
|
---|
[9785] | 1593 | GETBITS(eph._BDTweek, 13)
|
---|
[6812] | 1594 | GETBITS(i, 4)
|
---|
| 1595 | eph._URA = accuracyFromIndex(i, eph.type());
|
---|
| 1596 | GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
| 1597 | GETBITS(eph._AODE, 5)
|
---|
| 1598 | GETBITS(i, 17)
|
---|
| 1599 | i <<= 3;
|
---|
[9785] | 1600 | eph._TOC.setBDS(eph._BDTweek, i);
|
---|
[8783] | 1601 | GETFLOATSIGN(eph._clock_driftrate, 11, 1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6))
|
---|
| 1602 | GETFLOATSIGN(eph._clock_drift, 22, 1.0 / (double )(1 << 30) / (double )(1 << 20))
|
---|
| 1603 | GETFLOATSIGN(eph._clock_bias, 24, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
[6812] | 1604 | GETBITS(eph._AODC, 5)
|
---|
[8783] | 1605 | GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6))
|
---|
[6812] | 1606 | GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1607 | GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1608 | GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1609 | GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
|
---|
| 1610 | GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1611 | GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
|
---|
[9201] | 1612 | if (eph._sqrt_A < 1000.0) {
|
---|
[9210] | 1613 | #ifdef BNC_DEBUG_BCEP
|
---|
[9201] | 1614 | emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
|
---|
| 1615 | .arg(_staID).arg(1042,4).arg(eph._prn.toString().c_str())
|
---|
| 1616 | .arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
|
---|
[9210] | 1617 | #endif
|
---|
[9201] | 1618 | return false;
|
---|
| 1619 | }
|
---|
[6812] | 1620 | GETBITS(i, 17)
|
---|
| 1621 | i <<= 3;
|
---|
| 1622 | eph._TOEsec = i;
|
---|
[9785] | 1623 | eph._TOE.setBDS(eph._BDTweek, i);
|
---|
[8783] | 1624 | GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1625 | GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1626 | GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
|
---|
| 1627 | GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
| 1628 | GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6))
|
---|
| 1629 | GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
|
---|
[6812] | 1630 | GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
|
---|
[8783] | 1631 | GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)
|
---|
| 1632 | GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)
|
---|
[6812] | 1633 | GETBITS(eph._SatH1, 1)
|
---|
| 1634 |
|
---|
[8456] | 1635 | eph._TOT = 0.9999E9;
|
---|
[9765] | 1636 | if (eph._i0 > iMaxGEO) {
|
---|
| 1637 | eph._navType = t_eph::D1;
|
---|
| 1638 | }
|
---|
| 1639 | else {
|
---|
| 1640 | eph._navType = t_eph::D2;
|
---|
| 1641 | }
|
---|
| 1642 |
|
---|
[6812] | 1643 | emit newBDSEph(eph);
|
---|
| 1644 | decoded = true;
|
---|
| 1645 | }
|
---|
| 1646 | return decoded;
|
---|
| 1647 | }
|
---|
| 1648 |
|
---|
[8197] | 1649 | //
|
---|
| 1650 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 1651 | bool RTCM3Decoder::DecodeAntennaReceiver(unsigned char* data, int size) {
|
---|
[8234] | 1652 | char *antenna;
|
---|
| 1653 | char *antserialnum;
|
---|
| 1654 | char *receiver;
|
---|
| 1655 | char *recfirmware;
|
---|
| 1656 | char *recserialnum;
|
---|
[8197] | 1657 | int type;
|
---|
[8234] | 1658 | int antsernum = -1;
|
---|
[8197] | 1659 | int antnum = -1;
|
---|
| 1660 | int recnum = -1;
|
---|
[8234] | 1661 | int recsernum = -1;
|
---|
| 1662 | int recfirnum = -1;
|
---|
[8197] | 1663 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1664 |
|
---|
| 1665 | data += 3; /* header*/
|
---|
| 1666 | size -= 6; /* header + crc */
|
---|
| 1667 |
|
---|
[8198] | 1668 | GETBITS(type, 12)
|
---|
[8234] | 1669 | SKIPBITS(12) /* reference station ID */
|
---|
[8197] | 1670 | GETSTRING(antnum, antenna)
|
---|
[8234] | 1671 | if ((antnum > -1 && antnum < 265) &&
|
---|
| 1672 | (_antType.empty() || strncmp(_antType.back().descriptor, antenna, recnum) != 0)) {
|
---|
| 1673 | _antType.push_back(t_antInfo());
|
---|
| 1674 | memcpy(_antType.back().descriptor, antenna, antnum);
|
---|
| 1675 | _antType.back().descriptor[antnum] = 0;
|
---|
| 1676 | }
|
---|
| 1677 | SKIPBITS(8) /* antenna setup ID */
|
---|
| 1678 | if (type == 1008 || type == 1033 ) {
|
---|
| 1679 | GETSTRING(antsernum, antserialnum)
|
---|
| 1680 | if ((antsernum > -1 && antsernum < 265)) {
|
---|
| 1681 | memcpy(_antType.back().serialnumber, antserialnum, antsernum);
|
---|
| 1682 | _antType.back().serialnumber[antsernum] = 0;
|
---|
[8197] | 1683 | }
|
---|
| 1684 | }
|
---|
[8234] | 1685 |
|
---|
[8197] | 1686 | if (type == 1033) {
|
---|
| 1687 | GETSTRING(recnum, receiver)
|
---|
[8234] | 1688 | GETSTRING(recfirnum, recfirmware)
|
---|
| 1689 | GETSTRING(recsernum, recserialnum)
|
---|
| 1690 | if ((recnum > -1 && recnum < 265) &&
|
---|
| 1691 | (_recType.empty() || strncmp(_recType.back().descriptor, receiver, recnum) != 0)) {
|
---|
| 1692 | _recType.push_back(t_recInfo());
|
---|
| 1693 | memcpy(_recType.back().descriptor, receiver, recnum);
|
---|
| 1694 | _recType.back().descriptor[recnum] = 0;
|
---|
| 1695 | if (recfirnum > -1 && recfirnum < 265) {
|
---|
| 1696 | memcpy(_recType.back().firmware, recfirmware, recfirnum);
|
---|
| 1697 | _recType.back().firmware[recfirnum] = 0;
|
---|
[8197] | 1698 | }
|
---|
[8234] | 1699 | if (recsernum > -1 && recsernum < 265) {
|
---|
| 1700 | memcpy(_recType.back().serialnumber, recserialnum, recsernum);
|
---|
| 1701 | _recType.back().serialnumber[recsernum] = 0;
|
---|
| 1702 | }
|
---|
[8197] | 1703 | }
|
---|
| 1704 | }
|
---|
| 1705 | return true;
|
---|
[6812] | 1706 | }
|
---|
| 1707 |
|
---|
| 1708 | //
|
---|
| 1709 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1710 | bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) {
|
---|
[6812] | 1711 | int type;
|
---|
| 1712 | uint64_t numbits = 0, bitfield = 0;
|
---|
| 1713 | double x, y, z;
|
---|
| 1714 |
|
---|
| 1715 | data += 3; /* header */
|
---|
| 1716 | size -= 6; /* header + crc */
|
---|
| 1717 |
|
---|
| 1718 | GETBITS(type, 12)
|
---|
[8234] | 1719 | _antList.push_back(t_antRefPoint());
|
---|
| 1720 | _antList.back().type = t_antRefPoint::ARP;
|
---|
[6812] | 1721 | SKIPBITS(22)
|
---|
| 1722 | GETBITSSIGN(x, 38)
|
---|
| 1723 | _antList.back().xx = x * 1e-4;
|
---|
| 1724 | SKIPBITS(2)
|
---|
| 1725 | GETBITSSIGN(y, 38)
|
---|
| 1726 | _antList.back().yy = y * 1e-4;
|
---|
| 1727 | SKIPBITS(2)
|
---|
| 1728 | GETBITSSIGN(z, 38)
|
---|
| 1729 | _antList.back().zz = z * 1e-4;
|
---|
[8158] | 1730 | if (type == 1006)
|
---|
| 1731 | {
|
---|
[6812] | 1732 | double h;
|
---|
| 1733 | GETBITS(h, 16)
|
---|
| 1734 | _antList.back().height = h * 1e-4;
|
---|
| 1735 | _antList.back().height_f = true;
|
---|
| 1736 | }
|
---|
[8158] | 1737 | _antList.back().message = type;
|
---|
[6812] | 1738 |
|
---|
| 1739 | return true;
|
---|
| 1740 | }
|
---|
| 1741 |
|
---|
[7753] | 1742 | //
|
---|
[6812] | 1743 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1744 | t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
|
---|
[6812] | 1745 | bool decoded = false;
|
---|
| 1746 |
|
---|
| 1747 | errmsg.clear();
|
---|
| 1748 |
|
---|
[8158] | 1749 | while (bufLen && _MessageSize < sizeof(_Message)) {
|
---|
[6812] | 1750 | int l = sizeof(_Message) - _MessageSize;
|
---|
[8158] | 1751 | if (l > bufLen)
|
---|
[6812] | 1752 | l = bufLen;
|
---|
[8158] | 1753 | memcpy(_Message + _MessageSize, buffer, l);
|
---|
[6812] | 1754 | _MessageSize += l;
|
---|
| 1755 | bufLen -= l;
|
---|
| 1756 | buffer += l;
|
---|
| 1757 | int id;
|
---|
[8158] | 1758 | while ((id = GetMessage())) {
|
---|
[6812] | 1759 | /* reset station ID for file loading as it can change */
|
---|
[8158] | 1760 | if (_rawFile)
|
---|
[6812] | 1761 | _staID = _rawFile->staID();
|
---|
| 1762 | /* store the id into the list of loaded blocks */
|
---|
| 1763 | _typeList.push_back(id);
|
---|
| 1764 |
|
---|
[6856] | 1765 | /* SSR I+II data handled in another function, already pass the
|
---|
[6812] | 1766 | * extracted data block. That does no harm, as it anyway skip everything
|
---|
| 1767 | * else. */
|
---|
[8971] | 1768 | if ((id >= 1057 && id <= 1068) ||
|
---|
[9212] | 1769 | (id >= 1240 && id <= 1270) ||
|
---|
| 1770 | (id == 4076)) {
|
---|
[9048] | 1771 | if (!_coDecoders.contains(_staID.toLatin1())) {
|
---|
| 1772 | _coDecoders[_staID.toLatin1()] = new RTCM3coDecoder(_staID);
|
---|
[9306] | 1773 | if (id == 4076) {
|
---|
| 1774 | _coDecoders[_staID.toLatin1()]->initSsrFormatType(RTCM3coDecoder::IGSssr);
|
---|
| 1775 | }
|
---|
| 1776 | else {
|
---|
| 1777 | _coDecoders[_staID.toLatin1()]->initSsrFormatType(RTCM3coDecoder::RTCMssr);
|
---|
| 1778 | }
|
---|
[9048] | 1779 | }
|
---|
| 1780 | RTCM3coDecoder* coDecoder = _coDecoders[_staID.toLatin1()];
|
---|
[9212] | 1781 | if (coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize, errmsg) == success) {
|
---|
[6812] | 1782 | decoded = true;
|
---|
| 1783 | }
|
---|
| 1784 | }
|
---|
[9285] | 1785 | else if (id >= 1070 && id <= 1237) { /* MSM */
|
---|
[8158] | 1786 | if (DecodeRTCM3MSM(_Message, _BlockSize))
|
---|
[6812] | 1787 | decoded = true;
|
---|
| 1788 | }
|
---|
[8158] | 1789 | else {
|
---|
| 1790 | switch (id) {
|
---|
| 1791 | case 1001:
|
---|
| 1792 | case 1003:
|
---|
[9210] | 1793 | emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
|
---|
| 1794 | .arg(_staID).arg(id).toLatin1(), true));
|
---|
[8158] | 1795 | break; /* no use decoding partial data ATM, remove break when data can be used */
|
---|
| 1796 | case 1002:
|
---|
| 1797 | case 1004:
|
---|
| 1798 | if (DecodeRTCM3GPS(_Message, _BlockSize))
|
---|
| 1799 | decoded = true;
|
---|
| 1800 | break;
|
---|
| 1801 | case 1009:
|
---|
| 1802 | case 1011:
|
---|
[9210] | 1803 | emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
|
---|
| 1804 | .arg(_staID).arg(id).toLatin1(), true));
|
---|
[8158] | 1805 | break; /* no use decoding partial data ATM, remove break when data can be used */
|
---|
| 1806 | case 1010:
|
---|
| 1807 | case 1012:
|
---|
| 1808 | if (DecodeRTCM3GLONASS(_Message, _BlockSize))
|
---|
| 1809 | decoded = true;
|
---|
| 1810 | break;
|
---|
| 1811 | case 1019:
|
---|
| 1812 | if (DecodeGPSEphemeris(_Message, _BlockSize))
|
---|
| 1813 | decoded = true;
|
---|
| 1814 | break;
|
---|
| 1815 | case 1020:
|
---|
| 1816 | if (DecodeGLONASSEphemeris(_Message, _BlockSize))
|
---|
| 1817 | decoded = true;
|
---|
| 1818 | break;
|
---|
| 1819 | case 1043:
|
---|
| 1820 | if (DecodeSBASEphemeris(_Message, _BlockSize))
|
---|
| 1821 | decoded = true;
|
---|
| 1822 | break;
|
---|
| 1823 | case 1044:
|
---|
| 1824 | if (DecodeQZSSEphemeris(_Message, _BlockSize))
|
---|
| 1825 | decoded = true;
|
---|
| 1826 | break;
|
---|
[9002] | 1827 | case 1041:
|
---|
[8783] | 1828 | if (DecodeIRNSSEphemeris(_Message, _BlockSize))
|
---|
| 1829 | decoded = true;
|
---|
| 1830 | break;
|
---|
[8158] | 1831 | case 1045:
|
---|
| 1832 | case 1046:
|
---|
| 1833 | if (DecodeGalileoEphemeris(_Message, _BlockSize))
|
---|
| 1834 | decoded = true;
|
---|
| 1835 | break;
|
---|
[9000] | 1836 | case 1042:
|
---|
[8158] | 1837 | if (DecodeBDSEphemeris(_Message, _BlockSize))
|
---|
| 1838 | decoded = true;
|
---|
| 1839 | break;
|
---|
| 1840 | case 1007:
|
---|
| 1841 | case 1008:
|
---|
| 1842 | case 1033:
|
---|
[8197] | 1843 | DecodeAntennaReceiver(_Message, _BlockSize);
|
---|
[8158] | 1844 | break;
|
---|
| 1845 | case 1005:
|
---|
| 1846 | case 1006:
|
---|
| 1847 | DecodeAntennaPosition(_Message, _BlockSize);
|
---|
| 1848 | break;
|
---|
[296] | 1849 | }
|
---|
| 1850 | }
|
---|
| 1851 | }
|
---|
[6812] | 1852 | }
|
---|
| 1853 | return decoded ? success : failure;
|
---|
[8158] | 1854 | }
|
---|
| 1855 | ;
|
---|
[6812] | 1856 |
|
---|
[7753] | 1857 | //
|
---|
[6812] | 1858 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1859 | uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) {
|
---|
[6812] | 1860 | uint32_t crc = 0;
|
---|
[9025] | 1861 | int ii;
|
---|
[8158] | 1862 | while (size--) {
|
---|
[6812] | 1863 | crc ^= (*buf++) << (16);
|
---|
[9025] | 1864 | for (ii = 0; ii < 8; ii++) {
|
---|
[6812] | 1865 | crc <<= 1;
|
---|
[8158] | 1866 | if (crc & 0x1000000)
|
---|
[6812] | 1867 | crc ^= 0x01864cfb;
|
---|
[1021] | 1868 | }
|
---|
[296] | 1869 | }
|
---|
[6812] | 1870 | return crc;
|
---|
| 1871 | }
|
---|
[1021] | 1872 |
|
---|
[7753] | 1873 | //
|
---|
[6812] | 1874 | ////////////////////////////////////////////////////////////////////////////
|
---|
[8158] | 1875 | int RTCM3Decoder::GetMessage(void) {
|
---|
[6812] | 1876 | unsigned char *m, *e;
|
---|
| 1877 | int i;
|
---|
| 1878 |
|
---|
[8158] | 1879 | m = _Message + _SkipBytes;
|
---|
| 1880 | e = _Message + _MessageSize;
|
---|
[6812] | 1881 | _NeedBytes = _SkipBytes = 0;
|
---|
[8158] | 1882 | while (e - m >= 3) {
|
---|
| 1883 | if (m[0] == 0xD3) {
|
---|
| 1884 | _BlockSize = ((m[1] & 3) << 8) | m[2];
|
---|
| 1885 | if (e - m >= static_cast<int>(_BlockSize + 6)) {
|
---|
| 1886 | if (static_cast<uint32_t>((m[3 + _BlockSize] << 16)
|
---|
| 1887 | | (m[3 + _BlockSize + 1] << 8)
|
---|
| 1888 | | (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) {
|
---|
| 1889 | _BlockSize += 6;
|
---|
[6812] | 1890 | _SkipBytes = _BlockSize;
|
---|
| 1891 | break;
|
---|
| 1892 | }
|
---|
| 1893 | else
|
---|
| 1894 | ++m;
|
---|
| 1895 | }
|
---|
[8158] | 1896 | else {
|
---|
[6812] | 1897 | _NeedBytes = _BlockSize;
|
---|
| 1898 | break;
|
---|
| 1899 | }
|
---|
| 1900 | }
|
---|
| 1901 | else
|
---|
| 1902 | ++m;
|
---|
[658] | 1903 | }
|
---|
[8158] | 1904 | if (e - m < 3)
|
---|
[6812] | 1905 | _NeedBytes = 3;
|
---|
| 1906 |
|
---|
| 1907 | /* copy buffer to front */
|
---|
| 1908 | i = m - _Message;
|
---|
[8158] | 1909 | if (i && m < e)
|
---|
| 1910 | memmove(_Message, m, static_cast<size_t>(_MessageSize - i));
|
---|
[6812] | 1911 | _MessageSize -= i;
|
---|
| 1912 |
|
---|
[8158] | 1913 | return !_NeedBytes ? ((_Message[3] << 4) | (_Message[4] >> 4)) : 0;
|
---|
[296] | 1914 | }
|
---|
[1807] | 1915 |
|
---|
[3001] | 1916 | // Time of Corrections
|
---|
| 1917 | //////////////////////////////////////////////////////////////////////////////
|
---|
| 1918 | int RTCM3Decoder::corrGPSEpochTime() const {
|
---|
[8158] | 1919 | return
|
---|
| 1920 | _coDecoders.size() > 0 ?
|
---|
| 1921 | _coDecoders.begin().value()->corrGPSEpochTime() : -1;
|
---|
[3001] | 1922 | }
|
---|