| 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 |
|
|---|
| 48 | using namespace std;
|
|---|
| 49 |
|
|---|
| 50 | // Constructor
|
|---|
| 51 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 52 | bncEphUser::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 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 73 | void 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 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 84 | bncEphUser::~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 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 98 | void bncEphUser::slotNewGPSEph(t_ephGPS eph) {
|
|---|
| 99 | if (fromEphMount(eph)) {
|
|---|
| 100 | putNewEph(&eph, true);
|
|---|
| 101 | }
|
|---|
| 102 | }
|
|---|
| 103 |
|
|---|
| 104 | // New Glonass Ephemeris
|
|---|
| 105 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 106 | void bncEphUser::slotNewGlonassEph(t_ephGlo eph) {
|
|---|
| 107 | if (fromEphMount(eph)) {
|
|---|
| 108 | putNewEph(&eph, true);
|
|---|
| 109 | }
|
|---|
| 110 | }
|
|---|
| 111 |
|
|---|
| 112 | // New Galileo Ephemeris
|
|---|
| 113 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 114 | void bncEphUser::slotNewGalileoEph(t_ephGal eph) {
|
|---|
| 115 | if (fromEphMount(eph)) {
|
|---|
| 116 | putNewEph(&eph, true);
|
|---|
| 117 | }
|
|---|
| 118 | }
|
|---|
| 119 |
|
|---|
| 120 | // New SBAS Ephemeris
|
|---|
| 121 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 122 | void bncEphUser::slotNewSBASEph(t_ephSBAS eph) {
|
|---|
| 123 | if (fromEphMount(eph)) {
|
|---|
| 124 | putNewEph(&eph, true);
|
|---|
| 125 | }
|
|---|
| 126 | }
|
|---|
| 127 |
|
|---|
| 128 | // New BDS Ephemeris
|
|---|
| 129 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 130 | void bncEphUser::slotNewBDSEph(t_ephBDS eph) {
|
|---|
| 131 | if (fromEphMount(eph)) {
|
|---|
| 132 | putNewEph(&eph, true);
|
|---|
| 133 | }
|
|---|
| 134 | }
|
|---|
| 135 |
|
|---|
| 136 | //
|
|---|
| 137 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 138 | t_irc bncEphUser::putNewEph(t_eph* eph, bool realTime) {
|
|---|
| 139 |
|
|---|
| 140 | QMutexLocker locker(&_mutex);
|
|---|
| 141 |
|
|---|
| 142 | if (eph == 0) {
|
|---|
| 143 | return failure;
|
|---|
| 144 | }
|
|---|
| 145 |
|
|---|
| 146 | if (_voting && realTime) {
|
|---|
| 147 | releaseHeld();
|
|---|
| 148 | }
|
|---|
| 149 |
|
|---|
| 150 | // satellite outside the range supported by BNC (e.g. G39)
|
|---|
| 151 | if (eph->prn().toInt() == 0) {
|
|---|
| 152 | return failure;
|
|---|
| 153 | }
|
|---|
| 154 |
|
|---|
| 155 | t_eph* newEph = clone(eph);
|
|---|
| 156 | if (newEph == 0) {
|
|---|
| 157 | return failure;
|
|---|
| 158 | }
|
|---|
| 159 |
|
|---|
| 160 | QString prn(newEph->prn().toInternalString().c_str());
|
|---|
| 161 |
|
|---|
| 162 | // data set received before, e.g. from a slower stream after a newer one
|
|---|
| 163 | if (_eph.contains(prn)) {
|
|---|
| 164 | const deque<t_eph*>& qq = _eph[prn];
|
|---|
| 165 | for (unsigned ii = 0; ii < qq.size(); ii++) {
|
|---|
| 166 | if (qq[ii]->sameDataSet(eph)) {
|
|---|
| 167 | delete newEph;
|
|---|
| 168 | if (_voting && realTime) {
|
|---|
| 169 | vote(prn, ii, eph);
|
|---|
| 170 | }
|
|---|
| 171 | return failure;
|
|---|
| 172 | }
|
|---|
| 173 | }
|
|---|
| 174 | }
|
|---|
| 175 |
|
|---|
| 176 | const t_eph* ephOld = ephLast(prn);
|
|---|
| 177 |
|
|---|
| 178 | if (ephOld &&
|
|---|
| 179 | (ephOld->checkState() == t_eph::bad ||
|
|---|
| 180 | ephOld->checkState() == t_eph::outdated )) {
|
|---|
| 181 | ephOld = 0;
|
|---|
| 182 | }
|
|---|
| 183 |
|
|---|
| 184 | if ( (ephOld == 0) || eph->isNewerThan(ephOld)) {
|
|---|
| 185 | checkEphemeris(eph, realTime);
|
|---|
| 186 | }
|
|---|
| 187 | else {
|
|---|
| 188 | delete newEph;
|
|---|
| 189 | return failure;
|
|---|
| 190 | }
|
|---|
| 191 |
|
|---|
| 192 | if (eph->checkState() != t_eph::bad &&
|
|---|
| 193 | eph->checkState() != t_eph::outdated) {
|
|---|
| 194 | deque<t_eph*>& qq = _eph[prn];
|
|---|
| 195 | newEph->setCheckState(eph->checkState());
|
|---|
| 196 | qq.push_back(newEph);
|
|---|
| 197 | if (_voting && realTime) {
|
|---|
| 198 | t_votes* votes = new t_votes;
|
|---|
| 199 | votes->_content.append(votingContent(newEph));
|
|---|
| 200 | votes->_eph.append(clone(newEph));
|
|---|
| 201 | votes->_station[newEph->receptStaID()] = 0;
|
|---|
| 202 | votes->_first = currentTime();
|
|---|
| 203 | if (qq.size() > 1) {
|
|---|
| 204 | const t_votes* prev = _votes.value(qq[qq.size()-2], 0);
|
|---|
| 205 | votes->_released = !(prev && prev->_station.size() >= VOTE_AGREE);
|
|---|
| 206 | }
|
|---|
| 207 | _votes[newEph] = votes;
|
|---|
| 208 | }
|
|---|
| 209 | if (qq.size() > _maxQueueSize) {
|
|---|
| 210 | delete _votes.take(qq.front());
|
|---|
| 211 | delete qq.front();
|
|---|
| 212 | qq.pop_front();
|
|---|
| 213 | }
|
|---|
| 214 | ephBufferChanged();
|
|---|
| 215 | return success;
|
|---|
| 216 | }
|
|---|
| 217 | else {
|
|---|
| 218 | delete newEph;
|
|---|
| 219 | return failure;
|
|---|
| 220 | }
|
|---|
| 221 | }
|
|---|
| 222 |
|
|---|
| 223 | // Copy of an ephemeris of any system
|
|---|
| 224 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 225 | t_eph* bncEphUser::clone(const t_eph* eph) {
|
|---|
| 226 | if (const t_ephGPS* e = dynamic_cast<const t_ephGPS*>(eph)) return new t_ephGPS(*e);
|
|---|
| 227 | if (const t_ephGlo* e = dynamic_cast<const t_ephGlo*>(eph)) return new t_ephGlo(*e);
|
|---|
| 228 | if (const t_ephGal* e = dynamic_cast<const t_ephGal*>(eph)) return new t_ephGal(*e);
|
|---|
| 229 | if (const t_ephSBAS* e = dynamic_cast<const t_ephSBAS*>(eph)) return new t_ephSBAS(*e);
|
|---|
| 230 | if (const t_ephBDS* e = dynamic_cast<const t_ephBDS*>(eph)) return new t_ephBDS(*e);
|
|---|
| 231 | return 0;
|
|---|
| 232 | }
|
|---|
| 233 |
|
|---|
| 234 | // Content compared in the vote: the uploaded RTCM3 message without CRC and,
|
|---|
| 235 | // for GLONASS, without the fields differing legitimately between receivers
|
|---|
| 236 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 237 | QByteArray bncEphUser::votingContent(const t_eph* eph) {
|
|---|
| 238 | unsigned char buffer[80];
|
|---|
| 239 | int size = 0;
|
|---|
| 240 | if (const t_ephGPS* e = dynamic_cast<const t_ephGPS*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
|
|---|
| 241 | else if (const t_ephGlo* e = dynamic_cast<const t_ephGlo*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
|
|---|
| 242 | else if (const t_ephGal* e = dynamic_cast<const t_ephGal*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
|
|---|
| 243 | else if (const t_ephSBAS* e = dynamic_cast<const t_ephSBAS*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
|
|---|
| 244 | else if (const t_ephBDS* e = dynamic_cast<const t_ephBDS*>(eph)) size = t_ephEncoder::RTCM3(*e, buffer);
|
|---|
| 245 | if (size <= 6) {
|
|---|
| 246 | return QByteArray();
|
|---|
| 247 | }
|
|---|
| 248 | if (eph->system() == t_eph::GLONASS) {
|
|---|
| 249 | // message bits (after the 3 header bytes) of tk and of the system time
|
|---|
| 250 | // offsets tau_c and tau_GPS: receivers take the latter from the string 5
|
|---|
| 251 | // of any satellite, they differ within the same data set
|
|---|
| 252 | const int field[3][2] = {{27, 12}, {293, 32}, {330, 22}};
|
|---|
| 253 | for (int ii = 0; ii < 3; ii++) {
|
|---|
| 254 | for (int bit = 24 + field[ii][0]; bit < 24 + field[ii][0] + field[ii][1]; bit++) {
|
|---|
| 255 | buffer[bit / 8] &= ~(0x80 >> (bit % 8));
|
|---|
| 256 | }
|
|---|
| 257 | }
|
|---|
| 258 | }
|
|---|
| 259 | return QByteArray((const char*) buffer, size - 3);
|
|---|
| 260 | }
|
|---|
| 261 |
|
|---|
| 262 | // Names of the fields in which two vote contents (RTCM3 messages) differ
|
|---|
| 263 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 264 | namespace {
|
|---|
| 265 | struct t_field {const char* name; int nbits;};
|
|---|
| 266 | const t_field fields1019[] = {{"type",12},{"sv",6},{"week",10},{"URA",4},{"L2code",2},{"IDOT",14},{"IODE",8},
|
|---|
| 267 | {"toc",16},{"af2",8},{"af1",16},{"af0",22},{"IODC",10},{"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},
|
|---|
| 268 | {"Cus",16},{"sqrtA",32},{"toe",16},{"Cic",16},{"Omega0",32},{"Cis",16},{"i0",32},{"Crc",16},{"omega",32},
|
|---|
| 269 | {"OmegaDot",24},{"TGD",8},{"health",6},{"L2P",1},{"fit",1},{0,0}};
|
|---|
| 270 | const t_field fields1020[] = {{"type",12},{"sv",6},{"fc",5},{"Cn",1},{"A",1},{"P1",2},{"tk",12},{"Bn",1},{"P2",1},
|
|---|
| 271 | {"tb",7},{"xv",24},{"x",27},{"xa",5},{"yv",24},{"y",27},{"ya",5},{"zv",24},{"z",27},{"za",5},{"P3",1},
|
|---|
| 272 | {"gamma",11},{"P",2},{"ln3",1},{"tau",22},{"dtau",5},{"En",5},{"P4",1},{"FT",4},{"NT",11},{"M",2},{"add",1},
|
|---|
| 273 | {"NA",11},{"tauc",32},{"N4",5},{"tauGPS",22},{"ln5",1},{0,0}};
|
|---|
| 274 | const t_field fields1042[] = {{"type",12},{"sv",6},{"week",13},{"URAI",4},{"IDOT",14},{"AODE",5},{"toc",17},
|
|---|
| 275 | {"a2",11},{"a1",22},{"a0",24},{"AODC",5},{"Crs",18},{"dn",16},{"M0",32},{"Cuc",18},{"e",32},{"Cus",18},
|
|---|
| 276 | {"sqrtA",32},{"toe",17},{"Cic",18},{"Omega0",32},{"Cis",18},{"i0",32},{"Crc",18},{"omega",32},{"OmegaDot",24},
|
|---|
| 277 | {"TGD1",10},{"TGD2",10},{"SatH1",1},{0,0}};
|
|---|
| 278 | const t_field fields1044[] = {{"type",12},{"sv",4},{"toc",16},{"af2",8},{"af1",16},{"af0",22},{"IODE",8},
|
|---|
| 279 | {"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},{"Cus",16},{"sqrtA",32},{"toe",16},{"Cic",16},{"Omega0",32},
|
|---|
| 280 | {"Cis",16},{"i0",32},{"Crc",16},{"omega",32},{"OmegaDot",24},{"IDOT",14},{"L2code",2},{"week",10},{"URA",4},
|
|---|
| 281 | {"health",6},{"TGD",8},{"IODC",10},{"fit",1},{0,0}};
|
|---|
| 282 | #define GAL_COMMON {"type",12},{"sv",6},{"week",12},{"IODnav",10},{"SISA",8},{"IDOT",14},{"toc",14},{"af2",6}, \
|
|---|
| 283 | {"af1",21},{"af0",31},{"Crs",16},{"dn",16},{"M0",32},{"Cuc",16},{"e",32},{"Cus",16},{"sqrtA",32},{"toe",14}, \
|
|---|
| 284 | {"Cic",16},{"Omega0",32},{"Cis",16},{"i0",32},{"Crc",16},{"omega",32},{"OmegaDot",24},{"BGDa",10}
|
|---|
| 285 | const t_field fields1045[] = {GAL_COMMON,{"E5aHS",2},{"E5aDVS",1},{0,0}};
|
|---|
| 286 | const t_field fields1046[] = {GAL_COMMON,{"BGDb",10},{"E5bHS",2},{"E5bDVS",1},{"E1BHS",2},{"E1BDVS",1},{0,0}};
|
|---|
| 287 |
|
|---|
| 288 | unsigned long long getBits(const QByteArray& buf, int pos, int nbits) {
|
|---|
| 289 | unsigned long long val = 0;
|
|---|
| 290 | for (int bit = pos; bit < pos + nbits; bit++) {
|
|---|
| 291 | val = (val << 1) | ((static_cast<unsigned char>(buf[bit / 8]) >> (7 - bit % 8)) & 1);
|
|---|
| 292 | }
|
|---|
| 293 | return val;
|
|---|
| 294 | }
|
|---|
| 295 | }
|
|---|
| 296 |
|
|---|
| 297 | QString bncEphUser::differingFields(const QByteArray& c1, const QByteArray& c2) {
|
|---|
| 298 | if (c1.size() != c2.size() || c1.size() < 5) {
|
|---|
| 299 | return "content";
|
|---|
| 300 | }
|
|---|
| 301 | const t_field* fields = 0;
|
|---|
| 302 | switch (getBits(c1, 24, 12)) {
|
|---|
| 303 | case 1019: fields = fields1019; break;
|
|---|
| 304 | case 1020: fields = fields1020; break;
|
|---|
| 305 | case 1042: fields = fields1042; break;
|
|---|
| 306 | case 1044: fields = fields1044; break;
|
|---|
| 307 | case 1045: fields = fields1045; break;
|
|---|
| 308 | case 1046: fields = fields1046; break;
|
|---|
| 309 | }
|
|---|
| 310 | if (!fields) {
|
|---|
| 311 | return "content";
|
|---|
| 312 | }
|
|---|
| 313 | QStringList names;
|
|---|
| 314 | int pos = 24;
|
|---|
| 315 | for (int ii = 0; fields[ii].name; ii++) {
|
|---|
| 316 | if (pos + fields[ii].nbits > 8 * c1.size()) {
|
|---|
| 317 | break;
|
|---|
| 318 | }
|
|---|
| 319 | if (getBits(c1, pos, fields[ii].nbits) != getBits(c2, pos, fields[ii].nbits)) {
|
|---|
| 320 | names << fields[ii].name;
|
|---|
| 321 | }
|
|---|
| 322 | pos += fields[ii].nbits;
|
|---|
| 323 | }
|
|---|
| 324 | return names.isEmpty() ? QString("content") : names.join(" ");
|
|---|
| 325 | }
|
|---|
| 326 |
|
|---|
| 327 | // Satellite in the vote messages, for Galileo with the navigation message
|
|---|
| 328 | // (I/NAV and F/NAV data sets are buffered and voted separately)
|
|---|
| 329 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 330 | static QString voteSat(const t_eph* eph) {
|
|---|
| 331 | QString sat(QString(eph->prn().toInternalString().c_str()).left(3));
|
|---|
| 332 | if (eph->type() == t_eph::INAV) sat += " INAV";
|
|---|
| 333 | else if (eph->type() == t_eph::FNAV) sat += " FNAV";
|
|---|
| 334 | return sat;
|
|---|
| 335 | }
|
|---|
| 336 |
|
|---|
| 337 | const double bncEphUser::VOTE_HOLD = 40.0; // SSR providers switch to a new IOD after 60 s
|
|---|
| 338 | const int bncEphUser::VOTE_AGREE = 3;
|
|---|
| 339 |
|
|---|
| 340 | // Current GPS time (time of the input data when replaying a file)
|
|---|
| 341 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 342 | bncTime bncEphUser::currentTime() {
|
|---|
| 343 | return bncTime(currentDateAndTimeGPS().toString(Qt::ISODate).toStdString());
|
|---|
| 344 | }
|
|---|
| 345 |
|
|---|
| 346 | // Release the data sets held back longer than VOTE_HOLD seconds
|
|---|
| 347 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 348 | void bncEphUser::releaseHeld() {
|
|---|
| 349 | bncTime now = currentTime();
|
|---|
| 350 | bool changed = false;
|
|---|
| 351 | QStringList msgs; // sorted: the QHash order differs between runs
|
|---|
| 352 | QHashIterator<const t_eph*, t_votes*> it(_votes);
|
|---|
| 353 | while (it.hasNext()) {
|
|---|
| 354 | it.next();
|
|---|
| 355 | t_votes* votes = it.value();
|
|---|
| 356 | if (!votes->_released && now - votes->_first >= VOTE_HOLD) {
|
|---|
| 357 | votes->_released = true;
|
|---|
| 358 | changed = true;
|
|---|
| 359 | const t_eph* eph = it.key();
|
|---|
| 360 | QVector<int> count(votes->_content.size(), 0);
|
|---|
| 361 | QMapIterator<QString, int> is(votes->_station);
|
|---|
| 362 | while (is.hasNext()) {
|
|---|
| 363 | is.next();
|
|---|
| 364 | count[is.value()]++;
|
|---|
| 365 | }
|
|---|
| 366 | msgs << QString("ephemeris vote %1 %2 %3: copy of %4 uploaded after %5 s without %6 agreeing streams (%7 of %8 streams)")
|
|---|
| 367 | .arg(voteSat(eph))
|
|---|
| 368 | .arg(eph->TOC().datestr().c_str())
|
|---|
| 369 | .arg(eph->TOC().timestr(0).c_str())
|
|---|
| 370 | .arg(eph->receptStaID())
|
|---|
| 371 | .arg(int(VOTE_HOLD))
|
|---|
| 372 | .arg(VOTE_AGREE)
|
|---|
| 373 | .arg(count[votes->_current])
|
|---|
| 374 | .arg(votes->_station.size());
|
|---|
| 375 | }
|
|---|
| 376 | }
|
|---|
| 377 | msgs.sort();
|
|---|
| 378 | for (const QString& msg : msgs) {
|
|---|
| 379 | BNC_CORE->message(msg.toLatin1(), false, "bce", t_logRecord::info);
|
|---|
| 380 | }
|
|---|
| 381 | if (changed) {
|
|---|
| 382 | ephBufferChanged();
|
|---|
| 383 | }
|
|---|
| 384 | }
|
|---|
| 385 |
|
|---|
| 386 | // Vote over the copies of a data set, check of the hold every second
|
|---|
| 387 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 388 | void bncEphUser::enableVoting() {
|
|---|
| 389 | _voting = true;
|
|---|
| 390 | QTimer* timer = new QTimer(this);
|
|---|
| 391 | connect(timer, SIGNAL(timeout()), this, SLOT(slotVoteTimer()));
|
|---|
| 392 | timer->start(1000);
|
|---|
| 393 | }
|
|---|
| 394 |
|
|---|
| 395 | // Release of held data sets without new ephemerides; not when replaying a
|
|---|
| 396 | // file, where the time of the input data decides (reproducible results)
|
|---|
| 397 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 398 | void bncEphUser::slotVoteTimer() {
|
|---|
| 399 | if (BNC_CORE->dateAndTimeGPSSet()) {
|
|---|
| 400 | return;
|
|---|
| 401 | }
|
|---|
| 402 | QMutexLocker locker(&_mutex);
|
|---|
| 403 | releaseHeld();
|
|---|
| 404 | }
|
|---|
| 405 |
|
|---|
| 406 | // Data set to be uploaded
|
|---|
| 407 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 408 | t_eph* bncEphUser::ephUpload(const QString& prn) {
|
|---|
| 409 | if (!_eph.contains(prn)) {
|
|---|
| 410 | return 0;
|
|---|
| 411 | }
|
|---|
| 412 | const deque<t_eph*>& qq = _eph[prn];
|
|---|
| 413 | const t_votes* votes = _votes.value(qq.back(), 0);
|
|---|
| 414 | if (votes && !votes->_released && qq.size() > 1) {
|
|---|
| 415 | return qq[qq.size()-2];
|
|---|
| 416 | }
|
|---|
| 417 | return qq.back();
|
|---|
| 418 | }
|
|---|
| 419 |
|
|---|
| 420 | // Further copy of the buffered data set qq[index] received from a stream
|
|---|
| 421 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 422 | void bncEphUser::vote(const QString& prn, unsigned index, const t_eph* eph) {
|
|---|
| 423 | deque<t_eph*>& qq = _eph[prn];
|
|---|
| 424 | t_votes* votes = _votes.value(qq[index], 0);
|
|---|
| 425 | if (!votes) {
|
|---|
| 426 | return;
|
|---|
| 427 | }
|
|---|
| 428 | QByteArray content = votingContent(eph);
|
|---|
| 429 | int iVar = votes->_content.indexOf(content);
|
|---|
| 430 | if (iVar < 0) {
|
|---|
| 431 | iVar = votes->_content.size();
|
|---|
| 432 | votes->_content.append(content);
|
|---|
| 433 | votes->_eph.append(clone(eph));
|
|---|
| 434 | }
|
|---|
| 435 | votes->_station[eph->receptStaID()] = iVar;
|
|---|
| 436 |
|
|---|
| 437 | QVector<int> count(votes->_content.size(), 0);
|
|---|
| 438 | QMapIterator<QString, int> it(votes->_station);
|
|---|
| 439 | while (it.hasNext()) {
|
|---|
| 440 | it.next();
|
|---|
| 441 | count[it.value()]++;
|
|---|
| 442 | }
|
|---|
| 443 | int iBest = votes->_current;
|
|---|
| 444 | for (int ii = 0; ii < count.size(); ii++) {
|
|---|
| 445 | if (count[ii] > count[iBest]) {
|
|---|
| 446 | iBest = ii;
|
|---|
| 447 | }
|
|---|
| 448 | }
|
|---|
| 449 | bool released = false;
|
|---|
| 450 | if (!votes->_released && count[iBest] >= VOTE_AGREE) {
|
|---|
| 451 | votes->_released = true;
|
|---|
| 452 | released = true;
|
|---|
| 453 | }
|
|---|
| 454 | if (iBest == votes->_current) {
|
|---|
| 455 | if (released) {
|
|---|
| 456 | ephBufferChanged();
|
|---|
| 457 | }
|
|---|
| 458 | return;
|
|---|
| 459 | }
|
|---|
| 460 |
|
|---|
| 461 | t_eph* oldEph = qq[index];
|
|---|
| 462 | t_eph* newEph = clone(votes->_eph[iBest]);
|
|---|
| 463 | if (!radialDistanceOk(newEph)) {
|
|---|
| 464 | delete newEph;
|
|---|
| 465 | if (released) {
|
|---|
| 466 | ephBufferChanged();
|
|---|
| 467 | }
|
|---|
| 468 | return;
|
|---|
| 469 | }
|
|---|
| 470 | if (newEph->isUnhealthy()) {
|
|---|
| 471 | newEph->setCheckState(t_eph::unhealthy);
|
|---|
| 472 | }
|
|---|
| 473 | else if (oldEph->checkState() == t_eph::unhealthy) {
|
|---|
| 474 | newEph->setCheckState(t_eph::ok);
|
|---|
| 475 | }
|
|---|
| 476 | else {
|
|---|
| 477 | newEph->setCheckState(oldEph->checkState());
|
|---|
| 478 | }
|
|---|
| 479 |
|
|---|
| 480 | QStringList stations;
|
|---|
| 481 | for (int ii = 0; ii < count.size(); ii++) {
|
|---|
| 482 | QStringList hlp;
|
|---|
| 483 | QMapIterator<QString, int> is(votes->_station);
|
|---|
| 484 | while (is.hasNext()) {
|
|---|
| 485 | is.next();
|
|---|
| 486 | if (is.value() == ii) {
|
|---|
| 487 | hlp << is.key();
|
|---|
| 488 | }
|
|---|
| 489 | }
|
|---|
| 490 | if (!hlp.isEmpty()) {
|
|---|
| 491 | stations << (ii == iBest ? "+ " : "- ") + hlp.join(" ");
|
|---|
| 492 | }
|
|---|
| 493 | }
|
|---|
| 494 | BNC_CORE->message(QString("ephemeris vote %1 %2 %3: copy of %4 replaced by that of %5 (%6 of %7 streams; %8; differing: %9)")
|
|---|
| 495 | .arg(voteSat(oldEph))
|
|---|
| 496 | .arg(oldEph->TOC().datestr().c_str())
|
|---|
| 497 | .arg(oldEph->TOC().timestr(0).c_str())
|
|---|
| 498 | .arg(oldEph->receptStaID())
|
|---|
| 499 | .arg(newEph->receptStaID())
|
|---|
| 500 | .arg(count[iBest])
|
|---|
| 501 | .arg(votes->_station.size())
|
|---|
| 502 | .arg(stations.join(", "))
|
|---|
| 503 | .arg(differingFields(votes->_content[votes->_current], votes->_content[iBest])).toLatin1(),
|
|---|
| 504 | false, "bce", t_logRecord::info);
|
|---|
| 505 |
|
|---|
| 506 | _votes.remove(oldEph);
|
|---|
| 507 | delete oldEph;
|
|---|
| 508 | qq[index] = newEph;
|
|---|
| 509 | votes->_current = iBest;
|
|---|
| 510 | _votes[newEph] = votes;
|
|---|
| 511 | ephBufferChanged();
|
|---|
| 512 | }
|
|---|
| 513 |
|
|---|
| 514 | //
|
|---|
| 515 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 516 | bool bncEphUser::radialDistanceOk(const t_eph* eph) {
|
|---|
| 517 | ColumnVector xc(6);
|
|---|
| 518 | ColumnVector vv(3);
|
|---|
| 519 | if (eph->getCrd(eph->TOC(), xc, vv, false) != success) {
|
|---|
| 520 | return false;
|
|---|
| 521 | }
|
|---|
| 522 | double rr = xc.Rows(1,3).NormFrobenius();
|
|---|
| 523 |
|
|---|
| 524 | const double MINDIST = 2.e7;
|
|---|
| 525 | const double MAXDIST = 6.e7;
|
|---|
| 526 | if (rr < MINDIST || rr > MAXDIST || std::isnan(rr)) {
|
|---|
| 527 | return false;
|
|---|
| 528 | }
|
|---|
| 529 | return true;
|
|---|
| 530 | }
|
|---|
| 531 |
|
|---|
| 532 | //
|
|---|
| 533 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 534 | void bncEphUser::checkEphemeris(t_eph* eph, bool realTime) {
|
|---|
| 535 |
|
|---|
| 536 | if (!eph) {
|
|---|
| 537 | return;
|
|---|
| 538 | }
|
|---|
| 539 |
|
|---|
| 540 | // Check whether the epoch is too far away the current time
|
|---|
| 541 | // --------------------------------------------------------
|
|---|
| 542 | if (realTime) {
|
|---|
| 543 | QDateTime now = currentDateAndTimeGPS();
|
|---|
| 544 | bncTime currentTime(now.toString(Qt::ISODate).toStdString());
|
|---|
| 545 | if (outDatedBcep(eph, currentTime)) {
|
|---|
| 546 | eph->setCheckState(t_eph::outdated);
|
|---|
| 547 | return;
|
|---|
| 548 | }
|
|---|
| 549 | }
|
|---|
| 550 |
|
|---|
| 551 | // Check health status
|
|---|
| 552 | // -------------------
|
|---|
| 553 | if (eph->isUnhealthy()) {
|
|---|
| 554 | eph->setCheckState(t_eph::unhealthy);
|
|---|
| 555 | return;
|
|---|
| 556 | }
|
|---|
| 557 |
|
|---|
| 558 | // GPS/QZSS: IODE and IODC of the same upload
|
|---|
| 559 | // -------------------------------------------
|
|---|
| 560 | const t_ephGPS* ephGPS = dynamic_cast<const t_ephGPS*>(eph);
|
|---|
| 561 | if (ephGPS && !ephGPS->iodeIodcConsistent()) {
|
|---|
| 562 | eph->setCheckState(t_eph::bad);
|
|---|
| 563 | return;
|
|---|
| 564 | }
|
|---|
| 565 |
|
|---|
| 566 | // Simple Check - check satellite radial distance
|
|---|
| 567 | // ----------------------------------------------
|
|---|
| 568 | if (!radialDistanceOk(eph)) {
|
|---|
| 569 | eph->setCheckState(t_eph::bad);
|
|---|
| 570 | return;
|
|---|
| 571 | }
|
|---|
| 572 | ColumnVector xc(6);
|
|---|
| 573 | ColumnVector vv(3);
|
|---|
| 574 | eph->getCrd(eph->TOC(), xc, vv, false);
|
|---|
| 575 |
|
|---|
| 576 | // Check consistency with older ephemeris
|
|---|
| 577 | // --------------------------------------
|
|---|
| 578 | const double MAXDIFF_INIT = 10.0;
|
|---|
| 579 | const double MINDIFF = 0.005;
|
|---|
| 580 | QString prn = QString(eph->prn().toInternalString().c_str());
|
|---|
| 581 | t_eph* ephL = ephLast(prn);
|
|---|
| 582 |
|
|---|
| 583 | // maxdiff adapted to individual systems
|
|---|
| 584 | char sys = eph->prn().system();
|
|---|
| 585 | double MAXDIFF = MAXDIFF_INIT;
|
|---|
| 586 | if (sys == 'C' ||
|
|---|
| 587 | sys == 'S' ) {
|
|---|
| 588 | MAXDIFF *= 10.0;
|
|---|
| 589 | }
|
|---|
| 590 |
|
|---|
| 591 | if (ephL && ephL->checkState() != t_eph::bad) {
|
|---|
| 592 | // Update of ephemeris data sets after an outage: if the time since the
|
|---|
| 593 | // last data set exceeds the system's update interval, the last one is
|
|---|
| 594 | // outdated and a consistency check against it (extrapolated over the
|
|---|
| 595 | // whole gap) is meaningless - it would reject the first new data set
|
|---|
| 596 | // after every gap. The new data set stays unchecked and serves as
|
|---|
| 597 | // reference for the next one.
|
|---|
| 598 | // -----------------------------------------------------------------
|
|---|
| 599 | double dt = eph->TOC() - ephL->TOC();
|
|---|
| 600 |
|
|---|
| 601 | // update interval: 2h
|
|---|
| 602 | if (eph->system() == t_eph::GPS && dt > 7200.0) {
|
|---|
| 603 | ephL->setCheckState(t_eph::outdated);
|
|---|
| 604 | return;
|
|---|
| 605 | }
|
|---|
| 606 | // update interval: 3h
|
|---|
| 607 | else if (eph->system() == t_eph::Galileo && dt > 10800.0) {
|
|---|
| 608 | ephL->setCheckState(t_eph::outdated);
|
|---|
| 609 | return;
|
|---|
| 610 | }
|
|---|
| 611 | // updated every 30 minutes + 5 min
|
|---|
| 612 | else if (eph->system() == t_eph::GLONASS && dt > 2100.0) {
|
|---|
| 613 | ephL->setCheckState(t_eph::outdated);
|
|---|
| 614 | return;
|
|---|
| 615 | }
|
|---|
| 616 | // updated every ?
|
|---|
| 617 | else if (eph->system() == t_eph::QZSS && dt > 3600.0) {
|
|---|
| 618 | ephL->setCheckState(t_eph::outdated);
|
|---|
| 619 | return;
|
|---|
| 620 | }
|
|---|
| 621 | // maximum update interval: 300 sec
|
|---|
| 622 | else if (eph->system() == t_eph::SBAS && dt > 600.0) {
|
|---|
| 623 | ephL->setCheckState(t_eph::outdated);
|
|---|
| 624 | return;
|
|---|
| 625 | }
|
|---|
| 626 | // updates 1h + 5 min
|
|---|
| 627 | else if (eph->system() == t_eph::BDS && dt > 3900.0) {
|
|---|
| 628 | ephL->setCheckState(t_eph::outdated);
|
|---|
| 629 | return;
|
|---|
| 630 | }
|
|---|
| 631 | // update interval: up to 24 hours
|
|---|
| 632 | else if (eph->system() == t_eph::NavIC && dt > 86400.0) {
|
|---|
| 633 | ephL->setCheckState(t_eph::outdated);
|
|---|
| 634 | return;
|
|---|
| 635 | }
|
|---|
| 636 |
|
|---|
| 637 | ColumnVector xcL(6);
|
|---|
| 638 | ColumnVector vvL(3);
|
|---|
| 639 | if (ephL->getCrd(eph->TOC(), xcL, vvL, false) != success) {
|
|---|
| 640 | ephL->setCheckState(t_eph::bad);
|
|---|
| 641 | return;
|
|---|
| 642 | }
|
|---|
| 643 |
|
|---|
| 644 | double diff = (xc.Rows(1,3) - xcL.Rows(1,3)).NormFrobenius();
|
|---|
| 645 | double diffC = fabs(xc(4) - xcL(4)) * t_CST::c;
|
|---|
| 646 |
|
|---|
| 647 | if (diff < MAXDIFF &&
|
|---|
| 648 | diffC < MAXDIFF) {
|
|---|
| 649 | if (diff < MINDIFF &&
|
|---|
| 650 | diffC < MINDIFF &&
|
|---|
| 651 | ephL->checkState() == t_eph::ok) {
|
|---|
| 652 | // to prevent the same data sets with different TOC values
|
|---|
| 653 | eph->setCheckState(t_eph::bad);
|
|---|
| 654 | }
|
|---|
| 655 | else {
|
|---|
| 656 | eph->setCheckState(t_eph::ok);
|
|---|
| 657 | ephL->setCheckState(t_eph::ok);
|
|---|
| 658 | }
|
|---|
| 659 | }
|
|---|
| 660 | else {
|
|---|
| 661 | if (ephL->checkState() == t_eph::ok) {
|
|---|
| 662 | eph->setCheckState(t_eph::bad);
|
|---|
| 663 | }
|
|---|
| 664 | }
|
|---|
| 665 | }
|
|---|
| 666 | }
|
|---|
| 667 |
|
|---|