[2898] | 1 | /* -------------------------------------------------------------------------
|
---|
| 2 | * BKG NTRIP Client
|
---|
| 3 | * -------------------------------------------------------------------------
|
---|
| 4 | *
|
---|
| 5 | * Class: bncComb
|
---|
| 6 | *
|
---|
| 7 | * Purpose: Combinations of Orbit/Clock Corrections
|
---|
| 8 | *
|
---|
| 9 | * Author: L. Mervart
|
---|
| 10 | *
|
---|
| 11 | * Created: 22-Jan-2011
|
---|
| 12 | *
|
---|
| 13 | * Changes:
|
---|
| 14 | *
|
---|
| 15 | * -----------------------------------------------------------------------*/
|
---|
| 16 |
|
---|
[2921] | 17 | #include <iomanip>
|
---|
[2898] | 18 |
|
---|
| 19 | #include "bnccomb.h"
|
---|
| 20 | #include "bncapp.h"
|
---|
[2927] | 21 | #include "cmbcaster.h"
|
---|
[2910] | 22 | #include "bncsettings.h"
|
---|
[2898] | 23 |
|
---|
| 24 | using namespace std;
|
---|
| 25 |
|
---|
| 26 | // Constructor
|
---|
| 27 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 28 | bncComb::bncComb() {
|
---|
[2910] | 29 |
|
---|
| 30 | bncSettings settings;
|
---|
| 31 |
|
---|
| 32 | QStringList combineStreams = settings.value("combineStreams").toStringList();
|
---|
[2918] | 33 |
|
---|
| 34 | if (combineStreams.size() >= 2) {
|
---|
[2910] | 35 | QListIterator<QString> it(combineStreams);
|
---|
| 36 | while (it.hasNext()) {
|
---|
| 37 | QStringList hlp = it.next().split(" ");
|
---|
[2918] | 38 | cmbAC* newAC = new cmbAC();
|
---|
| 39 | newAC->mountPoint = hlp[0];
|
---|
| 40 | newAC->name = hlp[1];
|
---|
| 41 | newAC->weight = hlp[2].toDouble();
|
---|
| 42 |
|
---|
| 43 | _ACs[newAC->mountPoint] = newAC;
|
---|
[2910] | 44 | }
|
---|
| 45 | }
|
---|
[2927] | 46 |
|
---|
| 47 | _caster = new cmbCaster();
|
---|
[2898] | 48 | }
|
---|
| 49 |
|
---|
| 50 | // Destructor
|
---|
| 51 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 52 | bncComb::~bncComb() {
|
---|
[2918] | 53 | QMapIterator<QString, cmbAC*> it(_ACs);
|
---|
| 54 | while (it.hasNext()) {
|
---|
| 55 | it.next();
|
---|
| 56 | delete it.value();
|
---|
| 57 | }
|
---|
[2927] | 58 | delete _caster;
|
---|
[2898] | 59 | }
|
---|
| 60 |
|
---|
[2928] | 61 | // Read and store one correction line
|
---|
[2898] | 62 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 63 | void bncComb::processCorrLine(const QString& staID, const QString& line) {
|
---|
[2906] | 64 | QMutexLocker locker(&_mutex);
|
---|
[2913] | 65 |
|
---|
[2918] | 66 | // Find the relevant instance of cmbAC class
|
---|
| 67 | // -----------------------------------------
|
---|
| 68 | if (_ACs.find(staID) == _ACs.end()) {
|
---|
| 69 | return;
|
---|
| 70 | }
|
---|
| 71 | cmbAC* AC = _ACs[staID];
|
---|
| 72 |
|
---|
| 73 | // Read the Correction
|
---|
| 74 | // -------------------
|
---|
[2913] | 75 | t_corr* newCorr = new t_corr();
|
---|
| 76 | if (!newCorr->readLine(line) == success) {
|
---|
| 77 | delete newCorr;
|
---|
| 78 | return;
|
---|
| 79 | }
|
---|
| 80 |
|
---|
[2924] | 81 | // Reject delayed corrections
|
---|
| 82 | // --------------------------
|
---|
| 83 | if (_processedBeforeTime.valid() && newCorr->tt < _processedBeforeTime) {
|
---|
| 84 | delete newCorr;
|
---|
| 85 | return;
|
---|
| 86 | }
|
---|
| 87 |
|
---|
| 88 | // Process all older Epochs (if there are any)
|
---|
| 89 | // -------------------------------------------
|
---|
| 90 | const double waitTime = 5.0; // wait 5 sec
|
---|
| 91 | _processedBeforeTime = newCorr->tt - waitTime;
|
---|
| 92 |
|
---|
[2927] | 93 | QList<cmbEpoch*> epochsToProcess;
|
---|
| 94 |
|
---|
| 95 | QMapIterator<QString, cmbAC*> itAC(_ACs);
|
---|
| 96 | while (itAC.hasNext()) {
|
---|
| 97 | itAC.next();
|
---|
| 98 | cmbAC* AC = itAC.value();
|
---|
| 99 |
|
---|
| 100 | QMutableListIterator<cmbEpoch*> itEpo(AC->epochs);
|
---|
| 101 | while (itEpo.hasNext()) {
|
---|
| 102 | cmbEpoch* epoch = itEpo.next();
|
---|
| 103 | if (epoch->time < _processedBeforeTime) {
|
---|
| 104 | epochsToProcess.append(epoch);
|
---|
| 105 | itEpo.remove();
|
---|
| 106 | }
|
---|
| 107 | }
|
---|
| 108 | }
|
---|
| 109 |
|
---|
| 110 | if (epochsToProcess.size()) {
|
---|
| 111 | processEpochs(epochsToProcess);
|
---|
| 112 | }
|
---|
| 113 |
|
---|
[2920] | 114 | // Check Modulo Time
|
---|
| 115 | // -----------------
|
---|
| 116 | const int moduloTime = 10;
|
---|
| 117 | if (int(newCorr->tt.gpssec()) % moduloTime != 0.0) {
|
---|
| 118 | delete newCorr;
|
---|
| 119 | return;
|
---|
| 120 | }
|
---|
| 121 |
|
---|
[2918] | 122 | // Find/Create the instance of cmbEpoch class
|
---|
| 123 | // ------------------------------------------
|
---|
| 124 | cmbEpoch* newEpoch = 0;
|
---|
| 125 | QListIterator<cmbEpoch*> it(AC->epochs);
|
---|
| 126 | while (it.hasNext()) {
|
---|
| 127 | cmbEpoch* hlpEpoch = it.next();
|
---|
| 128 | if (hlpEpoch->time == newCorr->tt) {
|
---|
| 129 | newEpoch = hlpEpoch;
|
---|
| 130 | break;
|
---|
| 131 | }
|
---|
| 132 | }
|
---|
| 133 | if (newEpoch == 0) {
|
---|
[2927] | 134 | newEpoch = new cmbEpoch(AC->name);
|
---|
[2918] | 135 | newEpoch->time = newCorr->tt;
|
---|
| 136 | AC->epochs.append(newEpoch);
|
---|
| 137 | }
|
---|
[2924] | 138 |
|
---|
| 139 | // Merge or add the correction
|
---|
| 140 | // ---------------------------
|
---|
[2918] | 141 | if (newEpoch->corr.find(newCorr->prn) != newEpoch->corr.end()) {
|
---|
[2919] | 142 | newEpoch->corr[newCorr->prn]->readLine(line); // merge (multiple messages)
|
---|
[2918] | 143 | }
|
---|
[2919] | 144 | else {
|
---|
| 145 | newEpoch->corr[newCorr->prn] = newCorr;
|
---|
| 146 | }
|
---|
[2898] | 147 | }
|
---|
| 148 |
|
---|
[2928] | 149 | // Print one correction
|
---|
[2918] | 150 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2928] | 151 | void bncComb::printSingleCorr(const QString& acName, const t_corr* corr) {
|
---|
| 152 | cout.setf(ios::fixed);
|
---|
| 153 | cout << acName.toAscii().data() << " "
|
---|
| 154 | << corr->prn.toAscii().data() << " "
|
---|
| 155 | << corr->tt.timestr() << " "
|
---|
| 156 | << setw(4) << corr->iod << " "
|
---|
| 157 | << setw(8) << setprecision(4) << corr->dClk * t_CST::c << " "
|
---|
| 158 | << setw(8) << setprecision(4) << corr->rao[0] << " "
|
---|
| 159 | << setw(8) << setprecision(4) << corr->rao[1] << " "
|
---|
| 160 | << setw(8) << setprecision(4) << corr->rao[2] << endl;
|
---|
| 161 | }
|
---|
| 162 |
|
---|
| 163 | // Send results to caster
|
---|
| 164 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 165 | void bncComb::dumpResults(const bncTime& resTime,
|
---|
| 166 | const QMap<QString, t_corr*>& resCorr) {
|
---|
| 167 |
|
---|
| 168 | _caster->open();
|
---|
| 169 |
|
---|
| 170 | unsigned year, month, day;
|
---|
| 171 | resTime.civil_date (year, month, day);
|
---|
| 172 | double GPSweeks = resTime.gpssec();
|
---|
| 173 |
|
---|
| 174 | struct ClockOrbit co;
|
---|
| 175 | memset(&co, 0, sizeof(co));
|
---|
| 176 | co.GPSEpochTime = (int)GPSweeks;
|
---|
| 177 | co.GLONASSEpochTime = (int)fmod(GPSweeks, 86400.0)
|
---|
| 178 | + 3 * 3600 - gnumleap(year, month, day);
|
---|
| 179 | co.ClockDataSupplied = 1;
|
---|
| 180 | co.OrbitDataSupplied = 1;
|
---|
| 181 | co.SatRefDatum = DATUM_ITRF;
|
---|
| 182 |
|
---|
| 183 | struct ClockOrbit::SatData* sd = 0;
|
---|
| 184 |
|
---|
| 185 | QMapIterator<QString, t_corr*> it(resCorr);
|
---|
| 186 | while (it.hasNext()) {
|
---|
| 187 | it.next();
|
---|
| 188 | t_corr* corr = it.value();
|
---|
| 189 |
|
---|
| 190 | if (corr->prn[0] == 'G') {
|
---|
| 191 | sd = co.Sat + co.NumberOfGPSSat;
|
---|
| 192 | ++co.NumberOfGPSSat;
|
---|
| 193 | }
|
---|
| 194 | else if (corr->prn[0] == 'R') {
|
---|
| 195 | sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
|
---|
| 196 | ++co.NumberOfGLONASSSat;
|
---|
| 197 | }
|
---|
| 198 |
|
---|
| 199 | if (sd != 0) {
|
---|
| 200 | sd->ID = corr->prn.mid(1).toInt();
|
---|
| 201 | sd->IOD = corr->iod;
|
---|
| 202 | sd->Clock.DeltaA0 = corr->dClk;
|
---|
| 203 | sd->Orbit.DeltaRadial = corr->rao(1);
|
---|
| 204 | sd->Orbit.DeltaAlongTrack = corr->rao(2);
|
---|
| 205 | sd->Orbit.DeltaCrossTrack = corr->rao(3);
|
---|
| 206 | sd->Orbit.DotDeltaRadial = corr->dotRao(1);
|
---|
| 207 | sd->Orbit.DotDeltaAlongTrack = corr->dotRao(2);
|
---|
| 208 | sd->Orbit.DotDeltaCrossTrack = corr->dotRao(3);
|
---|
| 209 | }
|
---|
| 210 |
|
---|
| 211 | delete corr;
|
---|
| 212 | }
|
---|
| 213 |
|
---|
| 214 | if ( _caster->usedSocket() &&
|
---|
| 215 | (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
|
---|
| 216 | char obuffer[CLOCKORBIT_BUFFERSIZE];
|
---|
| 217 | int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
|
---|
| 218 | if (len > 0) {
|
---|
| 219 | _caster->write(obuffer, len);
|
---|
| 220 | }
|
---|
| 221 | }
|
---|
| 222 | }
|
---|
| 223 |
|
---|
| 224 | // Process Epochs
|
---|
| 225 | ////////////////////////////////////////////////////////////////////////////
|
---|
[2927] | 226 | void bncComb::processEpochs(const QList<cmbEpoch*>& epochs) {
|
---|
[2918] | 227 |
|
---|
[2928] | 228 | bncTime resTime = epochs.first()->time;
|
---|
| 229 | QMap<QString, t_corr*> resCorr;
|
---|
| 230 |
|
---|
[2927] | 231 | QListIterator<cmbEpoch*> itEpo(epochs);
|
---|
| 232 | while (itEpo.hasNext()) {
|
---|
| 233 | cmbEpoch* epo = itEpo.next();
|
---|
| 234 | QMapIterator<QString, t_corr*> itCorr(epo->corr);
|
---|
[2928] | 235 |
|
---|
[2927] | 236 | while (itCorr.hasNext()) {
|
---|
| 237 | itCorr.next();
|
---|
| 238 | t_corr* corr = itCorr.value();
|
---|
[2928] | 239 |
|
---|
| 240 | //// beg test
|
---|
| 241 | if (epo->acName == "BKG") {
|
---|
| 242 | resCorr[corr->prn] = new t_corr(*corr);
|
---|
| 243 | }
|
---|
| 244 | //// end test
|
---|
| 245 |
|
---|
[2927] | 246 | printSingleCorr(epo->acName, corr);
|
---|
[2928] | 247 | delete corr;
|
---|
[2918] | 248 | }
|
---|
| 249 | }
|
---|
[2919] | 250 |
|
---|
[2928] | 251 | dumpResults(resTime, resCorr);
|
---|
| 252 |
|
---|
[2927] | 253 | cout << "Corrections processed" << endl << endl;
|
---|
[2918] | 254 | }
|
---|
[2919] | 255 |
|
---|