Index: unk/BNC/src/PPP/bncrtrover.cpp =================================================================== --- /trunk/BNC/src/PPP/bncrtrover.cpp (revision 5676) +++ (revision ) @@ -1,507 +1,0 @@ - -#include -#include -#include - -#include "bncrtrover.h" -#include "bnccore.h" -#include "bncsettings.h" -#include "bnctime.h" - -#include "rtrover_interface.h" - -using namespace std; - -// Constructor -//////////////////////////////////////////////////////////////////////////// -t_bncRtrover::t_bncRtrover() : QThread(0) { -} - -// Destructor -//////////////////////////////////////////////////////////////////////////// -t_bncRtrover::~t_bncRtrover() { - rtrover_destroy(); -} - -// Run (virtual) -//////////////////////////////////////////////////////////////////////////// -void t_bncRtrover::run() { - bncSettings settings; - - // User Options - // ------------ - _mode = settings.value("rtroverMode").toByteArray(); - _roverMount = settings.value("rtroverRoverMount").toByteArray(); - _baseMount = settings.value("rtroverBaseMount").toByteArray(); - _corrMount = settings.value("rtroverCorrMount").toByteArray(); - _outputFile.setFileName(settings.value("rtroverOutput").toString()); - _outputFile.open(QIODevice::WriteOnly | QIODevice::Text); - - // Define Input Options - // -------------------- - rtrover_opt opt; - rtrover_initOptions(&opt); - - if (_mode == "PPP_DF") { - opt._mode = mode_PPP_DF; - } - else if (_mode == "SPP_DF") { - opt._mode = mode_SPP_DF; - } - else if (_mode == "PPP_SF") { - opt._mode = mode_PPP_SF; - } - else if (_mode == "SPP_SF") { - opt._mode = mode_SPP_SF; - } - else if (_mode == "PPP_AR") { - opt._mode = mode_PPP_AR; - } - else if (_mode == "RTK") { - opt._mode = mode_RTK; - } - else if (_mode == "PPP_FTTF") { - opt._mode = mode_PPP_FTTF; - } - - QByteArray antNameRover = settings.value("rtroverRoverAntenna").toByteArray(); - QByteArray antNameBase = settings.value("rtroverBaseAntenna").toByteArray(); - QByteArray antexFileName = settings.value("rtroverAntex").toByteArray(); - - if (!_roverMount.isEmpty()) opt._roverName = _roverMount.data(); - if (!_baseMount.isEmpty()) opt._baseName = _baseMount.data(); - if (!antNameRover.isEmpty()) opt._antNameRover = antNameRover.data(); - if (!antNameBase.isEmpty()) opt._antNameBase = antNameBase.data(); - if (!antexFileName.isEmpty()) opt._antexFileName = antexFileName.data(); - - opt._xyzAprRover[0] = settings.value("rtroverRoverRefCrdX").toDouble(); - opt._xyzAprRover[1] = settings.value("rtroverRoverRefCrdY").toDouble(); - opt._xyzAprRover[2] = settings.value("rtroverRoverRefCrdZ").toDouble(); - opt._xyzAprBase[0] = settings.value("rtroverBaseRefCrdX").toDouble(); - opt._xyzAprBase[1] = settings.value("rtroverBaseRefCrdY").toDouble(); - opt._xyzAprBase[2] = settings.value("rtroverBaseRefCrdZ").toDouble(); - opt._neuEccRover[0] = settings.value("rtroverRoverDN").toDouble(); - opt._neuEccRover[1] = settings.value("rtroverRoverDE").toDouble(); - opt._neuEccRover[2] = settings.value("rtroverRoverDU").toDouble(); - opt._neuEccBase[0] = settings.value("rtroverBaseDN").toDouble(); - opt._neuEccBase[1] = settings.value("rtroverBaseDE").toDouble(); - opt._neuEccBase[2] = settings.value("rtroverBaseDU").toDouble(); - opt._logLevel = 2; - - rtrover_setOptions(&opt); - - // Connect to BNC Signals - // ---------------------- - connect(BNC_CORE, SIGNAL(newCorrections(QList)), - this, SLOT(slotNewCorrections(QList))); - - connect(BNC_CORE, SIGNAL(newEphGPS(gpsephemeris)), - this, SLOT(slotNewEphGPS(gpsephemeris))); - - connect(BNC_CORE, SIGNAL(newEphGlonass(glonassephemeris)), - this, SLOT(slotNewEphGlonass(glonassephemeris))); - - connect(BNC_CORE, SIGNAL(newEphGalileo(galileoephemeris)), - this, SLOT(slotNewEphGalileo(galileoephemeris))); - - // Start processing loop - // --------------------- - QThread::exec(); -} - -// -//////////////////////////////////////////////////////////////////////////// -void t_bncRtrover::slotNewEphGPS(gpsephemeris gpseph) { - QMutexLocker locker(&_mutex); - - bncTime toc(gpseph.GPSweek, gpseph.TOC); - bncTime toe(gpseph.GPSweek, gpseph.TOE); - - rtrover_ephGPS eph; - eph._satellite._system = 'G'; - eph._satellite._number = gpseph.satellite; - eph._TOC._mjd = toc.mjd(); - eph._TOC._sec = toc.daysec(); - eph._TOE._mjd = toe.mjd(); - eph._TOE._sec = toe.daysec(); - eph._IODE = gpseph.IODE; - eph._IODC = gpseph.IODC; - eph._clock_bias = gpseph.clock_bias; - eph._clock_drift = gpseph.clock_drift; - eph._clock_driftrate = gpseph.clock_driftrate; - eph._Crs = gpseph.Crs; - eph._Delta_n = gpseph.Delta_n; - eph._M0 = gpseph.M0; - eph._Cuc = gpseph.Cuc; - eph._e = gpseph.e; - eph._Cus = gpseph.Cus; - eph._sqrt_A = gpseph.sqrt_A; - eph._Cic = gpseph.Cic; - eph._OMEGA0 = gpseph.OMEGA0; - eph._Cis = gpseph.Cis; - eph._i0 = gpseph.i0; - eph._Crc = gpseph.Crc; - eph._omega = gpseph.omega; - eph._OMEGADOT = gpseph.OMEGADOT; - eph._IDOT = gpseph.IDOT; - eph._TGD = gpseph.TGD; - eph._health = gpseph.SVhealth; - - rtrover_putGPSEphemeris(&eph); -} - -// -//////////////////////////////////////////////////////////////////////////// -void t_bncRtrover::slotNewEphGlonass(glonassephemeris gloeph) { - QMutexLocker locker(&_mutex); - - int wwUTC = gloeph.GPSWeek; - int towUTC = gloeph.GPSTOW; - updatetime(&wwUTC, &towUTC, gloeph.tb*1000, 1); // Moscow -> UTC - bncTime tUTC(wwUTC,towUTC); - - int wwGPS = gloeph.GPSWeek; - int towGPS = gloeph.GPSTOW; - updatetime(&wwGPS, &towGPS, gloeph.tb*1000, 0); // Moscow -> GPS - bncTime tGPS(wwGPS,towGPS); - - rtrover_ephGlo eph; - eph._satellite._system = 'R'; - eph._satellite._number = gloeph.almanac_number; - eph._timeUTC._mjd = tUTC.mjd(); - eph._timeUTC._sec = tUTC.daysec(); - eph._gps_utc = int(tGPS-tUTC); - eph._E = gloeph.E; - eph._tau = gloeph.tau; - eph._gamma = gloeph.gamma; - eph._x_pos = gloeph.x_pos; - eph._x_velocity = gloeph.x_velocity; - eph._x_acceleration = gloeph.x_acceleration; - eph._y_pos = gloeph.y_pos; - eph._y_velocity = gloeph.y_velocity; - eph._y_acceleration = gloeph.y_acceleration; - eph._z_pos = gloeph.z_pos; - eph._z_velocity = gloeph.z_velocity; - eph._z_acceleration = gloeph.z_acceleration; - eph._health = 0; // TODO ? - eph._frequency_number = gloeph.frequency_number; - - rtrover_putGloEphemeris(&eph); -} - -// -//////////////////////////////////////////////////////////////////////////// -void t_bncRtrover::slotNewEphGalileo(galileoephemeris /* galeph */) { - // not yet implemented -} - -// -//////////////////////////////////////////////////////////////////////////// -void t_bncRtrover::slotNewCorrections(QList corrList) { - QMutexLocker locker(&_mutex); - - if (corrList.size() == 0) { - return; - } - - int numOrbCorr = 0; - int numClkCorr = 0; - int numBiases = 0; - - rtrover_orbCorr orbCorr[corrList.size()]; - rtrover_clkCorr clkCorr[corrList.size()]; - rtrover_satBiases biases[corrList.size()]; - - QListIterator it(corrList); - while (it.hasNext()) { - QString line = it.next(); - - // Check the Mountpoint - // -------------------- - QStringList hlp = line.split(" "); - if (hlp.size() > 0) { - QString mountpoint = hlp[hlp.size()-1]; - if (mountpoint != _corrMount) { - continue; - } - } - - // Orbit and clock corrections - // --------------------------- - t_corr corr; - if (corr.readLine(line) == success) { - - if (corr.messageType == COTYPE_GPSCOMBINED || - corr.messageType == COTYPE_GLONASSCOMBINED || - corr.messageType == COTYPE_GPSORBIT || - corr.messageType == COTYPE_GLONASSORBIT ) { - - _IODs[corr.prn] = corr.iod; // remember iod; - - ++numOrbCorr; - rtrover_orbCorr& orbC = orbCorr[numOrbCorr-1]; - orbC._satellite._system = corr.prn.toAscii()[0]; - orbC._satellite._number = corr.prn.mid(1).toInt(); - orbC._iod = corr.iod; - orbC._time._mjd = corr.tRao.mjd(); - orbC._time._sec = corr.tRao.daysec(); - orbC._system = 'R'; - for (int ii = 0; ii < 3; ii++) { - orbC._xr[ii] = corr.rao[ii]; - orbC._dotXr[ii] = corr.dotRao[ii]; - } - } - - if (corr.messageType == COTYPE_GPSCOMBINED || - corr.messageType == COTYPE_GLONASSCOMBINED || - corr.messageType == COTYPE_GPSCLOCK || - corr.messageType == COTYPE_GLONASSCLOCK ) { - ++numClkCorr; - rtrover_clkCorr& clkC = clkCorr[numClkCorr-1]; - clkC._satellite._system = corr.prn.toAscii()[0]; - clkC._satellite._number = corr.prn.mid(1).toInt(); - if (_IODs.contains(corr.prn)) { - clkC._iod = _IODs[corr.prn]; - } - else { - clkC._iod = 0; - } - clkC._time._mjd = corr.tClk.mjd(); - clkC._time._sec = corr.tClk.daysec(); - clkC._dClk = corr.dClk; - clkC._dotDClk = corr.dotDClk; - clkC._dotDotDClk = corr.dotDotDClk; - } - } - - // Code Biases - // ----------- - t_bias bias; - if (bias.readLine(line) == success) { - ++numBiases; - rtrover_satBiases& satBiases = biases[numBiases-1]; - satBiases._satellite._system = bias._prn.toAscii()[0]; - satBiases._satellite._number = bias._prn.mid(1).toInt(); - satBiases._time._mjd = bias._time.mjd(); - satBiases._time._sec = bias._time.daysec(); - satBiases._numBiases = bias._value.size(); - satBiases._biases = new rtrover_bias[satBiases._numBiases]; - int iBias = -1; - QMapIterator it(bias._value); - while (it.hasNext()) { - it.next(); - ++iBias; - rtrover_bias& singleBias = satBiases._biases[iBias]; - singleBias._rnxType3ch[0] = 'C'; - singleBias._rnxType3ch[1] = it.key()[0]; - singleBias._rnxType3ch[2] = it.key()[1]; - singleBias._rnxType3ch[3] = '\0'; - singleBias._value = it.value(); - } - } - } - - // Pass Corrections and Biases to client library - // --------------------------------------------- - if (numOrbCorr > 0) { - rtrover_putOrbCorrections(numOrbCorr, orbCorr); - } - if (numClkCorr > 0) { - rtrover_putClkCorrections(numClkCorr, clkCorr); - } - if (numBiases > 0) { - rtrover_putBiases(numBiases, biases); - } - - // Clean the memory - // ---------------- - for (int ii = 0; ii < numBiases; ii++) { - delete [] biases[ii]._biases; - } -} - -// Auxiliary function - copy observation data -//////////////////////////////////////////////////////////////////////////// -void copyObs(const t_obs& obsBnc, rtrover_satObs& satObs) { - - bncTime obsTime(obsBnc.GPSWeek, obsBnc.GPSWeeks); - satObs._satellite._system = obsBnc.satSys; - satObs._satellite._number = obsBnc.satNum; - satObs._time._mjd = obsTime.mjd(); - satObs._time._sec = obsTime.daysec(); - - QMap allObs; - for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) { - if (obsBnc._measdata[iEntry] != 0.0) { - QByteArray rnxStr = obsBnc.rnxStr(iEntry).toAscii(); - if (rnxStr.length() >= 2) { - if (rnxStr == "L1") { - rnxStr = "L1C"; - } - else if (rnxStr == "L2") { - rnxStr = "L2P"; - } - QByteArray codeType = rnxStr.mid(1); - - bool existed = allObs.contains(codeType); - rtrover_obs& currObs = allObs[codeType]; - if (!existed) { - rtrover_initObs(&currObs); - currObs._rnxType2ch[0] = codeType[0]; - currObs._rnxType2ch[1] = codeType[1]; - currObs._rnxType2ch[2] = '\0'; - } - - if (rnxStr[0] == 'C') { - currObs._code = obsBnc._measdata[iEntry]; - currObs._codeValid = true; - } - else if (rnxStr[0] == 'L') { - currObs._phase = obsBnc._measdata[iEntry]; - currObs._phaseValid = true; - if (codeType[0] == '1') { - currObs._slip = obsBnc.slipL1; - currObs._slipCounter = obsBnc.slip_cnt_L1; - } - else if (codeType[0] == '2') { - currObs._slip = obsBnc.slipL2; - currObs._slipCounter = obsBnc.slip_cnt_L2; - } - else if (codeType[0] == '5') { - currObs._slip = obsBnc.slipL5; - currObs._slipCounter = obsBnc.slip_cnt_L5; - } - } - else if (rnxStr[0] == 'D') { - currObs._doppler = obsBnc._measdata[iEntry]; - currObs._dopplerValid = true; - } - else if (rnxStr[0] == 'S') { - currObs._snr = obsBnc._measdata[iEntry]; - currObs._snrValid = true; - } - } - } - } - satObs._numObs = allObs.size(); - satObs._obs = new rtrover_obs[satObs._numObs]; - int iObs = -1; - QMapIterator it(allObs); - while (it.hasNext()) { - it.next(); - ++iObs; - satObs._obs[iObs] = it.value(); - } -} - -// -//////////////////////////////////////////////////////////////////////////// -void t_bncRtrover::slotNewObs(QByteArray staID, QList obsList) { - QMutexLocker locker(&_mutex); - - if (staID != _roverMount && staID != _baseMount) { - return; - } - - // Store Observations into epochs - // ------------------------------ - QListIterator it(obsList); - while (it.hasNext()) { - - const t_obs& obsIn = it.next(); - - bncTime obsTime(obsIn.GPSWeek, obsIn.GPSWeeks); - - // Find corresponding epoch or create a new one - // -------------------------------------------- - t_epoData* epoData = 0; - for (unsigned ii = 0; ii < _epochs.size(); ii++) { - if (_epochs[ii]->_time == obsTime) { - epoData = _epochs[ii]; - break; - } - } - if (epoData == 0) { - if (_epochs.size() == 0 || _epochs.back()->_time < obsTime) { - epoData = new t_epoData(); - epoData->_time = obsTime; - _epochs.push_back(epoData); - } - else { - continue; - } - } - - // Store observation into epoch class - // ---------------------------------- - if (staID == _roverMount) { - epoData->_obsRover.push_back(obsIn); - } - else if (staID == _baseMount) { - epoData->_obsBase.push_back(obsIn); - } - } - - // Process Epochs - // -------------- - while (_epochs.size() > 1) { - const double WAITTIME = 5.0; - double dt = _epochs.back()->_time - _epochs.front()->_time; - if (dt > WAITTIME) { - processFrontEpoch(); - } - else { - break; - } - } -} - -// -//////////////////////////////////////////////////////////////////////////// -void t_bncRtrover::processFrontEpoch() { - - // Copy observations into rtrover_satObs structures - // ------------------------------------------------ - t_epoData* frontEpoData = _epochs.front(); - _epochs.erase(_epochs.begin()); - - int numSatRover = frontEpoData->_obsRover.size(); - rtrover_satObs satObsRover[numSatRover]; - for (int ii = 0; ii < numSatRover; ii++) { - const t_obs& obsBnc = frontEpoData->_obsRover[ii]; - rtrover_satObs& satObs = satObsRover[ii]; - copyObs(obsBnc, satObs); - } - - int numSatBase = frontEpoData->_obsBase.size(); - rtrover_satObs satObsBase[numSatBase]; - for (int ii = 0; ii < numSatBase; ii++) { - const t_obs& obsBnc = frontEpoData->_obsBase[ii]; - rtrover_satObs& satObs = satObsBase[ii]; - copyObs(obsBnc, satObs); - } - - delete frontEpoData; - - // Process single epoch - // -------------------- - rtrover_output output; - rtrover_processEpoch(numSatRover, satObsRover, numSatBase, satObsBase, &output); - - // Write output - // --------------------- - _outputFile.write(output._log); - _outputFile.flush(); - - // Free memory - // ----------- - rtrover_freeOutput(&output); - for (int ii = 0; ii < numSatRover; ii++) { - rtrover_satObs& satObs = satObsRover[ii]; - delete [] satObs._obs; - } - for (int ii = 0; ii < numSatBase; ii++) { - rtrover_satObs& satObs = satObsBase[ii]; - delete [] satObs._obs; - } -} Index: unk/BNC/src/PPP/bncrtrover.h =================================================================== --- /trunk/BNC/src/PPP/bncrtrover.h (revision 5676) +++ (revision ) @@ -1,42 +1,0 @@ -#ifndef BNCRTROVER_H -#define BNCRTROVER_H - -#include - -#include "RTCM/GPSDecoder.h" -#include "bncephuser.h" - -class t_bncRtrover : public QThread { - Q_OBJECT - - public: - t_bncRtrover(); - ~t_bncRtrover(); - virtual void run(); - - public slots: - void slotNewEphGPS(gpsephemeris gpseph); - void slotNewEphGlonass(glonassephemeris gloeph); - void slotNewEphGalileo(galileoephemeris galeph); - void slotNewCorrections(QList corrList); - void slotNewObs(QByteArray staID, QList obsList); - - private: - class t_epoData { - public: - bncTime _time; - std::vector _obsRover; - std::vector _obsBase; - }; - void processFrontEpoch(); - QMutex _mutex; - QByteArray _mode; - QFile _outputFile; - QByteArray _roverMount; - QByteArray _baseMount; - QByteArray _corrMount; - QMap _IODs; - std::vector _epochs; -}; - -#endif Index: /trunk/BNC/src/PPP/pppThread.cpp =================================================================== --- /trunk/BNC/src/PPP/pppThread.cpp (revision 5677) +++ /trunk/BNC/src/PPP/pppThread.cpp (revision 5677) @@ -0,0 +1,507 @@ + +#include +#include +#include + +#include "bncrtrover.h" +#include "bnccore.h" +#include "bncsettings.h" +#include "bnctime.h" + +#include "rtrover_interface.h" + +using namespace std; + +// Constructor +//////////////////////////////////////////////////////////////////////////// +t_bncRtrover::t_bncRtrover() : QThread(0) { +} + +// Destructor +//////////////////////////////////////////////////////////////////////////// +t_bncRtrover::~t_bncRtrover() { + rtrover_destroy(); +} + +// Run (virtual) +//////////////////////////////////////////////////////////////////////////// +void t_bncRtrover::run() { + bncSettings settings; + + // User Options + // ------------ + _mode = settings.value("rtroverMode").toByteArray(); + _roverMount = settings.value("rtroverRoverMount").toByteArray(); + _baseMount = settings.value("rtroverBaseMount").toByteArray(); + _corrMount = settings.value("rtroverCorrMount").toByteArray(); + _outputFile.setFileName(settings.value("rtroverOutput").toString()); + _outputFile.open(QIODevice::WriteOnly | QIODevice::Text); + + // Define Input Options + // -------------------- + rtrover_opt opt; + rtrover_initOptions(&opt); + + if (_mode == "PPP_DF") { + opt._mode = mode_PPP_DF; + } + else if (_mode == "SPP_DF") { + opt._mode = mode_SPP_DF; + } + else if (_mode == "PPP_SF") { + opt._mode = mode_PPP_SF; + } + else if (_mode == "SPP_SF") { + opt._mode = mode_SPP_SF; + } + else if (_mode == "PPP_AR") { + opt._mode = mode_PPP_AR; + } + else if (_mode == "RTK") { + opt._mode = mode_RTK; + } + else if (_mode == "PPP_FTTF") { + opt._mode = mode_PPP_FTTF; + } + + QByteArray antNameRover = settings.value("rtroverRoverAntenna").toByteArray(); + QByteArray antNameBase = settings.value("rtroverBaseAntenna").toByteArray(); + QByteArray antexFileName = settings.value("rtroverAntex").toByteArray(); + + if (!_roverMount.isEmpty()) opt._roverName = _roverMount.data(); + if (!_baseMount.isEmpty()) opt._baseName = _baseMount.data(); + if (!antNameRover.isEmpty()) opt._antNameRover = antNameRover.data(); + if (!antNameBase.isEmpty()) opt._antNameBase = antNameBase.data(); + if (!antexFileName.isEmpty()) opt._antexFileName = antexFileName.data(); + + opt._xyzAprRover[0] = settings.value("rtroverRoverRefCrdX").toDouble(); + opt._xyzAprRover[1] = settings.value("rtroverRoverRefCrdY").toDouble(); + opt._xyzAprRover[2] = settings.value("rtroverRoverRefCrdZ").toDouble(); + opt._xyzAprBase[0] = settings.value("rtroverBaseRefCrdX").toDouble(); + opt._xyzAprBase[1] = settings.value("rtroverBaseRefCrdY").toDouble(); + opt._xyzAprBase[2] = settings.value("rtroverBaseRefCrdZ").toDouble(); + opt._neuEccRover[0] = settings.value("rtroverRoverDN").toDouble(); + opt._neuEccRover[1] = settings.value("rtroverRoverDE").toDouble(); + opt._neuEccRover[2] = settings.value("rtroverRoverDU").toDouble(); + opt._neuEccBase[0] = settings.value("rtroverBaseDN").toDouble(); + opt._neuEccBase[1] = settings.value("rtroverBaseDE").toDouble(); + opt._neuEccBase[2] = settings.value("rtroverBaseDU").toDouble(); + opt._logLevel = 2; + + rtrover_setOptions(&opt); + + // Connect to BNC Signals + // ---------------------- + connect(BNC_CORE, SIGNAL(newCorrections(QList)), + this, SLOT(slotNewCorrections(QList))); + + connect(BNC_CORE, SIGNAL(newEphGPS(gpsephemeris)), + this, SLOT(slotNewEphGPS(gpsephemeris))); + + connect(BNC_CORE, SIGNAL(newEphGlonass(glonassephemeris)), + this, SLOT(slotNewEphGlonass(glonassephemeris))); + + connect(BNC_CORE, SIGNAL(newEphGalileo(galileoephemeris)), + this, SLOT(slotNewEphGalileo(galileoephemeris))); + + // Start processing loop + // --------------------- + QThread::exec(); +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_bncRtrover::slotNewEphGPS(gpsephemeris gpseph) { + QMutexLocker locker(&_mutex); + + bncTime toc(gpseph.GPSweek, gpseph.TOC); + bncTime toe(gpseph.GPSweek, gpseph.TOE); + + rtrover_ephGPS eph; + eph._satellite._system = 'G'; + eph._satellite._number = gpseph.satellite; + eph._TOC._mjd = toc.mjd(); + eph._TOC._sec = toc.daysec(); + eph._TOE._mjd = toe.mjd(); + eph._TOE._sec = toe.daysec(); + eph._IODE = gpseph.IODE; + eph._IODC = gpseph.IODC; + eph._clock_bias = gpseph.clock_bias; + eph._clock_drift = gpseph.clock_drift; + eph._clock_driftrate = gpseph.clock_driftrate; + eph._Crs = gpseph.Crs; + eph._Delta_n = gpseph.Delta_n; + eph._M0 = gpseph.M0; + eph._Cuc = gpseph.Cuc; + eph._e = gpseph.e; + eph._Cus = gpseph.Cus; + eph._sqrt_A = gpseph.sqrt_A; + eph._Cic = gpseph.Cic; + eph._OMEGA0 = gpseph.OMEGA0; + eph._Cis = gpseph.Cis; + eph._i0 = gpseph.i0; + eph._Crc = gpseph.Crc; + eph._omega = gpseph.omega; + eph._OMEGADOT = gpseph.OMEGADOT; + eph._IDOT = gpseph.IDOT; + eph._TGD = gpseph.TGD; + eph._health = gpseph.SVhealth; + + rtrover_putGPSEphemeris(&eph); +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_bncRtrover::slotNewEphGlonass(glonassephemeris gloeph) { + QMutexLocker locker(&_mutex); + + int wwUTC = gloeph.GPSWeek; + int towUTC = gloeph.GPSTOW; + updatetime(&wwUTC, &towUTC, gloeph.tb*1000, 1); // Moscow -> UTC + bncTime tUTC(wwUTC,towUTC); + + int wwGPS = gloeph.GPSWeek; + int towGPS = gloeph.GPSTOW; + updatetime(&wwGPS, &towGPS, gloeph.tb*1000, 0); // Moscow -> GPS + bncTime tGPS(wwGPS,towGPS); + + rtrover_ephGlo eph; + eph._satellite._system = 'R'; + eph._satellite._number = gloeph.almanac_number; + eph._timeUTC._mjd = tUTC.mjd(); + eph._timeUTC._sec = tUTC.daysec(); + eph._gps_utc = int(tGPS-tUTC); + eph._E = gloeph.E; + eph._tau = gloeph.tau; + eph._gamma = gloeph.gamma; + eph._x_pos = gloeph.x_pos; + eph._x_velocity = gloeph.x_velocity; + eph._x_acceleration = gloeph.x_acceleration; + eph._y_pos = gloeph.y_pos; + eph._y_velocity = gloeph.y_velocity; + eph._y_acceleration = gloeph.y_acceleration; + eph._z_pos = gloeph.z_pos; + eph._z_velocity = gloeph.z_velocity; + eph._z_acceleration = gloeph.z_acceleration; + eph._health = 0; // TODO ? + eph._frequency_number = gloeph.frequency_number; + + rtrover_putGloEphemeris(&eph); +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_bncRtrover::slotNewEphGalileo(galileoephemeris /* galeph */) { + // not yet implemented +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_bncRtrover::slotNewCorrections(QList corrList) { + QMutexLocker locker(&_mutex); + + if (corrList.size() == 0) { + return; + } + + int numOrbCorr = 0; + int numClkCorr = 0; + int numBiases = 0; + + rtrover_orbCorr orbCorr[corrList.size()]; + rtrover_clkCorr clkCorr[corrList.size()]; + rtrover_satBiases biases[corrList.size()]; + + QListIterator it(corrList); + while (it.hasNext()) { + QString line = it.next(); + + // Check the Mountpoint + // -------------------- + QStringList hlp = line.split(" "); + if (hlp.size() > 0) { + QString mountpoint = hlp[hlp.size()-1]; + if (mountpoint != _corrMount) { + continue; + } + } + + // Orbit and clock corrections + // --------------------------- + t_corr corr; + if (corr.readLine(line) == success) { + + if (corr.messageType == COTYPE_GPSCOMBINED || + corr.messageType == COTYPE_GLONASSCOMBINED || + corr.messageType == COTYPE_GPSORBIT || + corr.messageType == COTYPE_GLONASSORBIT ) { + + _IODs[corr.prn] = corr.iod; // remember iod; + + ++numOrbCorr; + rtrover_orbCorr& orbC = orbCorr[numOrbCorr-1]; + orbC._satellite._system = corr.prn.toAscii()[0]; + orbC._satellite._number = corr.prn.mid(1).toInt(); + orbC._iod = corr.iod; + orbC._time._mjd = corr.tRao.mjd(); + orbC._time._sec = corr.tRao.daysec(); + orbC._system = 'R'; + for (int ii = 0; ii < 3; ii++) { + orbC._xr[ii] = corr.rao[ii]; + orbC._dotXr[ii] = corr.dotRao[ii]; + } + } + + if (corr.messageType == COTYPE_GPSCOMBINED || + corr.messageType == COTYPE_GLONASSCOMBINED || + corr.messageType == COTYPE_GPSCLOCK || + corr.messageType == COTYPE_GLONASSCLOCK ) { + ++numClkCorr; + rtrover_clkCorr& clkC = clkCorr[numClkCorr-1]; + clkC._satellite._system = corr.prn.toAscii()[0]; + clkC._satellite._number = corr.prn.mid(1).toInt(); + if (_IODs.contains(corr.prn)) { + clkC._iod = _IODs[corr.prn]; + } + else { + clkC._iod = 0; + } + clkC._time._mjd = corr.tClk.mjd(); + clkC._time._sec = corr.tClk.daysec(); + clkC._dClk = corr.dClk; + clkC._dotDClk = corr.dotDClk; + clkC._dotDotDClk = corr.dotDotDClk; + } + } + + // Code Biases + // ----------- + t_bias bias; + if (bias.readLine(line) == success) { + ++numBiases; + rtrover_satBiases& satBiases = biases[numBiases-1]; + satBiases._satellite._system = bias._prn.toAscii()[0]; + satBiases._satellite._number = bias._prn.mid(1).toInt(); + satBiases._time._mjd = bias._time.mjd(); + satBiases._time._sec = bias._time.daysec(); + satBiases._numBiases = bias._value.size(); + satBiases._biases = new rtrover_bias[satBiases._numBiases]; + int iBias = -1; + QMapIterator it(bias._value); + while (it.hasNext()) { + it.next(); + ++iBias; + rtrover_bias& singleBias = satBiases._biases[iBias]; + singleBias._rnxType3ch[0] = 'C'; + singleBias._rnxType3ch[1] = it.key()[0]; + singleBias._rnxType3ch[2] = it.key()[1]; + singleBias._rnxType3ch[3] = '\0'; + singleBias._value = it.value(); + } + } + } + + // Pass Corrections and Biases to client library + // --------------------------------------------- + if (numOrbCorr > 0) { + rtrover_putOrbCorrections(numOrbCorr, orbCorr); + } + if (numClkCorr > 0) { + rtrover_putClkCorrections(numClkCorr, clkCorr); + } + if (numBiases > 0) { + rtrover_putBiases(numBiases, biases); + } + + // Clean the memory + // ---------------- + for (int ii = 0; ii < numBiases; ii++) { + delete [] biases[ii]._biases; + } +} + +// Auxiliary function - copy observation data +//////////////////////////////////////////////////////////////////////////// +void copyObs(const t_obs& obsBnc, rtrover_satObs& satObs) { + + bncTime obsTime(obsBnc.GPSWeek, obsBnc.GPSWeeks); + satObs._satellite._system = obsBnc.satSys; + satObs._satellite._number = obsBnc.satNum; + satObs._time._mjd = obsTime.mjd(); + satObs._time._sec = obsTime.daysec(); + + QMap allObs; + for (int iEntry = 0; iEntry < GNSSENTRY_NUMBER; ++iEntry) { + if (obsBnc._measdata[iEntry] != 0.0) { + QByteArray rnxStr = obsBnc.rnxStr(iEntry).toAscii(); + if (rnxStr.length() >= 2) { + if (rnxStr == "L1") { + rnxStr = "L1C"; + } + else if (rnxStr == "L2") { + rnxStr = "L2P"; + } + QByteArray codeType = rnxStr.mid(1); + + bool existed = allObs.contains(codeType); + rtrover_obs& currObs = allObs[codeType]; + if (!existed) { + rtrover_initObs(&currObs); + currObs._rnxType2ch[0] = codeType[0]; + currObs._rnxType2ch[1] = codeType[1]; + currObs._rnxType2ch[2] = '\0'; + } + + if (rnxStr[0] == 'C') { + currObs._code = obsBnc._measdata[iEntry]; + currObs._codeValid = true; + } + else if (rnxStr[0] == 'L') { + currObs._phase = obsBnc._measdata[iEntry]; + currObs._phaseValid = true; + if (codeType[0] == '1') { + currObs._slip = obsBnc.slipL1; + currObs._slipCounter = obsBnc.slip_cnt_L1; + } + else if (codeType[0] == '2') { + currObs._slip = obsBnc.slipL2; + currObs._slipCounter = obsBnc.slip_cnt_L2; + } + else if (codeType[0] == '5') { + currObs._slip = obsBnc.slipL5; + currObs._slipCounter = obsBnc.slip_cnt_L5; + } + } + else if (rnxStr[0] == 'D') { + currObs._doppler = obsBnc._measdata[iEntry]; + currObs._dopplerValid = true; + } + else if (rnxStr[0] == 'S') { + currObs._snr = obsBnc._measdata[iEntry]; + currObs._snrValid = true; + } + } + } + } + satObs._numObs = allObs.size(); + satObs._obs = new rtrover_obs[satObs._numObs]; + int iObs = -1; + QMapIterator it(allObs); + while (it.hasNext()) { + it.next(); + ++iObs; + satObs._obs[iObs] = it.value(); + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_bncRtrover::slotNewObs(QByteArray staID, QList obsList) { + QMutexLocker locker(&_mutex); + + if (staID != _roverMount && staID != _baseMount) { + return; + } + + // Store Observations into epochs + // ------------------------------ + QListIterator it(obsList); + while (it.hasNext()) { + + const t_obs& obsIn = it.next(); + + bncTime obsTime(obsIn.GPSWeek, obsIn.GPSWeeks); + + // Find corresponding epoch or create a new one + // -------------------------------------------- + t_epoData* epoData = 0; + for (unsigned ii = 0; ii < _epochs.size(); ii++) { + if (_epochs[ii]->_time == obsTime) { + epoData = _epochs[ii]; + break; + } + } + if (epoData == 0) { + if (_epochs.size() == 0 || _epochs.back()->_time < obsTime) { + epoData = new t_epoData(); + epoData->_time = obsTime; + _epochs.push_back(epoData); + } + else { + continue; + } + } + + // Store observation into epoch class + // ---------------------------------- + if (staID == _roverMount) { + epoData->_obsRover.push_back(obsIn); + } + else if (staID == _baseMount) { + epoData->_obsBase.push_back(obsIn); + } + } + + // Process Epochs + // -------------- + while (_epochs.size() > 1) { + const double WAITTIME = 5.0; + double dt = _epochs.back()->_time - _epochs.front()->_time; + if (dt > WAITTIME) { + processFrontEpoch(); + } + else { + break; + } + } +} + +// +//////////////////////////////////////////////////////////////////////////// +void t_bncRtrover::processFrontEpoch() { + + // Copy observations into rtrover_satObs structures + // ------------------------------------------------ + t_epoData* frontEpoData = _epochs.front(); + _epochs.erase(_epochs.begin()); + + int numSatRover = frontEpoData->_obsRover.size(); + rtrover_satObs satObsRover[numSatRover]; + for (int ii = 0; ii < numSatRover; ii++) { + const t_obs& obsBnc = frontEpoData->_obsRover[ii]; + rtrover_satObs& satObs = satObsRover[ii]; + copyObs(obsBnc, satObs); + } + + int numSatBase = frontEpoData->_obsBase.size(); + rtrover_satObs satObsBase[numSatBase]; + for (int ii = 0; ii < numSatBase; ii++) { + const t_obs& obsBnc = frontEpoData->_obsBase[ii]; + rtrover_satObs& satObs = satObsBase[ii]; + copyObs(obsBnc, satObs); + } + + delete frontEpoData; + + // Process single epoch + // -------------------- + rtrover_output output; + rtrover_processEpoch(numSatRover, satObsRover, numSatBase, satObsBase, &output); + + // Write output + // --------------------- + _outputFile.write(output._log); + _outputFile.flush(); + + // Free memory + // ----------- + rtrover_freeOutput(&output); + for (int ii = 0; ii < numSatRover; ii++) { + rtrover_satObs& satObs = satObsRover[ii]; + delete [] satObs._obs; + } + for (int ii = 0; ii < numSatBase; ii++) { + rtrover_satObs& satObs = satObsBase[ii]; + delete [] satObs._obs; + } +} Index: /trunk/BNC/src/PPP/pppThread.h =================================================================== --- /trunk/BNC/src/PPP/pppThread.h (revision 5677) +++ /trunk/BNC/src/PPP/pppThread.h (revision 5677) @@ -0,0 +1,42 @@ +#ifndef BNCRTROVER_H +#define BNCRTROVER_H + +#include + +#include "RTCM/GPSDecoder.h" +#include "bncephuser.h" + +class t_bncRtrover : public QThread { + Q_OBJECT + + public: + t_bncRtrover(); + ~t_bncRtrover(); + virtual void run(); + + public slots: + void slotNewEphGPS(gpsephemeris gpseph); + void slotNewEphGlonass(glonassephemeris gloeph); + void slotNewEphGalileo(galileoephemeris galeph); + void slotNewCorrections(QList corrList); + void slotNewObs(QByteArray staID, QList obsList); + + private: + class t_epoData { + public: + bncTime _time; + std::vector _obsRover; + std::vector _obsBase; + }; + void processFrontEpoch(); + QMutex _mutex; + QByteArray _mode; + QFile _outputFile; + QByteArray _roverMount; + QByteArray _baseMount; + QByteArray _corrMount; + QMap _IODs; + std::vector _epochs; +}; + +#endif