source: ntrip/trunk/BNC/src/upload/bncrtnetuploadcaster.cpp@ 6194

Last change on this file since 6194 was 6139, checked in by mervart, 11 years ago
File size: 24.3 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Server
3 * -------------------------------------------------------------------------
4 *
5 * Class: bncRtnetUploadCaster
6 *
7 * Purpose: Connection to NTRIP Caster
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Mar-2011
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <math.h>
18#include "bncrtnetuploadcaster.h"
19#include "bncsettings.h"
20#include "bncephuser.h"
21#include "bncclockrinex.h"
22#include "bncsp3.h"
23
24extern "C" {
25# include "rtcm3torinex.h"
26}
27
28using namespace std;
29
30// Constructor
31////////////////////////////////////////////////////////////////////////////
32bncRtnetUploadCaster::bncRtnetUploadCaster(const QString& mountpoint,
33 const QString& outHost, int outPort,
34 const QString& password,
35 const QString& crdTrafo, bool CoM,
36 const QString& sp3FileName,
37 const QString& rnxFileName,
38 int PID, int SID, int IOD, int iRow) :
39 bncUploadCaster(mountpoint, outHost, outPort, password, iRow, 0) {
40
41 if (!outHost.isEmpty()) {
42 _casterID += outHost;
43 }
44 if (!crdTrafo.isEmpty()) {
45 _casterID += " " + crdTrafo;
46 }
47 if (!sp3FileName.isEmpty()) {
48 _casterID += " " + sp3FileName;
49 }
50 if (!rnxFileName.isEmpty()) {
51 _casterID += " " + rnxFileName;
52 }
53
54 _crdTrafo = crdTrafo;
55 _CoM = CoM;
56 _PID = PID;
57 _SID = SID;
58 _IOD = IOD;
59
60 // Member that receives the ephemeris
61 // ----------------------------------
62 _ephUser = new bncEphUser();
63
64 bncSettings settings;
65 QString intr = settings.value("uploadIntr").toString();
66
67 _samplRtcmEphCorr = settings.value("uploadSamplRtcmEphCorr").toDouble();
68 int samplClkRnx = settings.value("uploadSamplClkRnx").toInt();
69 int samplSp3 = settings.value("uploadSamplSp3").toInt() * 60;
70
71 if (_samplRtcmEphCorr == 0.0) {
72 _usedEph = 0;
73 }
74 else {
75 _usedEph = new QMap<QString, t_eph*>;
76 }
77
78 // RINEX writer
79 // ------------
80 if (!rnxFileName.isEmpty()) {
81 _rnx = new bncClockRinex(rnxFileName, intr, samplClkRnx);
82 }
83 else {
84 _rnx = 0;
85 }
86
87 // SP3 writer
88 // ----------
89 if (!sp3FileName.isEmpty()) {
90 _sp3 = new bncSP3(sp3FileName, intr, samplSp3);
91 }
92 else {
93 _sp3 = 0;
94 }
95
96 // Set Transformation Parameters
97 // -----------------------------
98 if (_crdTrafo == "ETRF2000") {
99 _dx = 0.0521;
100 _dy = 0.0493;
101 _dz = -0.0585;
102 _dxr = 0.0001;
103 _dyr = 0.0001;
104 _dzr = -0.0018;
105 _ox = 0.000891;
106 _oy = 0.005390;
107 _oz = -0.008712;
108 _oxr = 0.000081;
109 _oyr = 0.000490;
110 _ozr = -0.000792;
111 _sc = 1.34;
112 _scr = 0.08;
113 _t0 = 2000.0;
114 }
115 else if (_crdTrafo == "NAD83") {
116 _dx = 0.99343;
117 _dy = -1.90331;
118 _dz = -0.52655;
119 _dxr = 0.00079;
120 _dyr = -0.00060;
121 _dzr = -0.00134;
122 _ox = -0.02591467;
123 _oy = -0.00942645;
124 _oz = -0.01159935;
125 _oxr = -0.00006667;
126 _oyr = 0.00075744;
127 _ozr = 0.00005133;
128 _sc = 1.71504;
129 _scr = -0.10201;
130 _t0 = 1997.0;
131 }
132 else if (_crdTrafo == "GDA94") {
133 _dx = -0.08468;
134 _dy = -0.01942;
135 _dz = 0.03201;
136 _dxr = 0.00142;
137 _dyr = 0.00134;
138 _dzr = 0.00090;
139 _ox = 0.0004254;
140 _oy = -0.0022578;
141 _oz = -0.0024015;
142 _oxr = -0.0015461;
143 _oyr = -0.0011820;
144 _ozr = -0.0011551;
145 _sc = 9.710;
146 _scr = 0.109;
147 _t0 = 1994.0;
148 }
149 else if (_crdTrafo == "SIRGAS2000") {
150 _dx = 0.0020;
151 _dy = 0.0041;
152 _dz = 0.0039;
153 _dxr = 0.0000;
154 _dyr = 0.0000;
155 _dzr = 0.0000;
156 _ox = 0.000170;
157 _oy = -0.000030;
158 _oz = 0.000070;
159 _oxr = 0.000000;
160 _oyr = 0.000000;
161 _ozr = 0.000000;
162 _sc = -1.000;
163 _scr = 0.000;
164 _t0 = 0000.0;
165 }
166 else if (_crdTrafo == "SIRGAS95") {
167 _dx = 0.0077;
168 _dy = 0.0058;
169 _dz = -0.0138;
170 _dxr = 0.0000;
171 _dyr = 0.0000;
172 _dzr = 0.0000;
173 _ox = 0.000000;
174 _oy = 0.000000;
175 _oz = -0.000030;
176 _oxr = 0.000000;
177 _oyr = 0.000000;
178 _ozr = 0.000000;
179 _sc = 1.570;
180 _scr = 0.000;
181 _t0 = 0000.0;
182 }
183 else if (_crdTrafo == "DREF91") {
184 _dx = -0.0118;
185 _dy = 0.1432;
186 _dz = -0.1117;
187 _dxr = 0.0001;
188 _dyr = 0.0001;
189 _dzr = -0.0018;
190 _ox = 0.003291;
191 _oy = 0.006190;
192 _oz = -0.011012;
193 _oxr = 0.000081;
194 _oyr = 0.000490;
195 _ozr = -0.000792;
196 _sc = 12.24;
197 _scr = 0.08;
198 _t0 = 2000.0;
199 }
200 else if (_crdTrafo == "Custom") {
201 _dx = settings.value("trafo_dx").toDouble();
202 _dy = settings.value("trafo_dy").toDouble();
203 _dz = settings.value("trafo_dz").toDouble();
204 _dxr = settings.value("trafo_dxr").toDouble();
205 _dyr = settings.value("trafo_dyr").toDouble();
206 _dzr = settings.value("trafo_dzr").toDouble();
207 _ox = settings.value("trafo_ox").toDouble();
208 _oy = settings.value("trafo_oy").toDouble();
209 _oz = settings.value("trafo_oz").toDouble();
210 _oxr = settings.value("trafo_oxr").toDouble();
211 _oyr = settings.value("trafo_oyr").toDouble();
212 _ozr = settings.value("trafo_ozr").toDouble();
213 _sc = settings.value("trafo_sc").toDouble();
214 _scr = settings.value("trafo_scr").toDouble();
215 _t0 = settings.value("trafo_t0").toDouble();
216 }
217}
218
219// Destructor
220////////////////////////////////////////////////////////////////////////////
221bncRtnetUploadCaster::~bncRtnetUploadCaster() {
222 if (isRunning()) {
223 wait();
224 }
225 delete _rnx;
226 delete _sp3;
227 delete _ephUser;
228 delete _usedEph;
229}
230
231//
232////////////////////////////////////////////////////////////////////////////
233void bncRtnetUploadCaster::decodeRtnetStream(char* buffer, int bufLen) {
234
235 QMutexLocker locker(&_mutex);
236
237 // Append to internal buffer
238 // -------------------------
239 _rtnetStreamBuffer.append(QByteArray(buffer, bufLen));
240
241 // Select buffer part that contains last epoch
242 // -------------------------------------------
243 QStringList lines;
244 int iEpoBeg = _rtnetStreamBuffer.lastIndexOf('*'); // begin of last epoch
245 if (iEpoBeg == -1) {
246 _rtnetStreamBuffer.clear();
247 return;
248 }
249 _rtnetStreamBuffer = _rtnetStreamBuffer.mid(iEpoBeg);
250
251 int iEpoEnd = _rtnetStreamBuffer.lastIndexOf("EOE"); // end of last epoch
252 if (iEpoEnd == -1) {
253 return;
254 }
255 else {
256 lines = _rtnetStreamBuffer.left(iEpoEnd).split('\n', QString::SkipEmptyParts);
257 _rtnetStreamBuffer = _rtnetStreamBuffer.mid(iEpoEnd+3);
258 }
259
260 if (lines.size() < 2) {
261 return;
262 }
263
264 // Keep the last unfinished line in buffer
265 // ---------------------------------------
266 int iLastEOL = _rtnetStreamBuffer.lastIndexOf('\n');
267 if (iLastEOL != -1) {
268 _rtnetStreamBuffer = _rtnetStreamBuffer.mid(iLastEOL+1);
269 }
270
271 // Read first line (with epoch time)
272 // ---------------------------------
273 QTextStream in(lines[0].toAscii());
274 QString hlp;
275 int year, month, day, hour, min;
276 double sec;
277 in >> hlp >> year >> month >> day >> hour >> min >> sec;
278 bncTime epoTime; epoTime.set( year, month, day, hour, min, sec);
279
280 emit(newMessage("bncRtnetUploadCaster: decode " +
281 QByteArray(epoTime.datestr().c_str()) + " " +
282 QByteArray(epoTime.timestr().c_str()) + " " +
283 _casterID.toAscii(), false));
284
285 struct ClockOrbit co;
286 memset(&co, 0, sizeof(co));
287 co.EpochTime[CLOCKORBIT_SATGPS] = static_cast<int>(epoTime.gpssec());
288 double gt = epoTime.gpssec() + 3 * 3600 - gnumleap(year, month, day);
289 co.EpochTime[CLOCKORBIT_SATGLONASS] = static_cast<int>(fmod(gt, 86400.0));
290
291 co.Supplied[COBOFS_CLOCK] = 1;
292 co.Supplied[COBOFS_ORBIT] = 1;
293 co.SatRefDatum = DATUM_ITRF;
294 co.SSRIOD = _IOD;
295 co.SSRProviderID = _PID; // 256 .. BKG, 257 ... EUREF
296 co.SSRSolutionID = _SID;
297
298 struct CodeBias bias;
299 memset(&bias, 0, sizeof(bias));
300 bias.EpochTime[CLOCKORBIT_SATGPS] = co.EpochTime[CLOCKORBIT_SATGPS];
301 bias.EpochTime[CLOCKORBIT_SATGLONASS] = co.EpochTime[CLOCKORBIT_SATGLONASS];
302
303 // Default Update Interval
304 // -----------------------
305 int clkUpdInd = 2; // 5 sec
306 int ephUpdInd = clkUpdInd; // default
307 if (_samplRtcmEphCorr == 10.0) {
308 ephUpdInd = 3;
309 }
310 else if (_samplRtcmEphCorr == 15.0) {
311 ephUpdInd = 4;
312 }
313 else if (_samplRtcmEphCorr == 30.0) {
314 ephUpdInd = 5;
315 }
316 else if (_samplRtcmEphCorr == 60.0) {
317 ephUpdInd = 6;
318 }
319 else if (_samplRtcmEphCorr == 120.0) {
320 ephUpdInd = 7;
321 }
322 else if (_samplRtcmEphCorr == 240.0) {
323 ephUpdInd = 8;
324 }
325 else if (_samplRtcmEphCorr == 300.0) {
326 ephUpdInd = 9;
327 }
328 else if (_samplRtcmEphCorr == 600.0) {
329 ephUpdInd = 10;
330 }
331 else if (_samplRtcmEphCorr == 900.0) {
332 ephUpdInd = 11;
333 }
334
335 co.UpdateInterval = clkUpdInd;
336 bias.UpdateInterval = clkUpdInd;
337
338 for (int ii = 1; ii < lines.size(); ii++) {
339
340 QString prn;
341 ColumnVector rtnAPC;
342 ColumnVector rtnVel;
343 ColumnVector rtnCoM;
344 double rtnClk;
345
346 QTextStream in(lines[ii].toAscii());
347
348 in >> prn;
349
350 t_eph* eph = 0;
351 const bncEphUser::t_ephPair* ephPair = _ephUser->ephPair(prn);
352 if (ephPair) {
353
354 eph = ephPair->last;
355
356 // Use previous ephemeris if the last one is too recent
357 // ----------------------------------------------------
358 const int MINAGE = 60; // seconds
359 if (ephPair->prev && eph->receptDateTime().isValid() &&
360 eph->receptDateTime().secsTo(currentDateAndTimeGPS()) < MINAGE) {
361 eph = ephPair->prev;
362 }
363
364 // Make sure the clock messages refer to same IOD as orbit messages
365 // ----------------------------------------------------------------
366 if (_usedEph) {
367 if (fmod(epoTime.gpssec(), _samplRtcmEphCorr) == 0.0) {
368 (*_usedEph)[prn] = eph;
369 }
370 else {
371 eph = 0;
372 if (_usedEph->contains(prn)) {
373 t_eph* usedEph = _usedEph->value(prn);
374 if (usedEph == ephPair->last) {
375 eph = ephPair->last;
376 }
377 else if (usedEph == ephPair->prev) {
378 eph = ephPair->prev;
379 }
380 }
381 }
382 }
383 }
384
385 if (eph) {
386
387 QMap<QString, double> codeBiases;
388
389 while (true) {
390 QString key;
391 int numVal = 0;
392 in >> key >> numVal;
393 if (in.status() != QTextStream::Ok) {
394 break;
395 }
396 if (key == "APC") {
397 rtnAPC.ReSize(3);
398 in >> rtnAPC[0] >> rtnAPC[1] >> rtnAPC[2];
399 }
400 else if (key == "Clk") {
401 in >> rtnClk;
402 }
403 else if (key == "Vel") {
404 rtnVel.ReSize(3);
405 in >> rtnVel[0] >> rtnVel[1] >> rtnVel[2];
406 }
407 else if (key == "CoM") {
408 rtnCoM.ReSize(3);
409 in >> rtnCoM[0] >> rtnCoM[1] >> rtnCoM[2];
410 }
411 else if (key == "CodeBias") {
412 for (int ii = 0; ii < numVal; ii++) {
413 QString type;
414 double value;
415 in >> type >> value;
416 codeBiases[type] = value;
417 }
418 }
419 else {
420 for (int ii = 0; ii < numVal; ii++) {
421 double dummy;
422 in >> dummy;
423 }
424 }
425 }
426 struct ClockOrbit::SatData* sd = 0;
427 if (prn[0] == 'G') {
428 sd = co.Sat + co.NumberOfSat[CLOCKORBIT_SATGPS];
429 ++co.NumberOfSat[CLOCKORBIT_SATGPS];
430 }
431 else if (prn[0] == 'R') {
432 sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfSat[CLOCKORBIT_SATGLONASS];
433 ++co.NumberOfSat[CLOCKORBIT_SATGLONASS];
434 }
435 if (sd) {
436 QString outLine;
437 processSatellite(eph, epoTime.gpsw(), epoTime.gpssec(), prn,
438 rtnAPC, rtnClk, rtnVel, rtnCoM, sd, outLine);
439 }
440
441 struct CodeBias::BiasSat* biasSat = 0;
442 if (prn[0] == 'G') {
443 biasSat = bias.Sat + bias.NumberOfSat[CLOCKORBIT_SATGPS];
444 ++bias.NumberOfSat[CLOCKORBIT_SATGPS];
445 }
446 else if (prn[0] == 'R') {
447 biasSat = bias.Sat + CLOCKORBIT_NUMGPS + bias.NumberOfSat[CLOCKORBIT_SATGLONASS];
448 ++bias.NumberOfSat[CLOCKORBIT_SATGLONASS];
449 }
450
451 // Code Biases
452 // -----------
453 if (biasSat) {
454 biasSat->ID = prn.mid(1).toInt();
455 biasSat->NumberOfCodeBiases = 0;
456 if (prn[0] == 'G') {
457 QMapIterator<QString, double> it(codeBiases);
458 while (it.hasNext()) {
459 it.next();
460 if (it.key() == "1C") {
461 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
462 biasSat->NumberOfCodeBiases += 1;
463 biasSat->Biases[ii].Type = CODETYPEGPS_L1_CA;
464 biasSat->Biases[ii].Bias = it.value();
465 }
466 else if (it.key() == "1C") {
467 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
468 biasSat->NumberOfCodeBiases += 1;
469 biasSat->Biases[ii].Type = CODETYPEGPS_L1_CA;
470 biasSat->Biases[ii].Bias = it.value();
471 }
472 else if (it.key() == "1P") {
473 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
474 biasSat->NumberOfCodeBiases += 1;
475 biasSat->Biases[ii].Type = CODETYPEGPS_L1_P;
476 biasSat->Biases[ii].Bias = it.value();
477 }
478 else if (it.key() == "1W") {
479 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
480 biasSat->NumberOfCodeBiases += 1;
481 biasSat->Biases[ii].Type = CODETYPEGPS_L1_Z;
482 biasSat->Biases[ii].Bias = it.value();
483 }
484 else if (it.key() == "2C") {
485 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
486 biasSat->NumberOfCodeBiases += 1;
487 biasSat->Biases[ii].Type = CODETYPEGPS_L2_CA;
488 biasSat->Biases[ii].Bias = it.value();
489 }
490 else if (it.key() == "2D") {
491 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
492 biasSat->NumberOfCodeBiases += 1;
493 biasSat->Biases[ii].Type = CODETYPEGPS_SEMI_CODELESS;
494 biasSat->Biases[ii].Bias = it.value();
495 }
496 else if (it.key() == "2S") {
497 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
498 biasSat->NumberOfCodeBiases += 1;
499 biasSat->Biases[ii].Type = CODETYPEGPS_L2_CM;
500 biasSat->Biases[ii].Bias = it.value();
501 }
502 else if (it.key() == "2L") {
503 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
504 biasSat->NumberOfCodeBiases += 1;
505 biasSat->Biases[ii].Type = CODETYPEGPS_L2_CL;
506 biasSat->Biases[ii].Bias = it.value();
507 }
508 else if (it.key() == "2X") {
509 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
510 biasSat->NumberOfCodeBiases += 1;
511 biasSat->Biases[ii].Type = CODETYPEGPS_L2_CML;
512 biasSat->Biases[ii].Bias = it.value();
513 }
514 else if (it.key() == "2P") {
515 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
516 biasSat->NumberOfCodeBiases += 1;
517 biasSat->Biases[ii].Type = CODETYPEGPS_L2_P;
518 biasSat->Biases[ii].Bias = it.value();
519 }
520 else if (it.key() == "2W") {
521 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
522 biasSat->NumberOfCodeBiases += 1;
523 biasSat->Biases[ii].Type = CODETYPEGPS_L2_Z;
524 biasSat->Biases[ii].Bias = it.value();
525 }
526 else if (it.key() == "5I") {
527 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
528 biasSat->NumberOfCodeBiases += 1;
529 biasSat->Biases[ii].Type = CODETYPEGPS_L5_I;
530 biasSat->Biases[ii].Bias = it.value();
531 }
532 else if (it.key() == "5Q") {
533 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
534 biasSat->NumberOfCodeBiases += 1;
535 biasSat->Biases[ii].Type = CODETYPEGPS_L5_Q;
536 biasSat->Biases[ii].Bias = it.value();
537 }
538 }
539 }
540 else if (prn[0] == 'R') {
541 QMapIterator<QString, double> it(codeBiases);
542 while (it.hasNext()) {
543 it.next();
544 if (it.key() == "1C") {
545 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
546 biasSat->NumberOfCodeBiases += 1;
547 biasSat->Biases[ii].Type = CODETYPEGLONASS_L1_CA;
548 biasSat->Biases[ii].Bias = it.value();
549 }
550 else if (it.key() == "1P") {
551 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
552 biasSat->NumberOfCodeBiases += 1;
553 biasSat->Biases[ii].Type = CODETYPEGLONASS_L1_P;
554 biasSat->Biases[ii].Bias = it.value();
555 }
556 else if (it.key() == "2C") {
557 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
558 biasSat->NumberOfCodeBiases += 1;
559 biasSat->Biases[ii].Type = CODETYPEGLONASS_L2_CA;
560 biasSat->Biases[ii].Bias = it.value();
561 }
562 else if (it.key() == "2P") {
563 int ii = biasSat->NumberOfCodeBiases; if (ii >= CLOCKORBIT_NUMBIAS) break;
564 biasSat->NumberOfCodeBiases += 1;
565 biasSat->Biases[ii].Type = CODETYPEGLONASS_L2_P;
566 biasSat->Biases[ii].Bias = it.value();
567 }
568 }
569 }
570 }
571 }
572 }
573
574 QByteArray hlpBufferCo;
575
576 // Orbit and Clock Corrections together
577 // ------------------------------------
578 if (_samplRtcmEphCorr == 0.0) {
579 if (co.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
580 char obuffer[CLOCKORBIT_BUFFERSIZE];
581 int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
582 if (len > 0) {
583 hlpBufferCo = QByteArray(obuffer, len);
584 }
585 }
586 }
587
588 // Orbit and Clock Corrections separately
589 // --------------------------------------
590 else {
591 if (co.NumberOfSat[CLOCKORBIT_SATGPS] > 0) {
592 char obuffer[CLOCKORBIT_BUFFERSIZE];
593 if (fmod(epoTime.gpssec(), _samplRtcmEphCorr) == 0.0) {
594 co.UpdateInterval = ephUpdInd;
595 int len1 = MakeClockOrbit(&co, COTYPE_GPSORBIT, 1, obuffer, sizeof(obuffer));
596 co.UpdateInterval = clkUpdInd;
597 if (len1 > 0) {
598 hlpBufferCo += QByteArray(obuffer, len1);
599 }
600 }
601 int mmsg = (co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) ? 1 : 0;
602 int len2 = MakeClockOrbit(&co, COTYPE_GPSCLOCK, mmsg, obuffer, sizeof(obuffer));
603 if (len2 > 0) {
604 hlpBufferCo += QByteArray(obuffer, len2);
605 }
606 }
607 if (co.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
608 char obuffer[CLOCKORBIT_BUFFERSIZE];
609 if (fmod(epoTime.gpssec(), _samplRtcmEphCorr) == 0.0) {
610 co.UpdateInterval = ephUpdInd;
611 int len1 = MakeClockOrbit(&co, COTYPE_GLONASSORBIT, 1, obuffer, sizeof(obuffer));
612 co.UpdateInterval = clkUpdInd;
613 if (len1 > 0) {
614 hlpBufferCo += QByteArray(obuffer, len1);
615 }
616 }
617 int len2 = MakeClockOrbit(&co, COTYPE_GLONASSCLOCK, 0, obuffer, sizeof(obuffer));
618 if (len2 > 0) {
619 hlpBufferCo += QByteArray(obuffer, len2);
620 }
621 }
622 }
623
624 // Biases
625 // ------
626 QByteArray hlpBufferBias;
627 if (bias.NumberOfSat[CLOCKORBIT_SATGPS] > 0 || bias.NumberOfSat[CLOCKORBIT_SATGLONASS] > 0) {
628 char obuffer[CLOCKORBIT_BUFFERSIZE];
629 int len = MakeCodeBias(&bias, BTYPE_AUTO, 0, obuffer, sizeof(obuffer));
630 if (len > 0) {
631 hlpBufferBias = QByteArray(obuffer, len);
632 }
633 }
634
635 _outBuffer += hlpBufferCo + hlpBufferBias;
636}
637
638//
639////////////////////////////////////////////////////////////////////////////
640void bncRtnetUploadCaster::processSatellite(t_eph* eph, int GPSweek,
641 double GPSweeks, const QString& prn,
642 const ColumnVector& rtnAPC,
643 double rtnClk,
644 const ColumnVector& rtnVel,
645 const ColumnVector& rtnCoM,
646 struct ClockOrbit::SatData* sd,
647 QString& outLine) {
648
649 // Broadcast Position and Velocity
650 // -------------------------------
651 ColumnVector xB(4);
652 ColumnVector vB(3);
653 eph->getCrd(bncTime(GPSweek, GPSweeks), xB, vB, false);
654
655 // Precise Position
656 // ----------------
657 ColumnVector xP = _CoM ? rtnCoM : rtnAPC;
658
659 double dc = 0.0;
660 if (_crdTrafo != "IGS08") {
661 crdTrafo(GPSweek, xP, dc);
662 }
663
664 // Difference in xyz
665 // -----------------
666 ColumnVector dx = xB.Rows(1,3) - xP;
667 ColumnVector dv = vB - rtnVel;
668
669 // Difference in RSW
670 // -----------------
671 ColumnVector rsw(3);
672 XYZ_to_RSW(xB.Rows(1,3), vB, dx, rsw);
673
674 ColumnVector dotRsw(3);
675 XYZ_to_RSW(xB.Rows(1,3), vB, dv, dotRsw);
676
677 // Clock Correction
678 // ----------------
679 double dClk = rtnClk - (xB(4) - dc) * t_CST::c;
680
681 if (sd) {
682 sd->ID = prn.mid(1).toInt();
683 sd->IOD = eph->IOD();
684 sd->Clock.DeltaA0 = dClk;
685 sd->Clock.DeltaA1 = 0.0; // TODO
686 sd->Clock.DeltaA2 = 0.0; // TODO
687 sd->Orbit.DeltaRadial = rsw(1);
688 sd->Orbit.DeltaAlongTrack = rsw(2);
689 sd->Orbit.DeltaCrossTrack = rsw(3);
690 sd->Orbit.DotDeltaRadial = dotRsw(1);
691 sd->Orbit.DotDeltaAlongTrack = dotRsw(2);
692 sd->Orbit.DotDeltaCrossTrack = dotRsw(3);
693 }
694
695 outLine.sprintf("%d %.1f %s %3d %10.3f %8.3f %8.3f %8.3f\n",
696 GPSweek, GPSweeks, eph->prn().toString().c_str(),
697 eph->IOD(), dClk, rsw(1), rsw(2), rsw(3));
698
699 double relativity = -2.0 * DotProduct(xP, rtnVel) / t_CST::c;
700 double sp3Clk = (rtnClk - relativity) / t_CST::c; // in seconds
701
702 if (_rnx) {
703 _rnx->write(GPSweek, GPSweeks, prn, sp3Clk);
704 }
705 if (_sp3) {
706 _sp3->write(GPSweek, GPSweeks, prn, rtnCoM, sp3Clk);
707 }
708}
709
710// Transform Coordinates
711////////////////////////////////////////////////////////////////////////////
712void bncRtnetUploadCaster::crdTrafo(int GPSWeek, ColumnVector& xyz,
713 double& dc) {
714
715 // Current epoch minus 2000.0 in years
716 // ------------------------------------
717 double dt = (GPSWeek - (1042.0+6.0/7.0)) / 365.2422 * 7.0 + 2000.0 - _t0;
718
719 ColumnVector dx(3);
720
721 dx(1) = _dx + dt * _dxr;
722 dx(2) = _dy + dt * _dyr;
723 dx(3) = _dz + dt * _dzr;
724
725 static const double arcSec = 180.0 * 3600.0 / M_PI;
726
727 double ox = (_ox + dt * _oxr) / arcSec;
728 double oy = (_oy + dt * _oyr) / arcSec;
729 double oz = (_oz + dt * _ozr) / arcSec;
730
731 double sc = 1.0 + _sc * 1e-9 + dt * _scr * 1e-9;
732
733 // Specify approximate center of area
734 // ----------------------------------
735 ColumnVector meanSta(3);
736
737 if (_crdTrafo == "ETRF2000") {
738 meanSta(1) = 3661090.0;
739 meanSta(2) = 845230.0;
740 meanSta(3) = 5136850.0;
741 }
742 else if (_crdTrafo == "NAD83") {
743 meanSta(1) = -1092950.0;
744 meanSta(2) = -4383600.0;
745 meanSta(3) = 4487420.0;
746 }
747 else if (_crdTrafo == "GDA94") {
748 meanSta(1) = -4052050.0;
749 meanSta(2) = 4212840.0;
750 meanSta(3) = -2545110.0;
751 }
752 else if (_crdTrafo == "SIRGAS2000") {
753 meanSta(1) = 3740860.0;
754 meanSta(2) = -4964290.0;
755 meanSta(3) = -1425420.0;
756 }
757 else if (_crdTrafo == "SIRGAS95") {
758 meanSta(1) = 3135390.0;
759 meanSta(2) = -5017670.0;
760 meanSta(3) = -2374440.0;
761 }
762 else if (_crdTrafo == "DREF91") {
763 meanSta(1) = 3959579.0;
764 meanSta(2) = 721719.0;
765 meanSta(3) = 4931539.0;
766 }
767 else if (_crdTrafo == "Custom") {
768 meanSta(1) = 0.0; // TODO
769 meanSta(2) = 0.0; // TODO
770 meanSta(3) = 0.0; // TODO
771 }
772
773 // Clock correction proportional to topocentric distance to satellites
774 // -------------------------------------------------------------------
775 double rho = (xyz - meanSta).norm_Frobenius();
776 dc = rho * (sc - 1.0) / sc / t_CST::c;
777
778 Matrix rMat(3,3);
779 rMat(1,1) = 1.0;
780 rMat(1,2) = -oz;
781 rMat(1,3) = oy;
782 rMat(2,1) = oz;
783 rMat(2,2) = 1.0;
784 rMat(2,3) = -ox;
785 rMat(3,1) = -oy;
786 rMat(3,2) = ox;
787 rMat(3,3) = 1.0;
788
789 xyz = sc * rMat * xyz + dx;
790}
791
Note: See TracBrowser for help on using the repository browser.