Index: /trunk/BNC/RTCM/GPSDecoder.h =================================================================== --- /trunk/BNC/RTCM/GPSDecoder.h (revision 1023) +++ /trunk/BNC/RTCM/GPSDecoder.h (revision 1024) @@ -47,4 +47,6 @@ double L1; // L1 carrier phase (cycles) double L2; // L2 carrier phase (cycles) + int slip_cnt_L1; // L1 cumulative loss of continuity indicator + int slip_cnt_L2; // L2 cumulative loss of continuity indicator double S1; // L1 signal-to noise ratio double S2; // L2 signal-to noise ratio Index: /trunk/BNC/RTCM/RTCM2.cpp =================================================================== --- /trunk/BNC/RTCM/RTCM2.cpp (revision 1023) +++ /trunk/BNC/RTCM/RTCM2.cpp (revision 1024) @@ -945,4 +945,6 @@ cph_L1.resize(0); // Carrier phase [m] cph_L2.resize(0); // Carrier phase [m] + slip_L1.resize(0); // Slip counter + slip_L2.resize(0); // Slip counter availability.reset(); // Message status flags @@ -1005,5 +1007,5 @@ bool isGPS,isCAcode,isL1,isOth; int NSat,idx; - int sid,prn; + int sid,prn,slip_cnt; double t,rng,cph; @@ -1117,4 +1119,7 @@ // Carrier phase measurement (mod 2^23 [cy]; sign matched to range) cph = -P.getBits(iSat*48+40,32)/256.0; + + // Slip counter + slip_cnt = P.getUnsignedBits(iSat*48+35,5); // Is this a new PRN? @@ -1132,12 +1137,16 @@ cph_L1.push_back(0.0); cph_L2.push_back(0.0); + slip_L1.push_back(-1); + slip_L2.push_back(-1); }; // Store measurement if (isL1) { - cph_L1[idx] = cph; + cph_L1 [idx] = cph; + slip_L1[idx] = slip_cnt; } else { - cph_L2[idx] = cph; + cph_L2 [idx] = cph; + slip_L2[idx] = slip_cnt; }; @@ -1339,2 +1348,5 @@ }; // End of namespace rtcm2 + + + Index: /trunk/BNC/RTCM/RTCM2.h =================================================================== --- /trunk/BNC/RTCM/RTCM2.h (revision 1023) +++ /trunk/BNC/RTCM/RTCM2.h (revision 1024) @@ -309,4 +309,6 @@ std::vector cph_L1; // Carrier phase on L1 [cy] std::vector cph_L2; // Carrier phase on L2 [cy] + std::vector slip_L1; // Carrier phase slip counter, L1 + std::vector slip_L2; // Carrier phase slip counter, L1 private: Index: /trunk/BNC/RTCM/RTCM2Decoder.cpp =================================================================== --- /trunk/BNC/RTCM/RTCM2Decoder.cpp (revision 1023) +++ /trunk/BNC/RTCM/RTCM2Decoder.cpp (revision 1024) @@ -39,9 +39,14 @@ * -----------------------------------------------------------------------*/ +#include +#include + #include "../bncutils.h" +#include "rtcm_utils.h" #include "GPSDecoder.h" #include "RTCM2Decoder.h" using namespace std; +using namespace rtcm2; // @@ -49,6 +54,6 @@ // -RTCM2Decoder::RTCM2Decoder() { - +RTCM2Decoder::RTCM2Decoder(const std::string& ID) { + _ID = ID; } @@ -58,8 +63,25 @@ RTCM2Decoder::~RTCM2Decoder() { -} - -// - + for (t_pairMap::iterator ii = _ephPair.begin(); ii != _ephPair.end(); ii++) { + delete ii->second; + } +} + + +// +t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz) { + if ( !_msg03.validMsg ) { + return failure; + } + + xx = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0); + yy = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0); + zz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0); + + return success; +} + + +// t_irc RTCM2Decoder::Decode(char* buffer, int bufLen) { @@ -95,24 +117,256 @@ _obsList.push_back(obs); if (_ObsBlock.PRN[iSat] > 100) { - obs->_o.satNum = _ObsBlock.PRN[iSat] % 100; - obs->_o.satSys = 'R'; - } - else { - obs->_o.satNum = _ObsBlock.PRN[iSat]; - obs->_o.satSys = 'G'; - } - obs->_o.GPSWeek = epochWeek; - obs->_o.GPSWeeks = epochSecs; - obs->_o.C1 = _ObsBlock.rng_C1[iSat]; - obs->_o.P1 = _ObsBlock.rng_P1[iSat]; - obs->_o.P2 = _ObsBlock.rng_P2[iSat]; - obs->_o.L1 = _ObsBlock.resolvedPhase_L1(iSat); - obs->_o.L2 = _ObsBlock.resolvedPhase_L2(iSat); + obs->_o.satNum = _ObsBlock.PRN[iSat] % 100; + obs->_o.satSys = 'R'; + } + else { + obs->_o.satNum = _ObsBlock.PRN[iSat]; + obs->_o.satSys = 'G'; + } + obs->_o.GPSWeek = epochWeek; + obs->_o.GPSWeeks = epochSecs; + obs->_o.C1 = _ObsBlock.rng_C1[iSat]; + obs->_o.P1 = _ObsBlock.rng_P1[iSat]; + obs->_o.P2 = _ObsBlock.rng_P2[iSat]; + obs->_o.L1 = _ObsBlock.resolvedPhase_L1(iSat); + obs->_o.L2 = _ObsBlock.resolvedPhase_L2(iSat); + obs->_o.slip_cnt_L1 = _ObsBlock.slip_L1[iSat]; + obs->_o.slip_cnt_L2 = _ObsBlock.slip_L2[iSat]; } _ObsBlock.clear(); } } + + else if ( _PP.ID() == 20 || _PP.ID() == 21 ) { + _msg2021.extract(_PP); + + if (_msg2021.valid()) { + translateCorr2Obs(); + } + } + + else if ( _PP.ID() == 3 ) { + _msg03.extract(_PP); + } + + else if ( _PP.ID() == 22 ) { + _msg22.extract(_PP); + } } return success; } + + +void RTCM2Decoder::storeEph(const gpsephemeris& gpseph) { + t_ephGPS* eph = new t_ephGPS; eph->set(&gpseph); + + string prn = eph->prn(); + + t_pairMap::iterator ip = _ephPair.find(prn); + if (ip == _ephPair.end() ) { + ip = _ephPair.insert(pair(prn, new t_ephPair)).first; + } + t_ephPair* pair = ip->second; + + if ( !pair->eph || eph->isNewerThan(pair->eph) ) { + delete pair->oldEph; + pair->oldEph = pair->eph; + pair->eph = eph; + + return; + } + + delete eph; +} + + +void RTCM2Decoder::storeEph(const t_ephGPS& gpseph) { + t_ephGPS* eph = new t_ephGPS(gpseph); + + string prn = eph->prn(); + + t_pairMap::iterator ip = _ephPair.find(prn); + if (ip == _ephPair.end() ) { + ip = _ephPair.insert(pair(prn, new t_ephPair)).first; + } + t_ephPair* pair = ip->second; + + if ( !pair->eph || eph->isNewerThan(pair->eph) ) { + delete pair->oldEph; + pair->oldEph = pair->eph; + pair->eph = eph; + + return; + } + + delete eph; +} + + +void RTCM2Decoder::translateCorr2Obs() { + + if ( !_msg03.validMsg || !_msg2021.valid() ) { + return; + } + + double stax = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0); + double stay = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0); + double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0); + + int refWeek; + double refSecs; + currentGPSWeeks(refWeek, refSecs); + + // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1) + // ---------------------------------------------------------------------------------- + double hoursec_est = _msg2021.hoursec(); // estimated time of measurement + double hoursec_rcv = rint(hoursec_est * 1e2) / 1e2; // receiver clock reading at hoursec_est + double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light; + + int GPSWeek; + double GPSWeeks; + resolveEpoch(hoursec_est, refWeek, refSecs, + GPSWeek, GPSWeeks); + + int GPSWeek_rcv; + double GPSWeeks_rcv; + resolveEpoch(hoursec_rcv, refWeek, refSecs, + GPSWeek_rcv, GPSWeeks_rcv); + + // Loop over all satellites + // ------------------------ + for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin(); + icorr != _msg2021.data.end(); icorr++) { + const RTCM2_2021::HiResCorr* corr = icorr->second; + + ostringstream oPRN; oPRN.fill('0'); + + oPRN << (corr->PRN < 200 ? 'G' : 'R') + << setw(2) << (corr->PRN < 200 ? corr->PRN : corr->PRN - 200); + + string PRN(oPRN.str()); + + t_pairMap::const_iterator ieph = _ephPair.find(PRN); + const t_eph* eph0 = 0; + const t_eph* eph1 = 0; + + if ( ieph != _ephPair.end() ) { + eph0 = ieph->second->eph; + eph1 = ieph->second->oldEph; + } + + + if ( !eph0 && !eph1 ) { + continue; + } + + double L1 = 0; + double L2 = 0; + double P1 = 0; + double P2 = 0; + string obsT = ""; + + // new observation + p_obs new_obs = 0; + + for (unsigned ii = 0; ii < 4; ii++) { + int IODcorr = 0; + double corrVal = 0; + const t_eph* eph = 0; + double* obsVal = 0; + + switch (ii) { + case 0: // --- L1 --- + IODcorr = corr->IODp1; + corrVal = corr->phase1 * LAMBDA_1; + obsVal = &L1; + obsT = "L1"; + break; + case 1: // --- L2 --- + IODcorr = corr->IODp2; + corrVal = corr->phase2 * LAMBDA_2; + obsVal = &L2; + obsT = "L2"; + break; + case 2: // --- P1 --- + IODcorr = corr->IODr1; + corrVal = corr->range1; + obsVal = &P1; + obsT = "P1"; + break; + case 3: // --- P2 --- + IODcorr = corr->IODr2; + corrVal = corr->range2; + obsVal = &P2; + obsT = "P2"; + break; + default: + continue; + } + + eph = 0; + if ( eph0 && eph0->IOD() == IODcorr ) + eph = eph0; + else if ( eph1 && eph1->IOD() == IODcorr ) + eph = eph1; + if ( eph && corr ) { + int GPSWeek_tot; + double GPSWeeks_tot; + double rho, xSat, ySat, zSat, clkSat; + cmpRho(eph, stax, stay, staz, + GPSWeek, GPSWeeks, + rho, GPSWeek_tot, GPSWeeks_tot, + xSat, ySat, zSat, clkSat); + + *obsVal = rho - corrVal + rcv_clk_bias - clkSat; + + if ( *obsVal == 0 ) *obsVal = ZEROVALUE; + + // Allocate new memory + // ------------------- + if ( !new_obs ) { + new_obs = new t_obs(); + + new_obs->_o.StatID[0] = '\x0'; + new_obs->_o.satSys = (corr->PRN < 200 ? 'G' : 'R'); + new_obs->_o.satNum = (corr->PRN < 200 ? corr->PRN : corr->PRN - 200); + + new_obs->_o.GPSWeek = GPSWeek_rcv; + new_obs->_o.GPSWeeks = GPSWeeks_rcv; + } + + // Store estimated measurements + // ---------------------------- + switch (ii) { + case 0: // --- L1 --- + new_obs->_o.L1 = *obsVal / LAMBDA_1; + new_obs->_o.slip_cnt_L1 = corr->lock1; + break; + case 1: // --- L2 --- + new_obs->_o.L2 = *obsVal / LAMBDA_2; + new_obs->_o.slip_cnt_L2 = corr->lock2; + break; + case 2: // --- C1 / P1 --- + if ( corr->Pind1 ) + new_obs->_o.P1 = *obsVal; + else + new_obs->_o.C1 = *obsVal; + break; + case 3: // --- C2 / P2 --- + if ( corr->Pind2 ) + new_obs->_o.P2 = *obsVal; + else + new_obs->_o.C2 = *obsVal; + break; + default: + continue; + } + } + } // loop over frequencies + + if ( new_obs ) { + _obsList.push_back( new_obs ); + } + } +} Index: /trunk/BNC/RTCM/RTCM2Decoder.h =================================================================== --- /trunk/BNC/RTCM/RTCM2Decoder.h (revision 1023) +++ /trunk/BNC/RTCM/RTCM2Decoder.h (revision 1024) @@ -26,20 +26,61 @@ #define INC_RTCM2DECODER_H +#include + #include "GPSDecoder.h" #include "RTCM2.h" +#include "RTCM2_2021.h" +#include "../RTCM3/rtcm3torinex.h" +#include "../RTCM3/ephemeris.h" class RTCM2Decoder: public GPSDecoder { public: - RTCM2Decoder(); + RTCM2Decoder(const std::string& ID); virtual ~RTCM2Decoder(); virtual t_irc Decode(char* buffer, int bufLen); + void storeEph(const gpsephemeris& gpseph); + void storeEph(const t_ephGPS& gpseph); + + t_irc getStaCrd(double& xx, double& yy, double& zz); + + const rtcm2::RTCM2_2021& msg2021() const { return _msg2021; } + private: - std::string _buffer; - rtcm2::RTCM2_Obs _ObsBlock; - rtcm2::RTCM2packet _PP; + class t_ephPair { + public: + t_ephPair() { + eph = 0; + oldEph = 0; + } + + ~t_ephPair() { + delete eph; + delete oldEph; + } + + t_eph* eph; + t_eph* oldEph; + }; + void translateCorr2Obs(); + + std::string _ID; + + std::string _buffer; + rtcm2::RTCM2packet _PP; + + // for messages 18, 19 decoding + rtcm2::RTCM2_Obs _ObsBlock; + + // for messages 20, 21 decoding + rtcm2::RTCM2_03 _msg03; + rtcm2::RTCM2_22 _msg22; + rtcm2::RTCM2_2021 _msg2021; + std::map _ephPair; + + typedef std::map t_pairMap; }; Index: /trunk/BNC/RTCM/RTCM2_2021.cpp =================================================================== --- /trunk/BNC/RTCM/RTCM2_2021.cpp (revision 1024) +++ /trunk/BNC/RTCM/RTCM2_2021.cpp (revision 1024) @@ -0,0 +1,228 @@ +#include +#include +#include + +#include "RTCM2_2021.h" + +using namespace rtcm2; +using namespace std; + +const double ZEROVALUE = 1e-100; + +RTCM2_2021::RTCM2_2021() { } + + +void RTCM2_2021::extract(const RTCM2packet& P) { + if ( !P.valid() || P.ID() != 20 && P.ID() != 21 ) { + return; + } + + // Error: at least 4 data words + if ( P.nDataWords()<5 ) { +#if ( DEBUG > 0 ) + cerr << "Error in RTCM2_Obs::extract(): less than 3 DW (" + << P.nDataWords() << ") detected" << endl; +#endif + return; + }; + + // Error: number of data words has to be odd number + if ( P.nDataWords()%2==0 ){ +#if ( DEBUG > 0 ) + cerr << "Error in RTCM2_Obs::extract(): odd number of DW (" + << P.nDataWords() << ") detected" << endl; +#endif + return; + }; + + // Current epoch (mod 3600 sec) + double tt = 0.6*P.modZCount() + + P.getUnsignedBits(4,20)*1.0e-6; + + // Clear old epoch + if ( tt != tt_ || valid_ ) { + clear(); + tt_ = tt; + valid_ = false; + } + + + // Frequency (exit if neither L1 nor L2) + bool isL1 = ( P.getUnsignedBits(0,1)==0 ); + if ( P.getUnsignedBits(1,1)==1 ) { + return; + } + + // Number of satellites + unsigned nSat = (P.nDataWords() - 1) / 2; + + double multipleMsgInd = true; + for (unsigned iSat = 0; iSat < nSat; iSat++) { + bool multInd = P.getBits (iSat*48 + 24, 1); + bool isGPS = ( P.getUnsignedBits(iSat*48 + 26, 1)==0 ); + unsigned PRN = P.getUnsignedBits(iSat*48 + 27, 5); + + multipleMsgInd = multipleMsgInd && multInd; + + if ( !isGPS ) { + PRN += 200; + } + if ( PRN == 0 ) { + PRN = 32; + } + + HiResCorr* corr = 0; + if ( !(corr = find_i(PRN)) ) { + data_i_[PRN] = HiResCorr(); + corr = &(data_i_[PRN]); + } + if ( !find(PRN) ) { + data[PRN] = corr; + } + + corr->PRN = PRN; + corr->tt = tt_; + + // Message number 20 + if ( P.ID() == 20 ) { + unsigned lossLock = P.getUnsignedBits(iSat*48 + 35, 5); + unsigned IOD = P.getUnsignedBits(iSat*48 + 40, 8); + double corrVal = P.getBits (iSat*48 + 48, 24) / 256.0; + + if ( isL1 ) { + corr->phase1 = (corrVal ? corrVal : ZEROVALUE); + corr->slip1 = (corr->lock1 != lossLock); + corr->lock1 = lossLock; + corr->IODp1 = IOD; + } + else { + corr->phase2 = (corrVal ? corrVal : ZEROVALUE); + corr->slip2 = (corr->lock2 != lossLock); + corr->lock2 = lossLock; + corr->IODp2 = IOD; + } + } + + // Message number 21 + else if ( P.ID() == 21 ) { + double dcorrUnit = ( P.getUnsignedBits(iSat*48 + 32, 1) ? 0.032 : 0.002); + double corrUnit = ( P.getUnsignedBits(iSat*48 + 36, 1) ? 0.320 : 0.020); + unsigned IOD = P.getUnsignedBits(iSat*48 + 40, 8); + double corrVal = P.getBits (iSat*48 + 48, 16) * corrUnit; + double dcorrVal = P.getBits (iSat*48 + 64, 8) * dcorrUnit; + + if ( isL1 ) { + corr-> range1 = (corrVal ? corrVal : ZEROVALUE); + corr->drange1 = dcorrVal; + corr->IODr1 = IOD; + } + else { + corr-> range2 = (corrVal ? corrVal : ZEROVALUE); + corr->drange2 = dcorrVal; + corr->IODr2 = IOD; + } + } + } + + valid_ = !multipleMsgInd; +} + +const RTCM2_2021::HiResCorr* RTCM2_2021::find(unsigned PRN) { + std::map::const_iterator ii = data.find(PRN); + return (ii != data.end() ? ii->second : 0); +} + + +RTCM2_2021::HiResCorr* RTCM2_2021::find_i(unsigned PRN) { + std::map::iterator ii = data_i_.find(PRN); + return (ii != data_i_.end() ? &(ii->second) : 0); +} + + +void RTCM2_2021::clear() { + tt_ = 0; + valid_ = false; + for (map::iterator + ii = data_i_.begin(); ii != data_i_.end(); ii++) { + ii->second.reset(); + } + data.clear(); +} + + +RTCM2_2021::HiResCorr::HiResCorr() : + PRN(0), tt(0), + phase1 (0), phase2 (2), + lock1 (0), lock2 (0), + slip1 (false), slip2 (false), + IODp1 (0), IODp2 (0), + range1 (0), range2 (0), + drange1(0), drange2(0), + Pind1 (false), Pind2 (false), + IODr1 (0), IODr2 (0) { +} + +void RTCM2_2021::HiResCorr::reset() { + // does not reset 'lock' indicators and PRN + tt = 0; + phase1 = 0; + phase2 = 0; + slip1 = false; + slip2 = false; + IODp1 = 0; + IODp2 = 0; + + range1 = 0; + range2 = 0; + drange1 = 0; + drange2 = 0; + IODr1 = 0; + IODr2 = 0; + Pind1 = false; + Pind2 = false; +} + +std::ostream& rtcm2::operator << (std::ostream& out, const RTCM2_2021::HiResCorr& cc) { + out.setf(ios::fixed); + out << setw(8) << setprecision(8) << cc.tt + << ' ' << setw(2) << cc.PRN + << " L1 " + << ' ' << setw(8) << setprecision(3) << (cc.phase1 ? cc.phase1 : 9999.999) + << ' ' << setw(1) << (cc.phase1 ? (cc.slip1 ? '1' : '0') : '.') + << ' ' << setw(2) << (cc.phase1 ? cc.lock1 : 99) + << ' ' << setw(3) << (cc.phase1 ? cc.IODp1 : 999) + << " L2 " + << ' ' << setw(8) << setprecision(3) << (cc.phase2 ? cc.phase2 : 9999.999) + << ' ' << setw(1) << (cc.phase2 ? (cc.slip2 ? '1' : '0') : '.') + << ' ' << setw(2) << (cc.phase2 ? cc.lock2 : 99) + << ' ' << setw(3) << (cc.phase2 ? cc.IODp2 : 999) + << " P1 " + << ' ' << setw(8) << setprecision(3) << (cc.range1 ? cc.range1 : 9999.999) + << ' ' << setw(3) << (cc.range1 ? cc.IODr1 : 999) + << " P2 " + << ' ' << setw(8) << setprecision(3) << (cc.range2 ? cc.range2 : 9999.999) + << ' ' << setw(3) << (cc.phase2 ? cc.IODr2 : 999); + + return out; +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +void RTCM2_22::extract(const RTCM2packet& P) { + if ( P.ID() != 22 ) { + return; + } + + const double dL1unit = 0.01 / 256; + + validMsg = true; + + dL1[0] = P.getBits( 0, 8) * dL1unit; + dL1[1] = P.getBits( 8, 8) * dL1unit; + dL1[2] = P.getBits(16, 8) * dL1unit; +} + +/////////////////////////////// + Index: /trunk/BNC/RTCM/RTCM2_2021.h =================================================================== --- /trunk/BNC/RTCM/RTCM2_2021.h (revision 1024) +++ /trunk/BNC/RTCM/RTCM2_2021.h (revision 1024) @@ -0,0 +1,76 @@ +#include +#include +#include "RTCM2.h" + +namespace rtcm2 { + +class RTCM2_2021 { + + public: + + RTCM2_2021(); // Constructor + + void extract(const RTCM2packet& P); // Packet handler + void clear(); // Initialization + bool valid() const { return valid_; } // Check for complete obs block + + double resolvedPhase_L1(int i) const; // L1 & L2 carrier phase of i-th sat + double resolvedPhase_L2(int i) const; // with resolved 2^24 cy ambiguity + // (based on rng_C1) + + public: + + struct HiResCorr { + + HiResCorr(); + void reset(); + + unsigned PRN; + double tt; + double phase1; + double phase2; + unsigned lock1; + unsigned lock2; + bool slip1; + bool slip2; + unsigned IODp1; + unsigned IODp2; + + double range1; + double range2; + double drange1; + double drange2; + bool Pind1; + bool Pind2; + unsigned IODr1; + unsigned IODr2; + + friend std::ostream& operator << (std::ostream& out, const HiResCorr& cc); + }; + + double hoursec() const { return tt_; } + std::map data; + + typedef std::map::const_iterator c_data_iterator; + typedef std::map::iterator data_iterator; + + private: + const HiResCorr* find (unsigned PRN); + HiResCorr* find_i(unsigned PRN); + + std::map data_i_; + double tt_; + bool valid_; +}; + +class RTCM2_22 { + public: + RTCM2_22() { } + + void extract(const RTCM2packet& P); + + bool validMsg; + double dL1[3]; + double dL2[3]; +}; +}; // end of namespace rtcm2 Index: /trunk/BNC/RTCM/rtcm_utils.cpp =================================================================== --- /trunk/BNC/RTCM/rtcm_utils.cpp (revision 1024) +++ /trunk/BNC/RTCM/rtcm_utils.cpp (revision 1024) @@ -0,0 +1,118 @@ +#include +#include +#include +#include +#include +#include + +#include "rtcm_utils.h" + +using namespace std; + +void resolveEpoch (double secsHour, + int refWeek, double refSecs, + int& epochWeek, double& epochSecs) { + + const double secsPerWeek = 604800.0; + + epochWeek = refWeek; + epochSecs = secsHour + 3600.0*(floor((refSecs-secsHour)/3600.0+0.5)); + + if (epochSecs<0 ) { epochWeek--; epochSecs+=secsPerWeek; }; + if (epochSecs>secsPerWeek) { epochWeek++; epochSecs-=secsPerWeek; }; +}; + + +int cmpRho(const t_eph* eph, + double stax, double stay, double staz, + int GPSWeek, double GPSWeeks, + double& rho, int& GPSWeek_tot, double& GPSWeeks_tot, + double& xSat, double& ySat, double& zSat, double& clkSat) { + + const double omega_earth = 7292115.1467e-11; + const double secsPerWeek = 604800.0; + + // Initial values + // -------------- + rho = 0.0; + eph->position(GPSWeek, GPSWeeks, xSat, ySat, zSat, clkSat); + + cout << "----- cmpRho -----\n"; + eph->print(cout); + cout << " pos " << setw(4) << GPSWeek + << " " << setw(14) << setprecision(6) << GPSWeeks + << " " << setw(13) << setprecision(3) << xSat + << " " << setw(13) << setprecision(3) << ySat + << " " << setw(13) << setprecision(3) << zSat + << endl; + + // Loop until the correct Time Of Transmission is found + // ---------------------------------------------------- + int niter = 0; + double rhoLast = 0; + do { + rhoLast = rho; + + // Correction station position due to Earth Rotation + // ------------------------------------------------- + double dPhi = omega_earth * rho / c_light; + double xRec = stax * cos(dPhi) - stay * sin(dPhi); + double yRec = stay * cos(dPhi) + stax * sin(dPhi); + double zRec = staz; + + double dx = xRec - xSat; + double dy = yRec - ySat; + double dz = zRec - zSat; + + rho = sqrt(dx*dx + dy*dy + dz*dz); + + GPSWeek_tot = GPSWeek; + GPSWeeks_tot = GPSWeeks - rho/c_light; + while ( GPSWeeks_tot < 0 ) { + GPSWeeks_tot += secsPerWeek; + GPSWeek_tot -= 1; + } + while ( GPSWeeks_tot > secsPerWeek ) { + GPSWeeks_tot -= secsPerWeek; + GPSWeek_tot += 1; + } + + eph->position(GPSWeek_tot, GPSWeeks_tot, xSat, ySat, zSat, clkSat); + + dx = xRec - xSat; + dy = yRec - ySat; + dz = zRec - zSat; + + rho = sqrt(dx*dx + dy*dy + dz*dz); + + cout << " scrd " << setw(4) << GPSWeek_tot + << " " << setw(15) << setprecision(8) << GPSWeeks_tot + << " " << setw(13) << setprecision(3) << xSat + << " " << setw(13) << setprecision(3) << ySat + << " " << setw(13) << setprecision(3) << zSat + << " rcv0 " << setw(12) << setprecision(3) << stax + << " " << setw(12) << setprecision(3) << stay + << " " << setw(12) << setprecision(3) << staz + << " rcv " << setw(12) << setprecision(3) << xRec + << " " << setw(12) << setprecision(3) << yRec + << " " << setw(12) << setprecision(3) << zRec + << " dPhi " << scientific << setw(13) << setprecision(10) << dPhi << fixed + << " rho " << setw(13) << setprecision(3) << rho + << endl; + + + ////cout.setf(ios::fixed); + //// + ////cout << "niter " << setw(3) << ++niter + //// << " " << setw(14) << setprecision(3) << rhoLast + //// << " " << setw(14) << setprecision(3) << rho + //// << endl; + + } while ( fabs(rho - rhoLast) > 1e-4); + + clkSat *= c_light; // satellite clock correction in meters + + cout << "***** cmpRho *****\n"; + + return 0; +} Index: /trunk/BNC/RTCM/rtcm_utils.h =================================================================== --- /trunk/BNC/RTCM/rtcm_utils.h (revision 1024) +++ /trunk/BNC/RTCM/rtcm_utils.h (revision 1024) @@ -0,0 +1,23 @@ +#ifndef RTCM_UTILS_H +#define RTCM_UTILS_H + +class t_eph; + +const double c_light = 299792458.0; +const double FRQ_L1 = 1575420000.0; +const double FRQ_L2 = 1227600000.0; +const double LAMBDA_1 = c_light / FRQ_L1; +const double LAMBDA_2 = c_light / FRQ_L2; +const double ZEROVALUE = 1e-100; + +void resolveEpoch (double secsHour, + int refWeek, double refSecs, + int& epochWeek, double& epochSecs); + +int cmpRho(const t_eph* eph, + double stax, double stay, double staz, + int GPSWeek, double GPSWeeks, + double& rho, int& GPSWeek_tot, double& GPSWeeks_tot, + double& xSat, double& ySat, double& zSat, double& clkSat); + +#endif Index: /trunk/BNC/bnc.pro =================================================================== --- /trunk/BNC/bnc.pro (revision 1023) +++ /trunk/BNC/bnc.pro (revision 1024) @@ -2,6 +2,6 @@ # Switch to debug configuration # ----------------------------- -CONFIG += release -###CONFIG -= debug +###CONFIG += release +CONFIG += debug DEFINES += NO_RTCM3_MAIN @@ -27,6 +27,8 @@ bncconst.h bnchtml.h bnctableitem.h bnczerodecoder.h \ RTCM/GPSDecoder.h RTCM/RTCM2.h RTCM/RTCM2Decoder.h \ + RTCM/RTCM2_2021.h RTCM/rtcm_utils.h \ RTCM3/RTCM3Decoder.h RTCM3/rtcm3torinex.h \ RTCM3/RTCM3coDecoder.h RTCM3/clock_orbit_rtcm.h \ + RTCM3/ephemeris.h RTCM3/timeutils.h \ RTIGS/RTIGSDecoder.h RTIGS/rtigs_records.h \ RTIGS/cgps_transform.h RTIGS/rtstruct.h RTIGS/rtacp.h RTIGS/gpswro.h @@ -37,6 +39,8 @@ bnczerodecoder.cpp \ RTCM/RTCM2.cpp RTCM/RTCM2Decoder.cpp \ + RTCM/RTCM2_2021.cpp RTCM/rtcm_utils.cpp \ RTCM3/RTCM3Decoder.cpp RTCM3/rtcm3torinex.c \ RTCM3/RTCM3coDecoder.cpp RTCM3/clock_orbit_rtcm.c \ + RTCM3/ephemeris.cpp RTCM3/timeutils.cpp \ RTIGS/RTIGSDecoder.cpp RTIGS/cgps_transform.cpp Index: /trunk/BNC/bncapp.cpp =================================================================== --- /trunk/BNC/bncapp.cpp (revision 1023) +++ /trunk/BNC/bncapp.cpp (revision 1024) @@ -39,5 +39,4 @@ * -----------------------------------------------------------------------*/ -#include #include #include @@ -190,4 +189,7 @@ void bncApp::slotNewGPSEph(gpsephemeris* gpseph) { + gpsephemeris copy_gpseph = *gpseph; + emit newEphGPS(copy_gpseph); + QMutexLocker locker(&_mutex); @@ -206,4 +208,5 @@ delete gpseph; } + } @@ -601,5 +604,4 @@ //////////////////////////////////////////////////////////////////////////// void bncApp::slotQuit() { - cout << "bncApp::slotQuit" << endl; delete _caster; quit(); Index: /trunk/BNC/bncapp.h =================================================================== --- /trunk/BNC/bncapp.h (revision 1023) +++ /trunk/BNC/bncapp.h (revision 1024) @@ -49,6 +49,7 @@ void slotQuit(); - signals: - void newMessage(QByteArray msg); + signals: + void newEphGPS(gpsephemeris gpseph); + void newMessage(QByteArray msg); private slots: Index: /trunk/BNC/bncgetthread.cpp =================================================================== --- /trunk/BNC/bncgetthread.cpp (revision 1023) +++ /trunk/BNC/bncgetthread.cpp (revision 1024) @@ -140,4 +140,5 @@ _rnx = new bncRinex(_staID, mountPoint, format, latitude, longitude, nmea); } + _rnx_set_position = false; msleep(100); //sleep 0.1 sec @@ -156,5 +157,5 @@ } delete _decoder; - delete _rnx; + delete _rnx; } @@ -365,5 +366,5 @@ if (_format.indexOf("RTCM_2") != -1) { emit(newMessage("Get Data: " + _staID + " in RTCM 2.x format")); - _decoder = new RTCM2Decoder(); + _decoder = new RTCM2Decoder(_staID.data()); } else if (_format.indexOf("RTCM_3") != -1) { @@ -557,5 +558,5 @@ delete [] data; - + QListIterator it(_decoder->_obsList); while (it.hasNext()) { @@ -645,12 +646,30 @@ // ------------ if (_rnx) { - long iSec = long(floor(obs->_o.GPSWeeks+0.5)); - long newTime = obs->_o.GPSWeek * 7*24*3600 + iSec; - if (_samplingRate == 0 || iSec % _samplingRate == 0) { - _rnx->deepCopy(obs); - } - _rnx->dumpEpoch(newTime); - } - + bool dump = true; + + RTCM2Decoder* decoder2 = dynamic_cast(_decoder); + if ( decoder2 && !_rnx_set_position ) { + double stax, stay, staz; + if ( decoder2->getStaCrd(stax, stay, staz) == success ) { + _rnx->setApproxPos(stax, stay, staz); + _rnx_set_position = true; + } + else { + dump = false; + } + } + + if ( dump ) { + long iSec = long(floor(obs->_o.GPSWeeks+0.5)); + long newTime = obs->_o.GPSWeek * 7*24*3600 + iSec; + if (_samplingRate == 0 || iSec % _samplingRate == 0) { + _rnx->deepCopy(obs); + } + _rnx->dumpEpoch(newTime); + } + } + + // Emit new observation signal + // --------------------------- bool firstObs = (obs == _decoder->_obsList.first()); obs->_status = t_obs::posted; @@ -736,2 +755,15 @@ } } + +// +////////////////////////////////////////////////////////////////////////////// +void bncGetThread::slotNewEphGPS(gpsephemeris gpseph) { + RTCM2Decoder* decoder = dynamic_cast(_decoder); + + if ( decoder ) { + QMutexLocker locker(&_mutex); + + decoder->storeEph(gpseph); + } +} + Index: /trunk/BNC/bncgetthread.h =================================================================== --- /trunk/BNC/bncgetthread.h (revision 1023) +++ /trunk/BNC/bncgetthread.h (revision 1024) @@ -26,4 +26,6 @@ #define BNCGETTHREAD_H +#include + #include #include @@ -31,4 +33,5 @@ #include "RTCM/GPSDecoder.h" +#include "RTCM3/rtcm3torinex.h" #include "bncconst.h" @@ -56,4 +59,7 @@ void error(QByteArray staID); void newMessage(QByteArray msg); + + public slots: + void slotNewEphGPS(gpsephemeris gpseph); protected: @@ -95,4 +101,5 @@ int _samplingRate; bncRinex* _rnx; + bool _rnx_set_position; QDateTime _decodeFailure; QDateTime _decodeStart; Index: /trunk/BNC/bncrinex.cpp =================================================================== --- /trunk/BNC/bncrinex.cpp (revision 1023) +++ /trunk/BNC/bncrinex.cpp (revision 1024) @@ -93,4 +93,6 @@ _rinexVers = 2; } + + _approxPos[0] = _approxPos[1] = _approxPos[2] = 0.0; } @@ -447,5 +449,4 @@ // ------------------ else { - double approxPos[3]; approxPos[0] = approxPos[1] = approxPos[2] = 0.0; double antennaNEU[3]; antennaNEU[0] = antennaNEU[1] = antennaNEU[2] = 0.0; @@ -477,7 +478,7 @@ << setw(20) << " " << "ANT # / TYPE" << endl; _out.unsetf(ios::left); - _out << setw(14) << setprecision(4) << approxPos[0] - << setw(14) << setprecision(4) << approxPos[1] - << setw(14) << setprecision(4) << approxPos[2] + _out << setw(14) << setprecision(4) << _approxPos[0] + << setw(14) << setprecision(4) << _approxPos[1] + << setw(14) << setprecision(4) << _approxPos[2] << " " << "APPROX POSITION XYZ" << endl; _out << setw(14) << setprecision(4) << antennaNEU[0] @@ -565,60 +566,18 @@ double sec = double(datTim.time().second()) + fmod(fObs->_o.GPSWeeks,1.0); - // RINEX Version 3 - // --------------- + // Epoch header line: RINEX Version 3 + // ---------------------------------- if (_rinexVers == 3) { - char sbasflag = 'S'; _out << datTim.toString("> yyyy MM dd hh mm ").toAscii().data() << setw(10) << setprecision(7) << sec << " " << 0 << setw(3) << dumpList.size() << endl; - - QListIterator it(dumpList); - while (it.hasNext()) { - p_obs obs = it.next(); -// Changed data output, C1P, C2C|X, L2C|X, S2C|X added. Changed Output for SBAS Perlt - if (sbasflag != obs->_o.satSys) { - _out << obs->_o.satSys - << setw(2) << setfill('0') << obs->_o.satNum << setfill(' ') - << setw(14) << setprecision(3) << obs->_o.C1 << " " - << setw(14) << setprecision(3) << obs->_o.P1 << " " - << setw(14) << setprecision(3) << obs->_o.L1 << " " - << setw(1) << obs->_o.SNR1 - << setw(14) << setprecision(3) << obs->_o.S1 << " " - << setw(14) << setprecision(3) << obs->_o.C2 << " " - << setw(14) << setprecision(3) << obs->_o.P2 << " " ; - if ((obs->_o.C2 != 0.0) && (obs->_o.P2 == 0.0)) { - _out << setw(14) << setprecision(3) << obs->_o.L2 << " " - << setw(1) << obs->_o.SNR2 - << setw(14) << setprecision(3) << obs->_o.S2 << " " - << " 0.000 0.000 "; - } - else { - _out << " 0.000 0.000 " - << setw(14) << setprecision(3) << obs->_o.L2 << " " - << setw(1) << obs->_o.SNR2 - << setw(14) << setprecision(3) << obs->_o.S2; - } - _out << endl; - } - else { - _out << obs->_o.satSys - << setw(2) << setfill('0') << obs->_o.satNum << setfill(' ') - << setw(14) << setprecision(3) << obs->_o.C1 << " " - << setw(14) << setprecision(3) << obs->_o.P1 << " " - << setw(14) << setprecision(3) << obs->_o.L1 << " " - << setw(1) << obs->_o.SNR1 - << setw(14) << setprecision(3) << obs->_o.S1 << endl; - } - delete obs; - } - } - - // RINEX Version 2 - // --------------- + } + // Epoch header line: RINEX Version 2 + // ---------------------------------- else { _out << datTim.toString(" yy MM dd hh mm ").toAscii().data() << setw(10) << setprecision(7) << sec << " " << 0 << setw(3) << dumpList.size(); - + QListIterator it(dumpList); int iSat = 0; while (it.hasNext()) { @@ -632,9 +591,75 @@ } _out << endl; - - it.toFront(); - while (it.hasNext()) { - p_obs obs = it.next(); - + } + + QListIterator it(dumpList); + while (it.hasNext()) { + p_obs obs = it.next(); + + // Cycle slips detection + // --------------------- + int prn = 0; + switch (obs->_o.satSys) { + case 'G': prn = obs->_o.satNum; break; + case 'R': prn = obs->_o.satNum + 200; break; + default: prn = obs->_o.satNum; + } + + int lli1 = 0; + int lli2 = 0; + if ( _slip_cnt_L1.find(prn) != _slip_cnt_L1.end() && + _slip_cnt_L1.find(prn).value() != obs->_o.slip_cnt_L1 ) { + lli1 = 1; + } + if ( _slip_cnt_L2.find(prn) != _slip_cnt_L2.end() && + _slip_cnt_L2.find(prn).value() != obs->_o.slip_cnt_L2 ) { + lli2 = 1; + } + _slip_cnt_L1[prn]= obs->_o.slip_cnt_L1; + _slip_cnt_L2[prn]= obs->_o.slip_cnt_L2; + + // RINEX Version 3 + // --------------- + if (_rinexVers == 3) { + char sbasflag = 'S'; + // Changed data output, C1P, C2C|X, L2C|X, S2C|X added. Changed Output for SBAS Perlt + if (sbasflag != obs->_o.satSys) { + _out << obs->_o.satSys + << setw(2) << setfill('0') << obs->_o.satNum << setfill(' ') + << setw(14) << setprecision(3) << obs->_o.C1 << " " + << setw(14) << setprecision(3) << obs->_o.P1 << " " + << setw(14) << setprecision(3) << obs->_o.L1 << lli1 + << setw(1) << obs->_o.SNR1 + << setw(14) << setprecision(3) << obs->_o.S1 << " " + << setw(14) << setprecision(3) << obs->_o.C2 << " " + << setw(14) << setprecision(3) << obs->_o.P2 << " " ; + if ((obs->_o.C2 != 0.0) && (obs->_o.P2 == 0.0)) { + _out << setw(14) << setprecision(3) << obs->_o.L2 << lli2 + << setw(1) << obs->_o.SNR2 + << setw(14) << setprecision(3) << obs->_o.S2 << " " + << " 0.000 0.000 "; + } + else { + _out << " 0.000 0.000 " + << setw(14) << setprecision(3) << obs->_o.L2 << " " + << setw(1) << obs->_o.SNR2 + << setw(14) << setprecision(3) << obs->_o.S2; + } + _out << endl; + } + else { + _out << obs->_o.satSys + << setw(2) << setfill('0') << obs->_o.satNum << setfill(' ') + << setw(14) << setprecision(3) << obs->_o.C1 << " " + << setw(14) << setprecision(3) << obs->_o.P1 << " " + << setw(14) << setprecision(3) << obs->_o.L1 << lli1 + << setw(1) << obs->_o.SNR1 + << setw(14) << setprecision(3) << obs->_o.S1 << endl; + } + } + + // RINEX Version 2 + // --------------- + else { char lli = ' '; char snr = ' '; @@ -643,14 +668,14 @@ _out << setw(14) << setprecision(3) << obs->_o.P1 << lli << snr; _out << setw(14) << setprecision(3) << obs->_o.P2 << lli << snr; - _out << setw(14) << setprecision(3) << obs->_o.L1 << lli + _out << setw(14) << setprecision(3) << obs->_o.L1 << lli1 << setw(1) << obs->_o.SNR1 << endl; - _out << setw(14) << setprecision(3) << obs->_o.L2 << lli + _out << setw(14) << setprecision(3) << obs->_o.L2 << lli2 << setw(1) << obs->_o.SNR2; _out << setw(14) << setprecision(3) << obs->_o.S1 ; _out << setw(16) << setprecision(3) << obs->_o.S2 ; _out << endl; - - delete obs; - } + } + + delete obs; } Index: /trunk/BNC/bncrinex.h =================================================================== --- /trunk/BNC/bncrinex.h (revision 1023) +++ /trunk/BNC/bncrinex.h (revision 1024) @@ -49,4 +49,10 @@ QDateTime* nextEpoch = 0); + void setApproxPos(double stax, double stay, double staz) { + _approxPos[0] = stax; + _approxPos[1] = stay; + _approxPos[2] = staz; + } + protected: virtual void run() {}; @@ -81,4 +87,8 @@ bool _reloadTable; bool _reloadDone; + double _approxPos[3]; + + QMap _slip_cnt_L1; + QMap _slip_cnt_L2; }; Index: /trunk/BNC/bncwindow.cpp =================================================================== --- /trunk/BNC/bncwindow.cpp (revision 1023) +++ /trunk/BNC/bncwindow.cpp (revision 1024) @@ -688,4 +688,7 @@ SLOT(slotNewBytes(QByteArray, double))); + connect(((bncApp*)qApp), SIGNAL(newEphGPS(gpsephemeris)), + getThread, SLOT(slotNewEphGPS(gpsephemeris))); + _caster->addGetThread(getThread);