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