Changeset 11215 in ntrip for trunk/BNC/src/bncephuser.cpp


Ignore:
Timestamp:
Nov 19, 2026, 11:04:10 PM (less than one hour ago)
Author:
stuerze
Message:

Upload Ephemeris: majority vote included

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/BNC/src/bncephuser.cpp

    r11185 r11215  
    4444#include "bnccore.h"
    4545#include "bncsettings.h"
     46#include "RTCM3/ephEncoder.h"
    4647
    4748using namespace std;
    … …  
    4950// Constructor
    5051////////////////////////////////////////////////////////////////////////////
    51 bncEphUser::bncEphUser(bool connectSlots, unsigned maxQueueSize) : _maxQueueSize(maxQueueSize) {
     52bncEphUser::bncEphUser(bool connectSlots, unsigned maxQueueSize) : _maxQueueSize(maxQueueSize), _voting(false) {
    5253  if (connectSlots) {
    5354    connect(BNC_CORE, SIGNAL(newGPSEph(t_ephGPS)),
    … …  
    9091    }
    9192  }
     93  qDeleteAll(_votes);
    9294}
    9395
    … …  
    142144  }
    143145
    144   const t_ephGPS*     ephGPS     = dynamic_cast<const t_ephGPS*>(eph);
    145   const t_ephGlo*     ephGlo     = dynamic_cast<const t_ephGlo*>(eph);
    146   const t_ephGal*     ephGal     = dynamic_cast<const t_ephGal*>(eph);
    147   const t_ephSBAS*    ephSBAS    = dynamic_cast<const t_ephSBAS*>(eph);
    148   const t_ephBDS*     ephBDS     = dynamic_cast<const t_ephBDS*>(eph);
    149 
    150   t_eph* newEph = 0;
    151 
    152   if      (ephGPS) {
    153     newEph = new t_ephGPS(*ephGPS);
    154   }
    155   else if (ephGlo) {
    156     newEph = new t_ephGlo(*ephGlo);
    157   }
    158   else if (ephGal) {
    159     newEph = new t_ephGal(*ephGal);
    160   }
    161   else if (ephSBAS) {
    162     newEph = new t_ephSBAS(*ephSBAS);
    163   }
    164   else if (ephBDS) {
    165     newEph = new t_ephBDS(*ephBDS);
    166   }
    167   else {
     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) {
    168153    return failure;
    169154  }
    … …  
    177162      if (qq[ii]->sameDataSet(eph)) {
    178163        delete newEph;
     164        if (_voting && realTime) {
     165          vote(prn, ii, eph);
     166        }
    179167        return failure;
    180168      }
    … …  
    203191    newEph->setCheckState(eph->checkState());
    204192    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    }
    205200    if (qq.size() > _maxQueueSize) {
     201      delete _votes.take(qq.front());
    206202      delete qq.front();
    207203      qq.pop_front();
    … …  
    214210    return failure;
    215211  }
     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();
    216399}
    217400
Note: See TracChangeset for help on using the changeset viewer.