[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 | *
|
---|
| 37 | * Changes:
|
---|
| 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 |
|
---|
| 47 | #include "RTCM3Decoder.h"
|
---|
[1807] | 48 | #include "../RTCM/rtcm_utils.h"
|
---|
[296] | 49 | #include "bncconst.h"
|
---|
[511] | 50 | #include "bncapp.h"
|
---|
[1535] | 51 | #include "bncutils.h"
|
---|
| 52 | #include "bncsettings.h"
|
---|
[296] | 53 |
|
---|
| 54 | using namespace std;
|
---|
| 55 |
|
---|
| 56 | #ifndef isinf
|
---|
| 57 | # define isinf(x) 0
|
---|
| 58 | #endif
|
---|
| 59 |
|
---|
[320] | 60 | // Error Handling
|
---|
| 61 | ////////////////////////////////////////////////////////////////////////////
|
---|
[504] | 62 | void RTCM3Error(const char*, ...) {
|
---|
[505] | 63 | }
|
---|
[320] | 64 |
|
---|
[296] | 65 | // Constructor
|
---|
| 66 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2527] | 67 | RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) :
|
---|
[2387] | 68 | GPSDecoder() {
|
---|
[505] | 69 |
|
---|
[2527] | 70 | _staID = staID;
|
---|
| 71 | _rawFile = rawFile;
|
---|
[2387] | 72 |
|
---|
[1535] | 73 | bncSettings settings;
|
---|
[1580] | 74 | _checkMountPoint = settings.value("miscMount").toString();
|
---|
[1022] | 75 |
|
---|
[939] | 76 | connect(this, SIGNAL(newGPSEph(gpsephemeris*)),
|
---|
[2545] | 77 | (bncApp*) qApp, SLOT(slotNewGPSEph(gpsephemeris*)),
|
---|
| 78 | Qt::DirectConnection);
|
---|
[939] | 79 | connect(this, SIGNAL(newGlonassEph(glonassephemeris*)),
|
---|
[2545] | 80 | (bncApp*) qApp, SLOT(slotNewGlonassEph(glonassephemeris*)),
|
---|
| 81 | Qt::DirectConnection);
|
---|
[939] | 82 |
|
---|
[880] | 83 | // Sub-Decoder for Clock and Orbit Corrections
|
---|
| 84 | // -------------------------------------------
|
---|
[970] | 85 | _coDecoder = new RTCM3coDecoder(staID);
|
---|
[1021] | 86 |
|
---|
| 87 | // Mode can be either observations or corrections
|
---|
| 88 | // ----------------------------------------------
|
---|
| 89 | _mode = unknown;
|
---|
[1807] | 90 |
|
---|
| 91 | // Antenna position (used for decoding of message 1003)
|
---|
| 92 | // ----------------------------------------------------
|
---|
| 93 | _antXYZ[0] = _antXYZ[1] = _antXYZ[2] = 0;
|
---|
| 94 |
|
---|
[296] | 95 | }
|
---|
| 96 |
|
---|
| 97 | // Destructor
|
---|
| 98 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 99 | RTCM3Decoder::~RTCM3Decoder() {
|
---|
[880] | 100 | delete _coDecoder;
|
---|
[296] | 101 | }
|
---|
| 102 |
|
---|
| 103 | //
|
---|
| 104 | ////////////////////////////////////////////////////////////////////////////
|
---|
[1218] | 105 | t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
|
---|
[508] | 106 |
|
---|
[1218] | 107 | errmsg.clear();
|
---|
| 108 |
|
---|
[913] | 109 | bool decoded = false;
|
---|
| 110 |
|
---|
[2551] | 111 | // If read from file, we set the mode according to staID
|
---|
| 112 | // -----------------------------------------------------
|
---|
| 113 | if (!_staID_corrections.isEmpty() && _rawFile) {
|
---|
| 114 | if (_rawFile->staID() == _staID_corrections) {
|
---|
| 115 | _mode = corrections;
|
---|
| 116 | }
|
---|
| 117 | else {
|
---|
| 118 | _mode = observations;
|
---|
| 119 | }
|
---|
| 120 | }
|
---|
| 121 |
|
---|
[880] | 122 | // Try to decode Clock and Orbit Corrections
|
---|
| 123 | // -----------------------------------------
|
---|
[1021] | 124 | if (_mode == unknown || _mode == corrections) {
|
---|
[1218] | 125 | if ( _coDecoder->Decode(buffer, bufLen, errmsg) == success ) {
|
---|
[1021] | 126 | decoded = true;
|
---|
[2551] | 127 | if (_mode == unknown) {
|
---|
| 128 | if (_rawFile) {
|
---|
| 129 | _staID_corrections = _rawFile->staID();
|
---|
| 130 | }
|
---|
| 131 | else {
|
---|
| 132 | _mode = corrections;
|
---|
| 133 | }
|
---|
[1021] | 134 | }
|
---|
| 135 | }
|
---|
[913] | 136 | }
|
---|
[880] | 137 |
|
---|
[2527] | 138 | // Find the corresponding parser
|
---|
| 139 | // -----------------------------
|
---|
| 140 | QByteArray staID("default");
|
---|
| 141 | if (_rawFile) {
|
---|
| 142 | staID = _rawFile->staID();
|
---|
| 143 | }
|
---|
| 144 |
|
---|
| 145 | bool newParser = !_parsers.contains(staID);
|
---|
| 146 |
|
---|
| 147 | RTCM3ParserData& parser = _parsers[staID];
|
---|
| 148 |
|
---|
| 149 | // Initialize a new parser
|
---|
| 150 | // -----------------------
|
---|
| 151 | if (newParser) {
|
---|
| 152 | parser.rinex3 = 0;
|
---|
| 153 | memset(&parser, 0, sizeof(parser));
|
---|
| 154 | double secGPS;
|
---|
| 155 | currentGPSWeeks(parser.GPSWeek, secGPS);
|
---|
| 156 | parser.GPSTOW = int(secGPS);
|
---|
| 157 | }
|
---|
| 158 |
|
---|
[880] | 159 | // Remaining part decodes the Observations
|
---|
| 160 | // ---------------------------------------
|
---|
[1033] | 161 | if (_mode == unknown || _mode == observations || _checkMountPoint == _staID || _checkMountPoint == "ALL") {
|
---|
[1127] | 162 |
|
---|
[1021] | 163 | for (int ii = 0; ii < bufLen; ii++) {
|
---|
[2527] | 164 | parser.Message[parser.MessageSize++] = buffer[ii];
|
---|
[1127] | 165 |
|
---|
[2527] | 166 | if (parser.MessageSize >= parser.NeedBytes) {
|
---|
[1185] | 167 |
|
---|
[2527] | 168 | while(int rr = RTCM3Parser(&parser)) {
|
---|
[1130] | 169 |
|
---|
[1239] | 170 | // RTCMv3 message types
|
---|
| 171 | // --------------------
|
---|
[2527] | 172 | _typeList.push_back(parser.blocktype);
|
---|
[1185] | 173 |
|
---|
[1239] | 174 | // RTCMv3 antenna descriptor
|
---|
| 175 | // -------------------------
|
---|
| 176 | if(rr == 1007 || rr == 1008 || rr == 1033)
|
---|
| 177 | {
|
---|
[2527] | 178 | _antType.push_back(parser.antenna); /* correct ? */
|
---|
[1239] | 179 | }
|
---|
[1185] | 180 |
|
---|
[1239] | 181 | // RTCMv3 antenna XYZ
|
---|
| 182 | // ------------------
|
---|
| 183 | else if(rr == 1005)
|
---|
| 184 | {
|
---|
[1268] | 185 | _antList.push_back(t_antInfo());
|
---|
| 186 | _antList.back().type = t_antInfo::ARP;
|
---|
[2527] | 187 | _antList.back().xx = parser.antX * 1e-4;
|
---|
| 188 | _antList.back().yy = parser.antY * 1e-4;
|
---|
| 189 | _antList.back().zz = parser.antZ * 1e-4;
|
---|
[1268] | 190 | _antList.back().message = rr;
|
---|
[1807] | 191 |
|
---|
| 192 | // Remember station position for 1003 message decoding
|
---|
[2527] | 193 | _antXYZ[0] = parser.antX * 1e-4;
|
---|
| 194 | _antXYZ[1] = parser.antY * 1e-4;
|
---|
| 195 | _antXYZ[2] = parser.antZ * 1e-4;
|
---|
[1239] | 196 | }
|
---|
[1033] | 197 |
|
---|
[1239] | 198 | // RTCMv3 antenna XYZ-H
|
---|
| 199 | // --------------------
|
---|
| 200 | else if(rr == 1006)
|
---|
| 201 | {
|
---|
[1268] | 202 | _antList.push_back(t_antInfo());
|
---|
| 203 | _antList.back().type = t_antInfo::ARP;
|
---|
[2527] | 204 | _antList.back().xx = parser.antX * 1e-4;
|
---|
| 205 | _antList.back().yy = parser.antY * 1e-4;
|
---|
| 206 | _antList.back().zz = parser.antZ * 1e-4;
|
---|
| 207 | _antList.back().height = parser.antH * 1e-4;
|
---|
[1268] | 208 | _antList.back().height_f = true;
|
---|
| 209 | _antList.back().message = rr;
|
---|
[1807] | 210 |
|
---|
| 211 | // Remember station position for 1003 message decoding
|
---|
[2527] | 212 | _antXYZ[0] = parser.antX * 1e-4;
|
---|
| 213 | _antXYZ[1] = parser.antY * 1e-4;
|
---|
| 214 | _antXYZ[2] = parser.antZ * 1e-4;
|
---|
[1239] | 215 | }
|
---|
| 216 |
|
---|
[1021] | 217 | // GNSS Observations
|
---|
| 218 | // -----------------
|
---|
[1239] | 219 | else if (rr == 1 || rr == 2) {
|
---|
[1127] | 220 | decoded = true;
|
---|
[1021] | 221 |
|
---|
[2527] | 222 | if (!parser.init) {
|
---|
| 223 | HandleHeader(&parser);
|
---|
| 224 | parser.init = 1;
|
---|
[1021] | 225 | }
|
---|
| 226 |
|
---|
[1807] | 227 | // apply "GPS Integer L1 Pseudorange Modulus Ambiguity"
|
---|
| 228 | bool applyModulusAmb = false;
|
---|
| 229 | ///if (rr == 2) {
|
---|
| 230 | /// applyModulusAmb = true;
|
---|
| 231 | ///}
|
---|
| 232 |
|
---|
[1021] | 233 | if (rr == 2) {
|
---|
[1299] | 234 | emit(newMessage( (_staID + ": No valid RINEX! All values are modulo 299792.458!").toAscii(), true));
|
---|
[1021] | 235 | }
|
---|
| 236 |
|
---|
[2527] | 237 | for (int ii = 0; ii < parser.Data.numsats; ii++) {
|
---|
[1021] | 238 | p_obs obs = new t_obs();
|
---|
| 239 | _obsList.push_back(obs);
|
---|
[2527] | 240 | if (parser.Data.satellites[ii] <= PRN_GPS_END) {
|
---|
[1021] | 241 | obs->_o.satSys = 'G';
|
---|
[2527] | 242 | obs->_o.satNum = parser.Data.satellites[ii];
|
---|
[366] | 243 | }
|
---|
[2527] | 244 | else if (parser.Data.satellites[ii] <= PRN_GLONASS_END) {
|
---|
[1021] | 245 | obs->_o.satSys = 'R';
|
---|
[2527] | 246 | obs->_o.satNum = parser.Data.satellites[ii] - PRN_GLONASS_START + 1;
|
---|
| 247 | obs->_o.slot = parser.Data.channels[ii];
|
---|
[1021] | 248 | }
|
---|
[511] | 249 | else {
|
---|
[1021] | 250 | obs->_o.satSys = 'S';
|
---|
[2527] | 251 | obs->_o.satNum = parser.Data.satellites[ii] - PRN_WAAS_START + 20;
|
---|
[1021] | 252 | }
|
---|
[2527] | 253 | obs->_o.GPSWeek = parser.Data.week;
|
---|
| 254 | obs->_o.GPSWeeks = parser.Data.timeofweek / 1000.0;
|
---|
[1807] | 255 |
|
---|
| 256 | // Estimate "GPS Integer L1 Pseudorange Modulus Ambiguity"
|
---|
| 257 | // -------------------------------------------------------
|
---|
| 258 | double modulusAmb = 0;
|
---|
| 259 | if (applyModulusAmb) {
|
---|
| 260 | // Missing antenna coordinates: skip all data
|
---|
| 261 | if ( !_antXYZ[0] && !_antXYZ[1] && !_antXYZ[2] ) {
|
---|
| 262 | continue;
|
---|
| 263 | }
|
---|
| 264 |
|
---|
| 265 | ostringstream prns;
|
---|
| 266 | prns << obs->_o.satSys << setfill('0') << setw(2) << obs->_o.satNum;
|
---|
| 267 |
|
---|
| 268 | string prn = prns.str();
|
---|
| 269 |
|
---|
| 270 | // Missing ephemerides, skip satellite
|
---|
| 271 | if (_ephList.find(prn) == _ephList.end()) {
|
---|
| 272 | continue;
|
---|
| 273 | }
|
---|
| 274 |
|
---|
| 275 | const t_eph* eph = &(_ephList.find(prn)->second);
|
---|
| 276 |
|
---|
| 277 | double rho, xSat, ySat, zSat, clkSat, GPSWeeks_tot;
|
---|
| 278 | int GPSWeek_tot;
|
---|
| 279 | cmpRho(eph, _antXYZ[0], _antXYZ[1], _antXYZ[2],
|
---|
| 280 | obs->_o.GPSWeek, obs->_o.GPSWeeks,
|
---|
| 281 | rho, GPSWeek_tot, GPSWeeks_tot,
|
---|
| 282 | xSat, ySat, zSat, clkSat);
|
---|
| 283 |
|
---|
| 284 | const double CC = 299792458.0;
|
---|
| 285 |
|
---|
| 286 | int nn = static_cast<int>(rho / (CC * 0.001));
|
---|
| 287 |
|
---|
| 288 | modulusAmb = nn * CC * 0.001;
|
---|
| 289 | }
|
---|
[1021] | 290 |
|
---|
[1807] | 291 | // Loop over all data types
|
---|
| 292 | // ------------------------
|
---|
[2527] | 293 | for (int jj = 0; jj < parser.numdatatypesGPS; jj++) {
|
---|
[1021] | 294 | int v = 0;
|
---|
| 295 | // sepearated declaration and initalization of df and pos. Perlt
|
---|
| 296 | int df;
|
---|
| 297 | int pos;
|
---|
[2527] | 298 | df = parser.dataflag[jj];
|
---|
| 299 | pos = parser.datapos[jj];
|
---|
| 300 | if ( (parser.Data.dataflags[ii] & df)
|
---|
| 301 | && !isnan(parser.Data.measdata[ii][pos])
|
---|
| 302 | && !isinf(parser.Data.measdata[ii][pos])) {
|
---|
[1021] | 303 | v = 1;
|
---|
[511] | 304 | }
|
---|
[1021] | 305 | else {
|
---|
[2527] | 306 | df = parser.dataflagGPS[jj];
|
---|
| 307 | pos = parser.dataposGPS[jj];
|
---|
| 308 | if ( (parser.Data.dataflags[ii] & df)
|
---|
| 309 | && !isnan(parser.Data.measdata[ii][pos])
|
---|
| 310 | && !isinf(parser.Data.measdata[ii][pos])) {
|
---|
[1126] | 311 | v = 1;
|
---|
[1021] | 312 | }
|
---|
[627] | 313 | }
|
---|
[1021] | 314 | if (!v) {
|
---|
| 315 | continue;
|
---|
[627] | 316 | }
|
---|
[1021] | 317 | else
|
---|
| 318 | {
|
---|
[2527] | 319 | int isat = (parser.Data.satellites[ii] < 120
|
---|
| 320 | ? parser.Data.satellites[ii]
|
---|
| 321 | : parser.Data.satellites[ii] - 80);
|
---|
[1126] | 322 |
|
---|
[1021] | 323 | // variables df and pos are used consequently. Perlt
|
---|
| 324 | if (df & GNSSDF_C1DATA) {
|
---|
[2527] | 325 | obs->_o.C1 = parser.Data.measdata[ii][pos] + modulusAmb;
|
---|
[1021] | 326 | }
|
---|
| 327 | else if (df & GNSSDF_C2DATA) {
|
---|
[2527] | 328 | obs->_o.C2 = parser.Data.measdata[ii][pos] + modulusAmb;
|
---|
[1021] | 329 | }
|
---|
| 330 | else if (df & GNSSDF_P1DATA) {
|
---|
[2527] | 331 | obs->_o.P1 = parser.Data.measdata[ii][pos] + modulusAmb;
|
---|
[1021] | 332 | }
|
---|
| 333 | else if (df & GNSSDF_P2DATA) {
|
---|
[2527] | 334 | obs->_o.P2 = parser.Data.measdata[ii][pos] + modulusAmb;
|
---|
[1021] | 335 | }
|
---|
| 336 | else if (df & (GNSSDF_L1CDATA|GNSSDF_L1PDATA)) {
|
---|
[2527] | 337 | obs->_o.L1 = parser.Data.measdata[ii][pos] + modulusAmb;
|
---|
| 338 | obs->_o.SNR1 = parser.Data.snrL1[ii];
|
---|
| 339 | obs->_o.lock_timei_L1 = parser.lastlockGPSl1[isat];
|
---|
[1021] | 340 | }
|
---|
| 341 | else if (df & (GNSSDF_L2CDATA|GNSSDF_L2PDATA)) {
|
---|
[2527] | 342 | obs->_o.L2 = parser.Data.measdata[ii][pos] + modulusAmb;
|
---|
| 343 | obs->_o.SNR2 = parser.Data.snrL2[ii];
|
---|
| 344 | obs->_o.lock_timei_L2 = parser.lastlockGPSl2[isat];
|
---|
[1021] | 345 | }
|
---|
| 346 | else if (df & (GNSSDF_S1CDATA|GNSSDF_S1PDATA)) {
|
---|
[2527] | 347 | obs->_o.S1 = parser.Data.measdata[ii][pos];
|
---|
[1021] | 348 | }
|
---|
| 349 | else if (df & (GNSSDF_S2CDATA|GNSSDF_S2PDATA)) {
|
---|
[2527] | 350 | obs->_o.S2 = parser.Data.measdata[ii][pos];
|
---|
[1021] | 351 | }
|
---|
[627] | 352 | }
|
---|
[511] | 353 | }
|
---|
[366] | 354 | }
|
---|
[296] | 355 | }
|
---|
[1021] | 356 |
|
---|
| 357 | // GPS Ephemeris
|
---|
| 358 | // -------------
|
---|
| 359 | else if (rr == 1019) {
|
---|
| 360 | decoded = true;
|
---|
[2527] | 361 | gpsephemeris* ep = new gpsephemeris(parser.ephemerisGPS);
|
---|
[1021] | 362 | emit newGPSEph(ep);
|
---|
| 363 | }
|
---|
| 364 |
|
---|
| 365 | // GLONASS Ephemeris
|
---|
| 366 | // -----------------
|
---|
| 367 | else if (rr == 1020) {
|
---|
| 368 | decoded = true;
|
---|
[2527] | 369 | glonassephemeris* ep = new glonassephemeris(parser.ephemerisGLONASS);
|
---|
[1021] | 370 | emit newGlonassEph(ep);
|
---|
| 371 | }
|
---|
[296] | 372 | }
|
---|
| 373 | }
|
---|
| 374 | }
|
---|
[2527] | 375 | if (!_rawFile && _mode == unknown && decoded) {
|
---|
[2387] | 376 | _mode = observations;
|
---|
[1021] | 377 | }
|
---|
[296] | 378 | }
|
---|
[1021] | 379 |
|
---|
| 380 | if (decoded) {
|
---|
| 381 | return success;
|
---|
[658] | 382 | }
|
---|
| 383 | else {
|
---|
[1021] | 384 | return failure;
|
---|
[658] | 385 | }
|
---|
[296] | 386 | }
|
---|
[1807] | 387 |
|
---|
| 388 | // Store ephemerides
|
---|
| 389 | ////////////////////////////////////////////////////////////////////////////////////////
|
---|
| 390 | bool RTCM3Decoder::storeEph(const gpsephemeris& gpseph) {
|
---|
| 391 | t_ephGPS eph; eph.set(&gpseph);
|
---|
| 392 |
|
---|
| 393 | return storeEph(eph);
|
---|
| 394 | }
|
---|
| 395 |
|
---|
| 396 |
|
---|
| 397 | bool RTCM3Decoder::storeEph(const t_ephGPS& gpseph) {
|
---|
[2463] | 398 | const double secPerWeek = 7.0 * 24.0 * 3600.0;
|
---|
[1807] | 399 | double weekold = 0.0;
|
---|
[2463] | 400 | double weeknew = gpseph.GPSweek() + gpseph.GPSweeks() / secPerWeek;
|
---|
[1807] | 401 | if ( _ephList.find(gpseph.prn()) != _ephList.end() ) {
|
---|
| 402 | weekold = _ephList.find(gpseph.prn())->second.GPSweek()
|
---|
[2463] | 403 | + _ephList.find(gpseph.prn())->second.GPSweeks() / secPerWeek;
|
---|
[1807] | 404 | }
|
---|
| 405 |
|
---|
[2463] | 406 | if ( weeknew - weekold > 1.0/secPerWeek ) {
|
---|
[1807] | 407 | _ephList[gpseph.prn()] = gpseph;
|
---|
| 408 |
|
---|
| 409 | return true;
|
---|
| 410 | }
|
---|
| 411 |
|
---|
| 412 | return false;
|
---|
| 413 | }
|
---|