source: ntrip/trunk/BNC/src/bncephuser.cpp@ 11215

Last change on this file since 11215 was 11215, checked in by stuerze, 43 minutes ago

Upload Ephemeris: majority vote included

File size: 18.5 KB
Line 
1// converting GNSS data streams from NTRIP broadcasters.
2//
3// Copyright (C) 2007
4// German Federal Agency for Cartography and Geodesy (BKG)
5// http://bkg.bund.de
6// Czech Technical University Prague, Department of Geodesy
7// http://www.fsv.cvut.cz
8//
9// Email: euref-ip@bkg.bund.de
10//
11// This program is free software; you can redistribute it and/or
12// modify it under the terms of the GNU General Public License
13// as published by the Free Software Foundation, version 2.
14//
15// This program is distributed in the hope that it will be useful,
16// but WITHOUT ANY WARRANTY; without even the implied warranty of
17// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18// GNU General Public License for more details.
19//
20// You should have received a copy of the GNU General Public License
21// along with this program; if not, write to the Free Software
22// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
23
24/* -------------------------------------------------------------------------
25 * BKG NTRIP Client
26 * -------------------------------------------------------------------------
27 *
28 * Class: bncEphUser
29 *
30 * Purpose: Base for Classes that use Ephemerides
31 *
32 * Author: L. Mervart
33 *
34 * Created: 27-Jan-2011
35 *
36 * Changes:
37 *
38 * -----------------------------------------------------------------------*/
39
40#include <cmath>
41#include <iostream>
42
43#include "bncephuser.h"
44#include "bnccore.h"
45#include "bncsettings.h"
46#include "RTCM3/ephEncoder.h"
47
48using namespace std;
49
50// Constructor
51////////////////////////////////////////////////////////////////////////////
52bncEphUser::bncEphUser(bool connectSlots, unsigned maxQueueSize) : _maxQueueSize(maxQueueSize), _voting(false) {
53 if (connectSlots) {
54 connect(BNC_CORE, SIGNAL(newGPSEph(t_ephGPS)),
55 this, SLOT(slotNewGPSEph(t_ephGPS)), Qt::DirectConnection);
56
57 connect(BNC_CORE, SIGNAL(newGlonassEph(t_ephGlo)),
58 this, SLOT(slotNewGlonassEph(t_ephGlo)), Qt::DirectConnection);
59
60 connect(BNC_CORE, SIGNAL(newGalileoEph(t_ephGal)),
61 this, SLOT(slotNewGalileoEph(t_ephGal)), Qt::DirectConnection);
62
63 connect(BNC_CORE, SIGNAL(newSBASEph(t_ephSBAS)),
64 this, SLOT(slotNewSBASEph(t_ephSBAS)), Qt::DirectConnection);
65
66 connect(BNC_CORE, SIGNAL(newBDSEph(t_ephBDS)),
67 this, SLOT(slotNewBDSEph(t_ephBDS)), Qt::DirectConnection);
68 }
69}
70
71// Ephemerides of the streams in the general option 'Ephemeris streams' only
72////////////////////////////////////////////////////////////////////////////
73void bncEphUser::useEphMounts(const QString& user) {
74 bncSettings settings;
75 _ephMounts = settings.value("ephMount").toString().split(QRegularExpression("[,\\s]+"),
76 Qt::SkipEmptyParts);
77 if (!_ephMounts.isEmpty()) {
78 BNC_CORE->message((user.trimmed() + ": ephemerides of " + _ephMounts.join(", ") + " only").toLatin1(), true, "bce", t_logRecord::info);
79 }
80}
81
82// Destructor
83////////////////////////////////////////////////////////////////////////////
84bncEphUser::~bncEphUser() {
85 QMapIterator<QString, deque<t_eph*> > it(_eph);
86 while (it.hasNext()) {
87 it.next();
88 const deque<t_eph*>& qq = it.value();
89 for (unsigned ii = 0; ii < qq.size(); ii++) {
90 delete qq[ii];
91 }
92 }
93 qDeleteAll(_votes);
94}
95
96// New GPS Ephemeris
97////////////////////////////////////////////////////////////////////////////
98void bncEphUser::slotNewGPSEph(t_ephGPS eph) {
99 if (fromEphMount(eph)) {
100 putNewEph(&eph, true);
101 }
102}
103
104// New Glonass Ephemeris
105////////////////////////////////////////////////////////////////////////////
106void bncEphUser::slotNewGlonassEph(t_ephGlo eph) {
107 if (fromEphMount(eph)) {
108 putNewEph(&eph, true);
109 }
110}
111
112// New Galileo Ephemeris
113////////////////////////////////////////////////////////////////////////////
114void bncEphUser::slotNewGalileoEph(t_ephGal eph) {
115 if (fromEphMount(eph)) {
116 putNewEph(&eph, true);
117 }
118}
119
120// New SBAS Ephemeris
121////////////////////////////////////////////////////////////////////////////
122void bncEphUser::slotNewSBASEph(t_ephSBAS eph) {
123 if (fromEphMount(eph)) {
124 putNewEph(&eph, true);
125 }
126}
127
128// New BDS Ephemeris
129////////////////////////////////////////////////////////////////////////////
130void bncEphUser::slotNewBDSEph(t_ephBDS eph) {
131 if (fromEphMount(eph)) {
132 putNewEph(&eph, true);
133 }
134}
135
136//
137////////////////////////////////////////////////////////////////////////////
138t_irc bncEphUser::putNewEph(t_eph* eph, bool realTime) {
139
140 QMutexLocker locker(&_mutex);
141
142 if (eph == 0) {
143 return failure;
144 }
145
146 // satellite outside the range supported by BNC (e.g. G39)
147 if (eph->prn().toInt() == 0) {
148 return failure;
149 }
150
151 t_eph* newEph = clone(eph);
152 if (newEph == 0) {
153 return failure;
154 }
155
156 QString prn(newEph->prn().toInternalString().c_str());
157
158 // data set received before, e.g. from a slower stream after a newer one
159 if (_eph.contains(prn)) {
160 const deque<t_eph*>& qq = _eph[prn];
161 for (unsigned ii = 0; ii < qq.size(); ii++) {
162 if (qq[ii]->sameDataSet(eph)) {
163 delete newEph;
164 if (_voting && realTime) {
165 vote(prn, ii, eph);
166 }
167 return failure;
168 }
169 }
170 }
171
172 const t_eph* ephOld = ephLast(prn);
173
174 if (ephOld &&
175 (ephOld->checkState() == t_eph::bad ||
176 ephOld->checkState() == t_eph::outdated )) {
177 ephOld = 0;
178 }
179
180 if ( (ephOld == 0) || eph->isNewerThan(ephOld)) {
181 checkEphemeris(eph, realTime);
182 }
183 else {
184 delete newEph;
185 return failure;
186 }
187
188 if (eph->checkState() != t_eph::bad &&
189 eph->checkState() != t_eph::outdated) {
190 deque<t_eph*>& qq = _eph[prn];
191 newEph->setCheckState(eph->checkState());
192 qq.push_back(newEph);
193 if (_voting && realTime) {
194 t_votes* votes = new t_votes;
195 votes->_content.append(votingContent(newEph));
196 votes->_eph.append(clone(newEph));
197 votes->_station[newEph->receptStaID()] = 0;
198 _votes[newEph] = votes;
199 }
200 if (qq.size() > _maxQueueSize) {
201 delete _votes.take(qq.front());
202 delete qq.front();
203 qq.pop_front();
204 }
205 ephBufferChanged();
206 return success;
207 }
208 else {
209 delete newEph;
210 return failure;
211 }
212}
213
214// Copy of an ephemeris of any system
215////////////////////////////////////////////////////////////////////////////
216t_eph* bncEphUser::clone(const t_eph* eph) {
217 if (const t_ephGPS* e = dynamic_cast<const t_ephGPS*>(eph)) return new t_ephGPS(*e);
218 if (const t_ephGlo* e = dynamic_cast<const t_ephGlo*>(eph)) return new t_ephGlo(*e);
219 if (const t_ephGal* e = dynamic_cast<const t_ephGal*>(eph)) return new t_ephGal(*e);
220 if (const t_ephSBAS* e = dynamic_cast<const t_ephSBAS*>(eph)) return new t_ephSBAS(*e);
221 if (const t_ephBDS* e = dynamic_cast<const t_ephBDS*>(eph)) return new t_ephBDS(*e);
222 return 0;
223}
224
225// Content compared in the vote: the uploaded RTCM3 message without CRC and,
226// for GLONASS, without the fields differing legitimately between receivers
227////////////////////////////////////////////////////////////////////////////
228QByteArray bncEphUser::votingContent(const t_eph* eph) {
229 unsigned char buffer[80];
230 int size = 0;
231 if (const t_ephGPS* e = dynamic_cast<const t_ephGPS*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
232 else if (const t_ephGlo* e = dynamic_cast<const t_ephGlo*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
233 else if (const t_ephGal* e = dynamic_cast<const t_ephGal*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
234 else if (const t_ephSBAS* e = dynamic_cast<const t_ephSBAS*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
235 else if (const t_ephBDS* e = dynamic_cast<const t_ephBDS*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
236 if (size <= 6) {
237 return QByteArray();
238 }
239 if (eph->system() == t_eph::GLONASS) {
240 // message bits (after the 3 header bytes) of tk and of the system time
241 // offsets tau_c and tau_GPS: receivers take the latter from the string 5
242 // of any satellite, they differ within the same data set
243 const int field[3][2] = {{27, 12}, {293, 32}, {330, 22}};
244 for (int ii = 0; ii < 3; ii++) {
245 for (int bit = 24 + field[ii][0]; bit < 24 + field[ii][0] + field[ii][1]; bit++) {
246 buffer[bit / 8] &= ~(0x80 >> (bit % 8));
247 }
248 }
249 }
250 return QByteArray((const char*) buffer, size - 3);
251}
252
253// Names of the fields in which two vote contents (RTCM3 messages) differ
254////////////////////////////////////////////////////////////////////////////
255namespace {
256struct t_field {const char* name; int nbits;};
257const t_field fields1019[] = {{"type",12},{"sv",6},{"week",10},{"URA",4},{"L2code",2},{"IDOT",14},{"IODE",8},
258 {"toc",16},{"af2",8},{"af1",16},{"af0",22},{"IODC",10},{"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},
259 {"Cus",16},{"sqrtA",32},{"toe",16},{"Cic",16},{"Omega0",32},{"Cis",16},{"i0",32},{"Crc",16},{"omega",32},
260 {"OmegaDot",24},{"TGD",8},{"health",6},{"L2P",1},{"fit",1},{0,0}};
261const t_field fields1020[] = {{"type",12},{"sv",6},{"fc",5},{"Cn",1},{"A",1},{"P1",2},{"tk",12},{"Bn",1},{"P2",1},
262 {"tb",7},{"xv",24},{"x",27},{"xa",5},{"yv",24},{"y",27},{"ya",5},{"zv",24},{"z",27},{"za",5},{"P3",1},
263 {"gamma",11},{"P",2},{"ln3",1},{"tau",22},{"dtau",5},{"En",5},{"P4",1},{"FT",4},{"NT",11},{"M",2},{"add",1},
264 {"NA",11},{"tauc",32},{"N4",5},{"tauGPS",22},{"ln5",1},{0,0}};
265const t_field fields1042[] = {{"type",12},{"sv",6},{"week",13},{"URAI",4},{"IDOT",14},{"AODE",5},{"toc",17},
266 {"a2",11},{"a1",22},{"a0",24},{"AODC",5},{"Crs",18},{"dn",16},{"M0",32},{"Cuc",18},{"e",32},{"Cus",18},
267 {"sqrtA",32},{"toe",17},{"Cic",18},{"Omega0",32},{"Cis",18},{"i0",32},{"Crc",18},{"omega",32},{"OmegaDot",24},
268 {"TGD1",10},{"TGD2",10},{"SatH1",1},{0,0}};
269const t_field fields1044[] = {{"type",12},{"sv",4},{"toc",16},{"af2",8},{"af1",16},{"af0",22},{"IODE",8},
270 {"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},{"Cus",16},{"sqrtA",32},{"toe",16},{"Cic",16},{"Omega0",32},
271 {"Cis",16},{"i0",32},{"Crc",16},{"omega",32},{"OmegaDot",24},{"IDOT",14},{"L2code",2},{"week",10},{"URA",4},
272 {"health",6},{"TGD",8},{"IODC",10},{"fit",1},{0,0}};
273#define GAL_COMMON {"type",12},{"sv",6},{"week",12},{"IODnav",10},{"SISA",8},{"IDOT",14},{"toc",14},{"af2",6}, \
274 {"af1",21},{"af0",31},{"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},{"Cus",16},{"sqrtA",32},{"toe",14}, \
275 {"Cic",16},{"Omega0",32},{"Cis",16},{"i0",32},{"Crc",16},{"omega",32},{"OmegaDot",24},{"BGDa",10}
276const t_field fields1045[] = {GAL_COMMON,{"E5aHS",2},{"E5aDVS",1},{0,0}};
277const t_field fields1046[] = {GAL_COMMON,{"BGDb",10},{"E5bHS",2},{"E5bDVS",1},{"E1BHS",2},{"E1BDVS",1},{0,0}};
278
279unsigned long long getBits(const QByteArray& buf, int pos, int nbits) {
280 unsigned long long val = 0;
281 for (int bit = pos; bit < pos + nbits; bit++) {
282 val = (val << 1) | ((static_cast<unsigned char>(buf[bit / 8]) >> (7 - bit % 8)) & 1);
283 }
284 return val;
285}
286}
287
288QString bncEphUser::differingFields(const QByteArray& c1, const QByteArray& c2) {
289 if (c1.size() != c2.size() || c1.size() < 5) {
290 return "content";
291 }
292 const t_field* fields = 0;
293 switch (getBits(c1, 24, 12)) {
294 case 1019: fields = fields1019; break;
295 case 1020: fields = fields1020; break;
296 case 1042: fields = fields1042; break;
297 case 1044: fields = fields1044; break;
298 case 1045: fields = fields1045; break;
299 case 1046: fields = fields1046; break;
300 }
301 if (!fields) {
302 return "content";
303 }
304 QStringList names;
305 int pos = 24;
306 for (int ii = 0; fields[ii].name; ii++) {
307 if (pos + fields[ii].nbits > 8 * c1.size()) {
308 break;
309 }
310 if (getBits(c1, pos, fields[ii].nbits) != getBits(c2, pos, fields[ii].nbits)) {
311 names << fields[ii].name;
312 }
313 pos += fields[ii].nbits;
314 }
315 return names.isEmpty() ? QString("content") : names.join(" ");
316}
317
318// Further copy of the buffered data set qq[index] received from a stream
319////////////////////////////////////////////////////////////////////////////
320void bncEphUser::vote(const QString& prn, unsigned index, const t_eph* eph) {
321 deque<t_eph*>& qq = _eph[prn];
322 t_votes* votes = _votes.value(qq[index], 0);
323 if (!votes) {
324 return;
325 }
326 QByteArray content = votingContent(eph);
327 int iVar = votes->_content.indexOf(content);
328 if (iVar < 0) {
329 iVar = votes->_content.size();
330 votes->_content.append(content);
331 votes->_eph.append(clone(eph));
332 }
333 votes->_station[eph->receptStaID()] = iVar;
334
335 QVector<int> count(votes->_content.size(), 0);
336 QMapIterator<QString, int> it(votes->_station);
337 while (it.hasNext()) {
338 it.next();
339 count[it.value()]++;
340 }
341 int iBest = votes->_current;
342 for (int ii = 0; ii < count.size(); ii++) {
343 if (count[ii] > count[iBest]) {
344 iBest = ii;
345 }
346 }
347 if (iBest == votes->_current) {
348 return;
349 }
350
351 t_eph* oldEph = qq[index];
352 t_eph* newEph = clone(votes->_eph[iBest]);
353 if (!radialDistanceOk(newEph)) {
354 delete newEph;
355 return;
356 }
357 if (newEph->isUnhealthy()) {
358 newEph->setCheckState(t_eph::unhealthy);
359 }
360 else if (oldEph->checkState() == t_eph::unhealthy) {
361 newEph->setCheckState(t_eph::ok);
362 }
363 else {
364 newEph->setCheckState(oldEph->checkState());
365 }
366
367 QStringList stations;
368 for (int ii = 0; ii < count.size(); ii++) {
369 QStringList hlp;
370 QMapIterator<QString, int> is(votes->_station);
371 while (is.hasNext()) {
372 is.next();
373 if (is.value() == ii) {
374 hlp << is.key();
375 }
376 }
377 if (!hlp.isEmpty()) {
378 stations << (ii == iBest ? "+ " : "- ") + hlp.join(" ");
379 }
380 }
381 BNC_CORE->message(QString("ephemeris vote %1 %2 %3: copy of %4 replaced by that of %5 (%6 of %7 streams; %8; differing: %9)")
382 .arg(prn.left(3))
383 .arg(oldEph->TOC().datestr().c_str())
384 .arg(oldEph->TOC().timestr(0).c_str())
385 .arg(oldEph->receptStaID())
386 .arg(newEph->receptStaID())
387 .arg(count[iBest])
388 .arg(votes->_station.size())
389 .arg(stations.join(", "))
390 .arg(differingFields(votes->_content[votes->_current], votes->_content[iBest])).toLatin1(),
391 false, "bce", t_logRecord::info);
392
393 _votes.remove(oldEph);
394 delete oldEph;
395 qq[index] = newEph;
396 votes->_current = iBest;
397 _votes[newEph] = votes;
398 ephBufferChanged();
399}
400
401//
402////////////////////////////////////////////////////////////////////////////
403bool bncEphUser::radialDistanceOk(const t_eph* eph) {
404 ColumnVector xc(6);
405 ColumnVector vv(3);
406 if (eph->getCrd(eph->TOC(), xc, vv, false) != success) {
407 return false;
408 }
409 double rr = xc.Rows(1,3).NormFrobenius();
410
411 const double MINDIST = 2.e7;
412 const double MAXDIST = 6.e7;
413 if (rr < MINDIST || rr > MAXDIST || std::isnan(rr)) {
414 return false;
415 }
416 return true;
417}
418
419//
420////////////////////////////////////////////////////////////////////////////
421void bncEphUser::checkEphemeris(t_eph* eph, bool realTime) {
422
423 if (!eph) {
424 return;
425 }
426
427 // Check whether the epoch is too far away the current time
428 // --------------------------------------------------------
429 if (realTime) {
430 QDateTime now = currentDateAndTimeGPS();
431 bncTime currentTime(now.toString(Qt::ISODate).toStdString());
432 if (outDatedBcep(eph, currentTime)) {
433 eph->setCheckState(t_eph::outdated);
434 return;
435 }
436 }
437
438 // Check health status
439 // -------------------
440 if (eph->isUnhealthy()) {
441 eph->setCheckState(t_eph::unhealthy);
442 return;
443 }
444
445 // GPS/QZSS: IODE and IODC of the same upload
446 // -------------------------------------------
447 const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
448 if (ephGPS && !ephGPS->iodeIodcConsistent()) {
449 eph->setCheckState(t_eph::bad);
450 return;
451 }
452
453 // Simple Check - check satellite radial distance
454 // ----------------------------------------------
455 if (!radialDistanceOk(eph)) {
456 eph->setCheckState(t_eph::bad);
457 return;
458 }
459 ColumnVector xc(6);
460 ColumnVector vv(3);
461 eph->getCrd(eph->TOC(), xc, vv, false);
462
463 // Check consistency with older ephemeris
464 // --------------------------------------
465 const double MAXDIFF_INIT = 10.0;
466 const double MINDIFF = 0.005;
467 QString prn = QString(eph->prn().toInternalString().c_str());
468 t_eph* ephL = ephLast(prn);
469
470 // maxdiff adapted to individual systems
471 char sys = eph->prn().system();
472 double MAXDIFF = MAXDIFF_INIT;
473 if (sys == 'C' ||
474 sys == 'S' ) {
475 MAXDIFF *= 10.0;
476 }
477
478 if (ephL && ephL->checkState() != t_eph::bad) {
479 // Update of ephemeris data sets after an outage: if the time since the
480 // last data set exceeds the system's update interval, the last one is
481 // outdated and a consistency check against it (extrapolated over the
482 // whole gap) is meaningless - it would reject the first new data set
483 // after every gap. The new data set stays unchecked and serves as
484 // reference for the next one.
485 // -----------------------------------------------------------------
486 double dt = eph->TOC() - ephL->TOC();
487
488 // update interval: 2h
489 if (eph->system() == t_eph::GPS && dt > 7200.0) {
490 ephL->setCheckState(t_eph::outdated);
491 return;
492 }
493 // update interval: 3h
494 else if (eph->system() == t_eph::Galileo && dt > 10800.0) {
495 ephL->setCheckState(t_eph::outdated);
496 return;
497 }
498 // updated every 30 minutes + 5 min
499 else if (eph->system() == t_eph::GLONASS && dt > 2100.0) {
500 ephL->setCheckState(t_eph::outdated);
501 return;
502 }
503 // updated every ?
504 else if (eph->system() == t_eph::QZSS && dt > 3600.0) {
505 ephL->setCheckState(t_eph::outdated);
506 return;
507 }
508 // maximum update interval: 300 sec
509 else if (eph->system() == t_eph::SBAS && dt > 600.0) {
510 ephL->setCheckState(t_eph::outdated);
511 return;
512 }
513 // updates 1h + 5 min
514 else if (eph->system() == t_eph::BDS && dt > 3900.0) {
515 ephL->setCheckState(t_eph::outdated);
516 return;
517 }
518 // update interval: up to 24 hours
519 else if (eph->system() == t_eph::NavIC && dt > 86400.0) {
520 ephL->setCheckState(t_eph::outdated);
521 return;
522 }
523
524 ColumnVector xcL(6);
525 ColumnVector vvL(3);
526 if (ephL->getCrd(eph->TOC(), xcL, vvL, false) != success) {
527 ephL->setCheckState(t_eph::bad);
528 return;
529 }
530
531 double diff = (xc.Rows(1,3) - xcL.Rows(1,3)).NormFrobenius();
532 double diffC = fabs(xc(4) - xcL(4)) * t_CST::c;
533
534 if (diff < MAXDIFF &&
535 diffC < MAXDIFF) {
536 if (diff < MINDIFF &&
537 diffC < MINDIFF &&
538 ephL->checkState() == t_eph::ok) {
539 // to prevent the same data sets with different TOC values
540 eph->setCheckState(t_eph::bad);
541 }
542 else {
543 eph->setCheckState(t_eph::ok);
544 ephL->setCheckState(t_eph::ok);
545 }
546 }
547 else {
548 if (ephL->checkState() == t_eph::ok) {
549 eph->setCheckState(t_eph::bad);
550 }
551 }
552 }
553}
554
Note: See TracBrowser for help on using the repository browser.