source: ntrip/trunk/BNC/src/RTCM3/RTCM3coDecoder.cpp@ 6141

Last change on this file since 6141 was 6141, checked in by mervart, 10 years ago
File size: 12.0 KB
Line 
1// Part of BNC, a utility for retrieving decoding and
2// converting GNSS data streams from NTRIP broadcasters.
3//
4// Copyright (C) 2007
5// German Federal Agency for Cartography and Geodesy (BKG)
6// http://www.bkg.bund.de
7// Czech Technical University Prague, Department of Geodesy
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.
24
25/* -------------------------------------------------------------------------
26 * BKG NTRIP Client
27 * -------------------------------------------------------------------------
28 *
29 * Class: RTCM3coDecoder
30 *
31 * Purpose: RTCM3 Clock Orbit Decoder
32 *
33 * Author: L. Mervart
34 *
35 * Created: 05-May-2008
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <stdio.h>
42#include <math.h>
43
44#include "RTCM3coDecoder.h"
45#include "bncutils.h"
46#include "bncrinex.h"
47#include "bnccore.h"
48#include "bncsettings.h"
49#include "rtcm3torinex.h"
50#include "bnctime.h"
51
52using namespace std;
53
54// Constructor
55////////////////////////////////////////////////////////////////////////////
56RTCM3coDecoder::RTCM3coDecoder(const QString& staID) {
57
58 _staID = staID;
59
60 // File Output
61 // -----------
62 bncSettings settings;
63 QString path = settings.value("corrPath").toString();
64 if (!path.isEmpty()) {
65 expandEnvVar(path);
66 if ( path.length() > 0 && path[path.length()-1] != QDir::separator() ) {
67 path += QDir::separator();
68 }
69 _fileNameSkl = path + staID;
70 }
71 _out = 0;
72
73 qRegisterMetaType<bncTime>("bncTime");
74 qRegisterMetaType< QList<t_orbCorr> >("QList:t_orbCorr");
75 qRegisterMetaType< QList<t_clkCorr> >("QList:t_clkCorr");
76
77 connect(this, SIGNAL(newOrbCorrections(QList<t_orbCorr>)),
78 BNC_CORE, SLOT(slotNewOrbCorrections(QList<t_orbCorr>)));
79
80 connect(this, SIGNAL(newClkCorrections(QList<t_clkCorr>)),
81 BNC_CORE, SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
82
83 connect(this, SIGNAL(providerIDChanged(QString)),
84 BNC_CORE, SIGNAL(providerIDChanged(QString)));
85
86 connect(this, SIGNAL(newMessage(QByteArray,bool)),
87 BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
88
89 memset(&_co, 0, sizeof(_co));
90 memset(&_bias, 0, sizeof(_bias));
91
92 _providerID[0] = -1;
93 _providerID[1] = -1;
94 _providerID[2] = -1;
95}
96
97// Destructor
98////////////////////////////////////////////////////////////////////////////
99RTCM3coDecoder::~RTCM3coDecoder() {
100 delete _out;
101}
102
103// Reopen Output File
104////////////////////////////////////////////////////////////////////////
105void RTCM3coDecoder::reopen() {
106
107 if (!_fileNameSkl.isEmpty()) {
108
109 bncSettings settings;
110
111 QDateTime datTim = currentDateAndTimeGPS();
112
113 QString hlpStr = bncRinex::nextEpochStr(datTim,
114 settings.value("corrIntr").toString());
115
116 QString fileNameHlp = _fileNameSkl
117 + QString("%1").arg(datTim.date().dayOfYear(), 3, 10, QChar('0'))
118 + hlpStr + datTim.toString(".yyC");
119
120 if (_fileName == fileNameHlp) {
121 return;
122 }
123 else {
124 _fileName = fileNameHlp;
125 }
126
127 delete _out;
128 if ( Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked) {
129 _out = new ofstream( _fileName.toAscii().data(), ios_base::out | ios_base::app );
130 }
131 else {
132 _out = new ofstream( _fileName.toAscii().data() );
133 }
134 }
135}
136
137//
138////////////////////////////////////////////////////////////////////////////
139t_irc RTCM3coDecoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
140
141 errmsg.clear();
142
143 _buffer.append(QByteArray(buffer,bufLen));
144
145 t_irc retCode = failure;
146
147 while(_buffer.size()) {
148
149 int bytesused = 0;
150 struct ClockOrbit co_sav;
151 memcpy(&co_sav, &_co, sizeof(co_sav)); // save state
152
153 GCOB_RETURN irc = GetSSR(&_co, &_bias, 0, 0, _buffer.data(),
154 _buffer.size(), &bytesused);
155
156 if (irc <= -30) { // not enough data - restore state and exit loop
157 memcpy(&_co, &co_sav, sizeof(co_sav));
158 break;
159 }
160
161 else if (irc < 0) { // error - skip 1 byte and retry
162 memset(&_co, 0, sizeof(_co));
163 memset(&_bias, 0, sizeof(_bias));
164 _buffer = _buffer.mid(bytesused ? bytesused : 1);
165 }
166
167 else { // OK or MESSAGEFOLLOWS
168 _buffer = _buffer.mid(bytesused);
169
170 if ( (irc == GCOBR_OK || irc == GCOBR_MESSAGEFOLLOWS ) &&
171 (_co.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || _co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0 ||
172 _bias.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || _bias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) ) {
173
174 reopen();
175
176 // Guess GPS week and sec using system time
177 // ----------------------------------------
178 int GPSweek;
179 double GPSweeksHlp;
180 currentGPSWeeks(GPSweek, GPSweeksHlp);
181
182 // Correction Epoch from GPSEpochTime
183 // ----------------------------------
184 if (_co.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || _bias.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
185 int GPSEpochTime = (_co.NumberOfSat[CLOCKORBIT_SATGPS] > 0) ?
186 _co.EpochTime[CLOCKORBIT_SATGPS] : _bias.EpochTime[CLOCKORBIT_SATGPS];
187 if (GPSweeksHlp > GPSEpochTime + 86400.0) {
188 GPSweek += 1;
189 }
190 else if (GPSweeksHlp < GPSEpochTime - 86400.0) {
191 GPSweek -= 1;
192 }
193 _lastTime.set(GPSweek, double(GPSEpochTime));
194 }
195
196 // Correction Epoch from Glonass Epoch
197 // -----------------------------------
198 else if (_co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0 || _bias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0){
199 int GLONASSEpochTime = (_co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) ?
200 _co.EpochTime[CLOCKORBIT_SATGLONASS] : _bias.EpochTime[CLOCKORBIT_SATGLONASS];
201
202 // Second of day (GPS time) from Glonass Epoch
203 // -------------------------------------------
204 QDate date = dateAndTimeFromGPSweek(GPSweek, GPSweeksHlp).date();
205 int leapSecond = gnumleap(date.year(), date.month(), date.day());
206 int GPSDaySec = GLONASSEpochTime - 3 * 3600 + leapSecond;
207
208 int weekDay = int(GPSweeksHlp/86400.0);
209 int GPSDaySecHlp = int(GPSweeksHlp) - weekDay * 86400;
210
211 // Handle the difference between system clock and correction epoch
212 // ---------------------------------------------------------------
213 if (GPSDaySec < GPSDaySecHlp - 3600) {
214 weekDay += 1;
215 if (weekDay > 6) {
216 weekDay = 0;
217 GPSweek += 1;
218 }
219 }
220 else if (GPSDaySec > GPSDaySecHlp + 3600) {
221 weekDay -= 1;
222 if (weekDay < 0) {
223 weekDay = 6;
224 GPSweek -= 1;
225 }
226 }
227 _lastTime.set(GPSweek, weekDay * 86400.0 + GPSDaySec);
228 }
229
230 checkProviderID();
231
232 sendResults();
233
234 retCode = success;
235
236 memset(&_co, 0, sizeof(_co));
237 memset(&_bias, 0, sizeof(_bias));
238 }
239 }
240 }
241
242 return retCode;
243}
244
245//
246////////////////////////////////////////////////////////////////////////////
247void RTCM3coDecoder::sendResults() {
248
249 QList<t_orbCorr> orbCorrections;
250 QList<t_clkCorr> clkCorrections;
251
252 // Loop over all satellites (GPS and Glonass)
253 // ------------------------------------------
254 for (unsigned ii = 0; ii < CLOCKORBIT_NUMGPS + _co.NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
255 char sysCh = ' ';
256 if (ii < _co.NumberOfSat[CLOCKORBIT_SATGPS]) {
257 sysCh = 'G';
258 }
259 else if (ii >= CLOCKORBIT_NUMGPS) {
260 sysCh = 'R';
261 }
262 else {
263 continue;
264 }
265
266 // Orbit correction
267 // ----------------
268 if ( _co.messageType == COTYPE_GPSCOMBINED ||
269 _co.messageType == COTYPE_GLONASSCOMBINED ||
270 _co.messageType == COTYPE_GPSORBIT ||
271 _co.messageType == COTYPE_GLONASSORBIT ) {
272
273 t_orbCorr orbCorr;
274 orbCorr._prn.set(sysCh, _co.Sat[ii].ID);
275 orbCorr._staID = _staID.toAscii().data();
276 orbCorr._iod = _co.Sat[ii].IOD;
277 orbCorr._time = _lastTime;
278 orbCorr._system = 'R';
279 orbCorr._xr[0] = _co.Sat[ii].Orbit.DeltaRadial;
280 orbCorr._xr[1] = _co.Sat[ii].Orbit.DeltaAlongTrack;
281 orbCorr._xr[2] = _co.Sat[ii].Orbit.DeltaCrossTrack;
282 orbCorr._dotXr[0] = _co.Sat[ii].Orbit.DotDeltaRadial;
283 orbCorr._dotXr[1] = _co.Sat[ii].Orbit.DotDeltaAlongTrack;
284 orbCorr._dotXr[2] = _co.Sat[ii].Orbit.DotDeltaCrossTrack;
285
286 orbCorrections.push_back(orbCorr);
287
288 _IODs[orbCorr._prn.toString()] = _co.Sat[ii].IOD;
289 }
290
291 if ( _co.messageType == COTYPE_GPSCOMBINED ||
292 _co.messageType == COTYPE_GLONASSCOMBINED ||
293 _co.messageType == COTYPE_GPSCLOCK ||
294 _co.messageType == COTYPE_GLONASSCLOCK ) {
295
296 t_clkCorr clkCorr;
297 clkCorr._prn.set(sysCh, _co.Sat[ii].ID);
298 clkCorr._staID = _staID.toAscii().data();
299 clkCorr._time = _lastTime;
300 clkCorr._dClk = _co.Sat[ii].Clock.DeltaA0,
301 clkCorr._dotDClk = _co.Sat[ii].Clock.DeltaA1,
302 clkCorr._dotDotDClk = _co.Sat[ii].Clock.DeltaA2;
303 clkCorr._clkPartial = 0.0;
304
305 if (_IODs.contains(clkCorr._prn.toString())) {
306 clkCorr._iod = _IODs[clkCorr._prn.toString()];
307 clkCorrections.push_back(clkCorr);
308 }
309 }
310
311 // High-Resolution Clocks
312 // ----------------------
313 if ( _co.messageType == COTYPE_GPSHR ||
314 _co.messageType == COTYPE_GLONASSHR ) {
315 }
316 }
317
318 // Loop over all satellites (GPS and Glonass)
319 // ------------------------------------------
320 QList<t_satBias> satBiases;
321 for (unsigned ii = 0; ii < CLOCKORBIT_NUMGPS + _bias.NumberOfSat[CLOCKORBIT_SATGLONASS]; ii++) {
322 char sysCh = ' ';
323 if (ii < _bias.NumberOfSat[CLOCKORBIT_SATGPS]) {
324 sysCh = 'G';
325 }
326 else if (ii >= CLOCKORBIT_NUMGPS) {
327 sysCh = 'R';
328 }
329 else {
330 continue;
331 }
332 t_satBias satBias;
333 satBias._prn.set(sysCh, _bias.Sat[ii].ID);
334 satBias._time = _lastTime;
335 satBias._nx = 0;
336 satBias._jumpCount = 0;
337 for (unsigned jj = 0; jj < _bias.Sat[ii].NumberOfCodeBiases; jj++) {
338 }
339 }
340
341 if (orbCorrections.size() > 0) {
342 emit newOrbCorrections(orbCorrections);
343 }
344 if (clkCorrections.size() > 0) {
345 emit newClkCorrections(clkCorrections);
346 }
347 if (_out) {
348 QListIterator<t_orbCorr> itOrb(orbCorrections);
349 while (itOrb.hasNext()) {
350 const t_orbCorr& orbCorr = itOrb.next();
351 *_out << "O " << orbCorr.toString() << endl;
352 }
353 QListIterator<t_clkCorr> itClk(clkCorrections);
354 while (itClk.hasNext()) {
355 const t_clkCorr& clkCorr = itClk.next();
356 *_out << "C " << clkCorr.toString() << endl;
357 }
358 _out->flush();
359 }
360}
361
362//
363////////////////////////////////////////////////////////////////////////////
364void RTCM3coDecoder::checkProviderID() {
365
366 if (_co.SSRProviderID == 0 && _co.SSRSolutionID == 0 && _co.SSRIOD == 0) {
367 return;
368 }
369
370 int newProviderID[3];
371 newProviderID[0] = _co.SSRProviderID;
372 newProviderID[1] = _co.SSRSolutionID;
373 newProviderID[2] = _co.SSRIOD;
374
375 bool alreadySet = false;
376 bool different = false;
377
378 for (unsigned ii = 0; ii < 3; ii++) {
379 if (_providerID[ii] != -1) {
380 alreadySet = true;
381 }
382 if (_providerID[ii] != newProviderID[ii]) {
383 different = true;
384 }
385 _providerID[ii] = newProviderID[ii];
386 }
387
388 if (alreadySet && different) {
389 emit newMessage("RTCM3coDecoder: Provider Changed " + _staID.toAscii() + "\n", true);
390 emit providerIDChanged(_staID);
391 }
392}
Note: See TracBrowser for help on using the repository browser.