Index: trunk/BNC/src/RTCM/GPSDecoder.cpp =================================================================== --- trunk/BNC/src/RTCM/GPSDecoder.cpp (revision 4279) +++ trunk/BNC/src/RTCM/GPSDecoder.cpp (revision 4279) @@ -0,0 +1,90 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: GPSDecoder + * + * Purpose: Decoder Base Class + * + * Author: L. Mervart + * + * Created: 16-Dec-2011 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include + +#include "GPSDecoder.h" +#include "bncsettings.h" + +using namespace std; + +// Constructor +////////////////////////////////////////////////////////////////////////////// +GPSDecoder::GPSDecoder() { + _rnx = 0; +} + +// Initialize RINEX Writer +////////////////////////////////////////////////////////////////////////////// +void GPSDecoder::initRinex(const QByteArray& staID, const QUrl& mountPoint, + const QByteArray& latitude, + const QByteArray& longitude, const QByteArray& nmea, + const QByteArray& ntripVersion) { + if (_rnx) { + return; + } + bncSettings settings; + if ( !settings.value("rnxPath").toString().isEmpty() ) { + _rnx = new bncRinex(staID, mountPoint, latitude, longitude, + nmea, ntripVersion); + } +} + +// Write RINEX Epoch +////////////////////////////////////////////////////////////////////////////// +void GPSDecoder::dumpRinexEpoch(const t_obs& obs, const QByteArray& format) { + if (_rnx) { + long iSec = long(floor(obs.GPSWeeks+0.5)); + long obsTime = obs.GPSWeek * 7*24*3600 + iSec; + if (_rnx->samplingRate() == 0 || iSec % _rnx->samplingRate() == 0) { + _rnx->deepCopy(obs); + } + _rnx->dumpEpoch(format, obsTime); + } +} + +// Set RINEX Reconnect Flag +////////////////////////////////////////////////////////////////////////////// +void GPSDecoder::setRinexReconnectFlag(bool flag) { + if (_rnx) { + _rnx->setReconnectFlag(flag); + } +} Index: trunk/BNC/src/RTCM/GPSDecoder.h =================================================================== --- trunk/BNC/src/RTCM/GPSDecoder.h (revision 4279) +++ trunk/BNC/src/RTCM/GPSDecoder.h (revision 4279) @@ -0,0 +1,168 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +#ifndef GPSDECODER_H +#define GPSDECODER_H + +#include +#include +#include +#include +#include + +#include "bncconst.h" +#include "bncrinex.h" + +class t_obs { + public: + t_obs() { + StatID[0] = '\x0'; + satSys = 'G'; + satNum = 0; + slotNum = 0; + GPSWeek = 0; + GPSWeeks = 0.0; + C1 = 0.0; + P1 = 0.0; + L1C = 0.0; + D1C = 0.0; + S1C = 0.0; + L1P = 0.0; + D1P = 0.0; + S1P = 0.0; + C2 = 0.0; + P2 = 0.0; + L2C = 0.0; + D2C = 0.0; + S2C = 0.0; + L2P = 0.0; + D2P = 0.0; + S2P = 0.0; + C5 = 0.0; + L5 = 0.0; + D5 = 0.0; + S5 = 0.0; + slip_cnt_L1 = -1; + slip_cnt_L2 = -1; + slip_cnt_L5 = -1; + } + + ~t_obs() {} + + double p1() const {return (P1 != 0.0 ? P1 : C1);} + double p2() const {return (P2 != 0.0 ? P2 : C2);} + double l1() const {return (L1P != 0.0 ? L1P : L1C);} + double l2() const {return (L2P != 0.0 ? L2P : L2C);} + double s1() const {return (L1P != 0.0 ? S1P : S1C);} + double s2() const {return (L2P != 0.0 ? S2P : S2C);} + + char StatID[20+1]; // Station ID + char satSys; // Satellite System ('G' or 'R') + int satNum; // Satellite Number (PRN for GPS NAVSTAR) + int slotNum; // Slot Number (for Glonass) + int GPSWeek; // Week of GPS-Time + double GPSWeeks; // Second of Week (GPS-Time) + + double C1; // CA-code pseudorange (meters) + double L1C; // L1 carrier phase (cycles) + double D1C; // Doppler L1 + double S1C; // raw L1 signal strength + bool has1C() const {return C1 != 0.0 || L1C != 0.0 || D1C != 0.0 || S1C != 0.0;} + + double P1; // P1-code pseudorange (meters) + double L1P; // L1 carrier phase (cycles) + double D1P; // Doppler L1 + double S1P; // raw L1 signal strength + bool has1P() const {return P1 != 0.0 || L1P != 0.0 || D1P != 0.0 || S1P != 0.0;} + + double C2; // CA-code pseudorange (meters) + double L2C; // L2 carrier phase (cycles) + double D2C; // Doppler L2 + double S2C; // raw L2 signal strength + bool has2C() const {return C2 != 0.0 || L2C != 0.0 || D2C != 0.0 || S2C != 0.0;} + + double P2; // P2-code pseudorange (meters) + double L2P; // L2 carrier phase (cycles) + double D2P; // Doppler L2 + double S2P; // raw L2 signal strength + bool has2P() const {return P2 != 0.0 || L2P != 0.0 || D2P != 0.0 || S2P != 0.0;} + + double C5; // Pseudorange (meters) + double L5; // L5 carrier phase (cycles) + double D5; // Doppler L5 + double S5; // raw L5 signal strength + bool has5C() const {return C5 != 0.0 || L5 != 0.0 || D5 != 0.0 || S5 != 0.0;} + + int slip_cnt_L1; // L1 cumulative loss of continuity indicator (negative value = undefined) + int slip_cnt_L2; // L2 cumulative loss of continuity indicator (negative value = undefined) + int slip_cnt_L5; // L5 cumulative loss of continuity indicator (negative value = undefined) +}; + +class GPSDecoder { + public: + GPSDecoder(); + + virtual ~GPSDecoder() {delete _rnx;} + + virtual t_irc Decode(char* buffer, int bufLen, + std::vector& errmsg) = 0; + + + virtual int corrGPSEpochTime() const {return -1;} + + void initRinex(const QByteArray& staID, const QUrl& mountPoint, + const QByteArray& latitude, const QByteArray& longitude, + const QByteArray& nmea, const QByteArray& ntripVersion); + + void dumpRinexEpoch(const t_obs& obs, const QByteArray& format); + + void setRinexReconnectFlag(bool flag); + + struct t_antInfo { + enum t_type { ARP, APC }; + + t_antInfo() { + xx = yy = zz = height = 0.0; + type = ARP; + height_f = false; + message = 0; + }; + + double xx; + double yy; + double zz; + t_type type; + double height; + bool height_f; + int message; + }; + + QList _obsList; + QList _typeList; // RTCM message types + QStringList _antType; // RTCM antenna descriptor + QList _antList; // RTCM antenna XYZ + bncRinex* _rnx; // RINEX writer +}; + +#endif Index: trunk/BNC/src/RTCM/RTCM2.cpp =================================================================== --- trunk/BNC/src/RTCM/RTCM2.cpp (revision 4279) +++ trunk/BNC/src/RTCM/RTCM2.cpp (revision 4279) @@ -0,0 +1,1360 @@ +//------------------------------------------------------------------------------ +// +// RTCM2.cpp +// +// Purpose: +// +// Module for extraction of RTCM2 messages +// +// References: +// +// RTCM 10402.3 Recommended Standards for Differential GNSS (Global +// Navigation Satellite Systems) Service; RTCM Paper 136-2001/SC104-STD, +// Version 2.3, 20 Aug. 2001; Radio Technical Commission For Maritime +// Services, Alexandria, Virgina (2001). +// ICD-GPS-200; Navstar GPS Space Segment / Navigation User Interfaces; +// Revison C; 25 Sept. 1997; Arinc Research Corp., El Segundo (1997). +// Jensen M.; RTCM2ASC Documentation; +// URL http://kom.aau.dk/~borre/masters/receiver/rtcm2asc.htm; +// last accessed 17 Sep. 2006 +// Sager J.; Decoder for RTCM SC-104 data from a DGPS beacon receiver; +// URL http://www.wsrcc.com/wolfgang/ftp/rtcm-0.3.tar.gz; +// last accessed 17 Sep. 2006 +// +// Notes: +// +// - The host computer is assumed to use little endian (Intel) byte order +// +// Last modified: +// +// 2006/09/17 OMO Created +// 2006/09/19 OMO Fixed getHeader() methods +// 2006/09/21 OMO Reduced phase ambiguity to 2^23 cycles +// 2006/10/05 OMO Specified const'ness of various member functions +// 2006/10/13 LMV Fixed resolvedPhase to handle missing C1 range +// 2006/10/14 LMV Fixed loop cunter in ThirtyBitWord +// 2006/10/14 LMV Exception handling +// 2006/10/17 OMO Removed obsolete check of multiple message indicator +// 2006/10/17 OMO Fixed parity handling +// 2006/10/18 OMO Improved screening of bad data in RTCM2_Obs::extract +// 2006/11/25 OMO Revised check for presence of GLONASS data +// 2007/05/25 GW Round time tag to 100 ms +// 2007/12/11 AHA Changed handling of C/A- and P-Code on L1 +// 2007/12/13 AHA Changed epoch comparison in packet extraction +// 2008/03/01 OMO Compilation flag for epoch rounding +// 2008/03/04 AHA Fixed problems with PRN 32 +// 2008/03/05 AHA Implemeted fix for Trimble 4000SSI receivers +// 2008/03/07 AHA Major revision of input buffer handling +// 2008/03/07 AHA Removed unnecessary failure flag +// 2008/03/10 AHA Corrected extraction of antenna serial number +// 2008/03/10 AHA Corrected buffer length check in getPacket() +// 2008/03/11 AHA isGPS-flag in RTCM2_Obs is now set to false on clear() +// 2008/03/14 AHA Added checks for data consistency in extraction routines +// 2008/09/01 AHA Harmonization with newest BNC version +// +// (c) DLR/GSOC +// +//------------------------------------------------------------------------------ + +#include +#include +#include +#include +#include +#include +#include + +#include "RTCM2.h" + +// Activate (1) or deactivate (0) debug output for tracing parity errors and +// undersized packets in get(Unsigned)Bits + +#define DEBUG 0 + +// Activate (1) or deactivate (0) rounding of measurement epochs to 100ms +// +// Note: A need to round the measurement epoch to integer tenths of a second was +// noted by BKG in the processing of RTCM2 data from various receivers in NTRIP +// real-time networks. It is unclear at present, whether this is due to an +// improper implementation of the RTCM2 standard in the respective receivers +// or an unclear formulation of the standard. + +#define ROUND_EPOCH 1 + +// Fix for data streams originating from TRIMBLE_4000SSI receivers. +// GPS PRN32 is erroneously flagged as GLONASS satellite in the C/A +// pseudorange messages. We therefore use a majority voting to +// determine the true constellation for this message. +// This fix is only required for Trimble4000SSI receivers but can also +// be used with all other known receivers. + +#define FIX_TRIMBLE_4000SSI 1 + +using namespace std; + + +// GPS constants + +const double c_light = 299792458.0; // Speed of light [m/s]; IAU 1976 +const double f_L1 = 1575.42e6; // L1 frequency [Hz] (10.23MHz*154) +const double f_L2 = 1227.60e6; // L2 frequency [Hz] (10.23MHz*120) + +const double lambda_L1 = c_light/f_L1; // L1 wavelength [m] (0.1903m) +const double lambda_L2 = c_light/f_L2; // L2 wavelength [m] + +// +// Bits for message availability checks +// + +const int bit_L1rngGPS = 0; +const int bit_L2rngGPS = 1; +const int bit_L1cphGPS = 2; +const int bit_L2cphGPS = 3; +const int bit_L1rngGLO = 4; +const int bit_L2rngGLO = 5; +const int bit_L1cphGLO = 6; +const int bit_L2cphGLO = 7; + + +// +// namespace rtcm2 +// + +namespace rtcm2 { + +//------------------------------------------------------------------------------ +// +// class ThirtyBitWord (implementation) +// +// Purpose: +// +// Handling of RTCM2 30bit words +// +//------------------------------------------------------------------------------ + +// Constructor + +ThirtyBitWord::ThirtyBitWord() : W(0) { +}; + +// Clear entire 30-bit word and 2-bit parity from previous word + +void ThirtyBitWord::clear() { + W = 0; +}; + +// Parity check + +bool ThirtyBitWord::validParity() const { + + // Parity stuff + + static const unsigned int PARITY_25 = 0xBB1F3480; + static const unsigned int PARITY_26 = 0x5D8F9A40; + static const unsigned int PARITY_27 = 0xAEC7CD00; + static const unsigned int PARITY_28 = 0x5763E680; + static const unsigned int PARITY_29 = 0x6BB1F340; + static const unsigned int PARITY_30 = 0x8B7A89C0; + + // Look-up table for parity of eight bit bytes + // (parity=0 if the number of 0s and 1s is equal, else parity=1) + static unsigned char byteParity[] = { + 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1, + 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0, + 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0, + 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1, + 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0, + 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1, + 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1, + 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0 + }; + + // Local variables + + unsigned int t, w, p; + + // The sign of the data is determined by the D30* parity bit + // of the previous data word. If D30* is set, invert the data + // bits D01..D24 to obtain the d01..d24 (but leave all other + // bits untouched). + + w = W; + if ( w & 0x40000000 ) w ^= 0x3FFFFFC0; + + // Compute the parity of the sign corrected data bits d01..d24 + // as described in the ICD-GPS-200 + + t = w & PARITY_25; + p = ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^ + byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] ); + + t = w & PARITY_26; + p = (p<<1) | + ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^ + byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] ); + + t = w & PARITY_27; + p = (p<<1) | + ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^ + byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] ); + + t = w & PARITY_28; + p = (p<<1) | + ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^ + byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] ); + + t = w & PARITY_29; + p = (p<<1) | + ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^ + byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] ); + + t = w & PARITY_30; + p = (p<<1) | + ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^ + byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] ); + + return ( (W & 0x3f) == p); + +}; + + +// Check preamble + +bool ThirtyBitWord::isHeader() const { + + const unsigned char Preamble = 0x66; + + unsigned char b = (value()>>22) & 0xFF; + + return ( b==Preamble ); + +}; + + +// Return entire 32-bit (current word and previous parity) + +unsigned int ThirtyBitWord::all() const { + return W; +}; + + +// Return sign-corrected 30-bit (or zero if parity mismatch) + +unsigned int ThirtyBitWord::value() const { + + unsigned int w = W; + + if (validParity()) { + // Return data and current parity bits. Invert data bits if D30* + // is set and discard old parity bits. + if ( w & 0x40000000 ) w ^= 0x3FFFFFC0; + return (w & 0x3FFFFFFF); + } + else { + // Error; invalid parity + return 0; + }; + +}; + + +// Append a byte with six data bits + +void ThirtyBitWord::append(unsigned char b) { + + // Look up table for swap (left-right) of 6 data bits + static const unsigned char + swap[] = { + 0,32,16,48, 8,40,24,56, 4,36,20,52,12,44,28,60, + 2,34,18,50,10,42,26,58, 6,38,22,54,14,46,30,62, + 1,33,17,49, 9,41,25,57, 5,37,21,53,13,45,29,61, + 3,35,19,51,11,43,27,59, 7,39,23,55,15,47,31,63 + }; + + // Bits 7 and 6 (of 0..7) must be "01" for valid data bytes + if ( (b & 0x40) != 0x40 ) { + // We simply skip the invalid input byte and leave the word unchanged +#if (DEBUG>0) + cerr << "Error in append()" << bitset<32>(all()) << endl; +#endif + return; + }; + + // Swap bits 0..5 to restore proper bit order for 30bit words + b = swap[ b & 0x3f]; + + // Fill word + W = ( (W <<6) | (b & 0x3f) ) ; + +}; + + +// Get next 30bit word from string + +void ThirtyBitWord::get(const string& buf) { + + // Check if string is long enough + + if (buf.size()<5) { + // Ignore; users should avoid this case prior to calling get() + +#if ( DEBUG > 0 ) + cerr << "Error in get(): packet too short (" << buf.size() <<")" << endl; +#endif + + return; + }; + + // Process 5 bytes + + for (int i=0; i<5; i++) append(buf[i]); + +#if (DEBUG>0) + if (!validParity()) { + cerr << "Parity error in get()" + << bitset<32>(all()) << endl; + }; +#endif + +}; + +// Get next 30bit word from file + +void ThirtyBitWord::get(istream& inp) { + + unsigned char b; + + for (int i=0; i<5; i++) { + inp >> b; + if (inp.fail()) { clear(); return; }; + append(b); + }; + +#if (DEBUG>0) + if (!validParity()) { + cerr << "Parity error in get()" + << bitset<32>(all()) << endl; + }; +#endif + +}; + +// Get next header word from string + +void ThirtyBitWord::getHeader(string& buf) { + + const unsigned int wordLen = 5; // Number of bytes representing a 30-bit word + const unsigned int spare = 1; // Number of spare words for resync of parity + // (same value as inRTCM2packet::getPacket()) + unsigned int i; + + i=0; + // append spare word (to get correct parity) and first consecutive word + while (i<(spare+1)*wordLen) { + // Process byte + append(buf[i]); + // Increment count + i++; + }; + + // start searching for preamble in first word after spare word + while (!isHeader() && i=(1+spare)*wordLen) buf.erase(0,i-(1+spare)*wordLen); + +#if (DEBUG>0) + if (!validParity()) { + cerr << "Parity error in getHeader()" + << bitset<32>(all()) << endl; + }; +#endif + +}; + +// Get next header word from file + +void ThirtyBitWord::getHeader(istream& inp) { + + unsigned char b; + unsigned int i; + + i=0; + while ( !isHeader() || i<5 ) { + inp >> b; + if (inp.fail()) { clear(); return; }; + append(b); i++; + }; + +#if (DEBUG>0) + if (!validParity()) { + cerr << "Parity error in getHeader()" + << bitset<32>(all()) << endl; + }; +#endif + +}; + + +//------------------------------------------------------------------------------ +// +// RTCM2packet (class implementation) +// +// Purpose: +// +// A class for handling RTCM2 data packets +// +//------------------------------------------------------------------------------ + +// Constructor + +RTCM2packet::RTCM2packet() { + clear(); +}; + +// Initialization + +void RTCM2packet::clear() { + + W.clear(); + + H1=0; + H2=0; + + DW.resize(0,0); + +}; + +// Complete packet, valid parity + +bool RTCM2packet::valid() const { + + // The methods for creating a packet (get,">>") ensure + // that a packet has a consistent number of data words + // and a valid parity in all header and data words. + // Therefore a packet is either empty or valid. + + return (H1!=0); + +}; + + +// +// Gets the next packet from the buffer +// + +void RTCM2packet::getPacket(std::string& buf) { + + const int wordLen = 5; // Number of bytes representing a 30-bit word + const int spare = 1; // Number of spare words for resync of parity + // (same value as used in ThirtyBitWord::getHeader) + unsigned int n; + + // Does the package content at least spare bytes and first header byte? + if (buf.size()<(spare+1)*wordLen) { + clear(); + return; + }; + + // Try to read a full packet. Processed bytes are removed from the input + // buffer except for the latest spare*wordLen bytes to restore the parity + // bytes upon subseqeunt calls of getPacket(). + + // Locate and read the first header word + W.getHeader(buf); + if (!W.isHeader()) { + // No header found; try again next time. buf retains only the spare + // words. The packet contents is cleared to indicate an unsuccessful + // termination of getPacket(). + clear(); + +#if ( DEBUG > 0 ) + cerr << "Error in getPacket(): W.isHeader() = false for H1" << endl; +#endif + + return; + }; + H1 = W.value(); + + // Do we have enough bytes to read the next word? If not, the packet + // contents is cleared to indicate an unsuccessful termination. The + // previously read spare and header bytes are retained in the buffer + // for use in the next call of getPacket(). + if (buf.size()<(spare+2)*wordLen) { + clear(); + +#if ( DEBUG > 0 ) + cerr << "Error in getPacket(): buffer too short for complete H2" << endl; +#endif + + return; + }; + + // Read the second header word + W.get(buf.substr((spare+1)*wordLen,buf.size()-(spare+1)*wordLen)); + H2 = W.value(); + if (!W.validParity()) { + // Invalid H2 word; delete first buffer byte and try to resynch next time. + // The packet contents is cleared to indicate an unsuccessful termination. + clear(); + buf.erase(0,1); + +#if ( DEBUG > 0 ) + cerr << "Error in getPacket(): W.validParity() = false for H2" << endl; +#endif + + return; + }; + + n = nDataWords(); + + // Do we have enough bytes to read the next word? If not, the packet + // contents is cleared to indicate an unsuccessful termination. The + // previously read spare and header bytes are retained in the buffer + // for use in the next call of getPacket(). + if (buf.size()<(spare+2+n)*wordLen) { + clear(); + +#if ( DEBUG > 0 ) + cerr << "Error in getPacket(): buffer too short for complete " << n + << " DWs" << endl; +#endif + + return; + }; + + DW.resize(n); + for (unsigned int i=0; i 0 ) + cerr << "Error in getPacket(): W.validParity() = false for DW" + << i << endl; +#endif + + return; + }; + }; + + // Successful packet extraction; delete total number of message bytes + // from buffer. + // Note: a total of "spare" words remain in the buffer to enable a + // parity resynchronization when searching the next header. + + buf.erase(0,(n+2)*wordLen); + + return; + +}; + + +// +// Gets the next packet from the input stream +// + +void RTCM2packet::getPacket(std::istream& inp) { + + int n; + + W.getHeader(inp); + H1 = W.value(); + if (inp.fail() || !W.isHeader()) { clear(); return; } + + W.get(inp); + H2 = W.value(); + if (inp.fail() || !W.validParity()) { clear(); return; } + + n = nDataWords(); + DW.resize(n); + for (int i=0; i> (istream& is, RTCM2packet& p) { + + p.getPacket(is); + + return is; + +}; + +// Access methods + +unsigned int RTCM2packet::header1() const { + return H1; +}; + +unsigned int RTCM2packet::header2() const { + return H2; +}; + +unsigned int RTCM2packet::dataWord(int i) const { + if ( (unsigned int)i < DW.size() ) { + return DW[i]; + } + else { + return 0; + } +}; + +unsigned int RTCM2packet::msgType() const { + return ( H1>>16 & 0x003F ); +}; + +unsigned int RTCM2packet::stationID() const { + return ( H1>> 6 & 0x03FF ); +}; + +unsigned int RTCM2packet::modZCount() const { + return ( H2>>17 & 0x01FFF ); +}; + +unsigned int RTCM2packet::seqNumber() const { + return ( H2>>14 & 0x0007 ); +}; + +unsigned int RTCM2packet::nDataWords() const { + return ( H2>> 9 & 0x001F ); +}; + +unsigned int RTCM2packet::staHealth() const { + return ( H2>> 6 & 0x0003 ); +}; + + +// +// Get unsigned bit field +// +// Bits are numbered from left (msb) to right (lsb) starting at bit 0 +// + +unsigned int RTCM2packet::getUnsignedBits ( unsigned int start, + unsigned int n ) const { + + unsigned int iFirst = start/24; // Index of first data word + unsigned int iLast = (start+n-1)/24; // Index of last data word + unsigned int bitField = 0; + unsigned int tmp; + + // Checks + + if (n>32) { + throw("Error: can't handle >32 bits in RTCM2packet::getUnsignedBits"); + }; + + if ( 24*DW.size() < start+n-1 ) { +#if (DEBUG>0) + cerr << "Debug output RTCM2packet::getUnsignedBits" << endl + << " P.msgType: " << setw(5) << msgType() << endl + << " P.nDataWords: " << setw(5) << nDataWords() << endl + << " start: " << setw(5) << start << endl + << " n: " << setw(5) << n << endl + << " P.H1: " << setw(5) << bitset<32>(H1) << endl + << " P.H2: " << setw(5) << bitset<32>(H2) << endl + << endl + << flush; +#endif + throw("Error: Packet too short in RTCM2packet::getUnsignedBits"); + } + + // Handle initial data word + // Get all data bits. Strip parity and unwanted leading bits. + // Store result in 24 lsb bits of tmp. + + tmp = (DW[iFirst]>>6) & 0xFFFFFF; + tmp = ( ( tmp << start%24) & 0xFFFFFF ) >> start%24 ; + + // Handle central data word + + if ( iFirst>6) & 0xFFFFFF; + bitField = (bitField << 24) | tmp; + }; + tmp = (DW[iLast]>>6) & 0xFFFFFF; + }; + + // Handle last data word + + tmp = tmp >> (23-(start+n-1)%24); + bitField = (bitField << ((start+n-1)%24+1)) | tmp; + + // Done + + return bitField; + +}; + +// +// Get signed bit field +// +// Bits are numbered from left (msb) to right (lsb) starting at bit 0 +// + +int RTCM2packet::getBits ( unsigned int start, + unsigned int n ) const { + + + // Checks + + if (n>32) { + throw("Error: can't handle >32 bits in RTCM2packet::getBits"); + }; + + if ( 24*DW.size() < start+n-1 ) { +#if (DEBUG>0) + cerr << "Debug output RTCM2packet::getUnsignedBits" << endl + << " P.msgType: " << setw(5) << msgType() << endl + << " P.nDataWords: " << setw(5) << nDataWords() << endl + << " start: " << setw(5) << start << endl + << " n: " << setw(5) << n << endl + << " P.H1: " << setw(5) << bitset<32>(H1) << endl + << " P.H2: " << setw(5) << bitset<32>(H2) << endl + << endl + << flush; +#endif + throw("Error: Packet too short in RTCM2packet::getBits"); + } + + return ((int)(getUnsignedBits(start,n)<<(32-n))>>(32-n)); + +}; + + +//------------------------------------------------------------------------------ +// +// RTCM2_03 (class implementation) +// +// Purpose: +// +// A class for handling RTCM 2 GPS Reference Station Parameters messages +// +//------------------------------------------------------------------------------ + +// Constructor +RTCM2_03::RTCM2_03(){ + validMsg = false; + x = 0.0; + y = 0.0; + z=0.0; +}; + +void RTCM2_03::extract(const RTCM2packet& P) { + + // Check validity, packet type and number of data words + + validMsg = (P.valid()); + if (!validMsg) return; + + validMsg = (P.ID()==03); + if (!validMsg) return; + + validMsg = (P.nDataWords()==4); + if (!validMsg) return; + + // Antenna reference point coordinates + + x = P.getBits( 0,32)*0.01; // X [m] + y = P.getBits(32,32)*0.01; // Y [m] + z = P.getBits(64,32)*0.01; // Z [m] + +}; + +//------------------------------------------------------------------------------ +// +// RTCM2_23 (class implementation) +// +// Purpose: +// +// A class for handling RTCM 2 Antenna Type Definition messages +// +//------------------------------------------------------------------------------ + +void RTCM2_23::extract(const RTCM2packet& P) { + + unsigned int nad, nas; + + const unsigned int nF1 = 8; // bits in first field (R,AF,SF,NAD) + const unsigned int nF2 =16; // bits in second field (SETUP ID,R,NAS) + const unsigned int nBits=24; // data bits in 30bit word + + // Check validity, packet type and number of data words + + validMsg = (P.valid()); + if (!validMsg) return; + + validMsg = (P.ID()==23); + if (!validMsg) return; + + // Check number of data words (can nad be read in?) + + validMsg = (P.nDataWords()>=1); + if (!validMsg){ + cerr << "RTCM2_23::extract: P.nDataWords()>=1" << endl; + return; + } + + // Antenna descriptor + antType = ""; + nad = P.getUnsignedBits(3,5); + + // Check number of data words (can antenna description be read in?) + validMsg = ( P.nDataWords() >= + (unsigned int)ceil((nF1+nad*8)/(double)nBits) ); + + if (!validMsg) return; + + for (unsigned int i=0;i= + (unsigned int)ceil((nF1+nad*8+nF2)/(double)nBits) ); + if (!validMsg) return; + + nas = P.getUnsignedBits(19+8*nad,5); + + // Check number of data words (can antenna serial number be read in?) + + validMsg = ( P.nDataWords() >= + (unsigned int)ceil((nF1+nad*8+nF2+nas*8)/(double)nBits) ); + if (!validMsg) return; + + antSN = ""; + for (unsigned int i=0;i 0 ) + cerr << "Error in RTCM2_Obs::extract(): less than 3 DW (" + << P.nDataWords() << ") detected" << endl; +#endif + + return; + }; + + // Check if number of data words is 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; + }; + + // Clear previous data if block was already complete + + if (valid()) clear(); + + // Process carrier phase message + + if ( P.ID()==18 ) { + + // Number of satellites in current message + NSat = (P.nDataWords()-1)/2; + + // Current epoch (mod 3600 sec) + t = 0.6*P.modZCount() + + P.getUnsignedBits(4,20)*1.0e-6; + +#if (ROUND_EPOCH==1) + // SC-104 V2.3 4-42 Note 1 4. Assume measurements at hard edges + // of receiver clock with minimum divisions of 10ms + // and clock error less then recommended 1.1ms + // Hence, round time tag to 100 ms + t = floor(t*100.0+0.5)/100.0; +#endif + + // Frequency (exit if neither L1 nor L2) + isL1 = ( P.getUnsignedBits(0,1)==0 ); + isOth = ( P.getUnsignedBits(1,1)==1 ); + if (isOth) return; + + // Constellation (for first satellite in message) + isGPS = ( P.getUnsignedBits(26,1)==0 ); + GPSonly = GPSonly && isGPS; + + // Multiple Message Indicator (only checked for first satellite) + // pendingMsg = ( P.getUnsignedBits(24,1)==1 ); + + // Handle epoch: store epoch of first GPS message and + // check consistency of subsequent messages. GLONASS time tags + // are different and have to be ignored + if (isGPS) { + if ( nSat==0 ) { + secs = t; // Store epoch + } +// else if (t!=secs) { + else if (abs(t-secs)>1e-6) { + clear(); secs = t; // Clear all data, then store epoch + }; + }; + + // Discard GLONASS observations if no prior GPS observations + // are available + if (!isGPS && !anyGPS() ) return; + + // Set availability flags + + if ( isL1 && isGPS) availability.set(bit_L1cphGPS); + if (!isL1 && isGPS) availability.set(bit_L2cphGPS); + if ( isL1 && !isGPS) availability.set(bit_L1cphGLO); + if (!isL1 && !isGPS) availability.set(bit_L2cphGLO); + +#if ( DEBUG > 0 ) + cerr << "RTCM2_Obs::extract(): availability " + << bitset<8>(availability) << endl; +#endif + + + // Process all satellites + + for (int iSat=0;iSatNSat); +#else + // Constellation (for first satellite in message) + isGPS = ( P.getUnsignedBits(26,1)==0 ); +#endif + GPSonly = GPSonly && isGPS; + + // Multiple Message Indicator (only checked for first satellite) + // pendingMsg = ( P.getUnsignedBits(24,1)==1 ); + + // Handle epoch: store epoch of first GPS message and + // check consistency of subsequent messages. GLONASS time tags + // are different and have to be ignored + if (isGPS) { + if ( nSat==0 ) { + secs = t; // Store epoch + } +// else if (t!=secs) { + else if (abs(t-secs)>1e-6) { + clear(); secs = t; // Clear all data, then store epoch + }; + }; + + // Discard GLONASS observations if no prior GPS observations + // are available + if (!isGPS && !anyGPS() ) return; + + // Set availability flags + if ( isL1 && isGPS) availability.set(bit_L1rngGPS); + if (!isL1 && isGPS) availability.set(bit_L2rngGPS); + if ( isL1 && !isGPS) availability.set(bit_L1rngGLO); + if (!isL1 && !isGPS) availability.set(bit_L2rngGLO); + +#if ( DEBUG > 0 ) + cerr << "RTCM2_Obs::extract(): availability " + << bitset<8>(availability) << endl; +#endif + + // Process all satellites + + for (int iSat=0;iSatnSat-1) return 0.0; + + rng = rng_C1[i]; + if (rng==0.0) rng = rng_P1[i]; + if (rng==0.0) return 0.0; + + n = floor( (rng/lambda_L1-cph_L1[i]) / ambig + 0.5 ); + + return cph_L1[i] + n*ambig; + +}; + +double RTCM2_Obs::resolvedPhase_L2(int i) const { + +//const double ambig = pow(2.0,24); // as per RTCM2 spec + const double ambig = pow(2.0,23); // used by many receivers + + double rng; + double n; + + if (!valid() || i<0 || i>nSat-1) return 0.0; + + rng = rng_C1[i]; + if (rng==0.0) rng = rng_P1[i]; + if (rng==0.0) return 0.0; + + n = floor( (rng/lambda_L2-cph_L2[i]) / ambig + 0.5 ); + + return cph_L2[i] + n*ambig; + +}; + +// +// Resolution of epoch using reference date (GPS week and secs) +// + +void RTCM2_Obs::resolveEpoch (int refWeek, double refSecs, + int& epochWeek, double& epochSecs ) const { + + const double secsPerWeek = 604800.0; + + epochWeek = refWeek; + epochSecs = secs + 3600.0*(floor((refSecs-secs)/3600.0+0.5)); + + if (epochSecs<0 ) { epochWeek--; epochSecs+=secsPerWeek; }; + if (epochSecs>secsPerWeek) { epochWeek++; epochSecs-=secsPerWeek; }; + +}; + +}; // End of namespace rtcm2 Index: trunk/BNC/src/RTCM/RTCM2.h =================================================================== --- trunk/BNC/src/RTCM/RTCM2.h (revision 4279) +++ trunk/BNC/src/RTCM/RTCM2.h (revision 4279) @@ -0,0 +1,336 @@ +//------------------------------------------------------------------------------ +// +// RTCM2.h +// +// Purpose: +// +// Module for extraction of RTCM2 messages +// +// References: +// +// RTCM 10402.3 Recommended Standards for Differential GNSS (Global +// Navigation Satellite Systems) Service; RTCM Paper 136-2001/SC104-STD, +// Version 2.3, 20 Aug. 2001; Radio Technical Commission For Maritime +// Services, Alexandria, Virgina (2001). +// ICD-GPS-200; Navstar GPS Space Segment / Navigation User Interfaces; +// Revison C; 25 Sept. 1997; Arinc Research Corp., El Segundo (1997). +// Jensen M.; RTCM2ASC Documentation; +// URL http://kom.aau.dk/~borre/masters/receiver/rtcm2asc.htm; +// last accessed 17 Sep. 2006 +// Sager J.; Decoder for RTCM SC-104 data from a DGPS beacon receiver; +// URL http://www.wsrcc.com/wolfgang/ftp/rtcm-0.3.tar.gz; +// last accessed 17 Sep. 2006 +// +// Last modified: +// +// 2006/09/17 OMO Created +// 2006/10/05 OMO Specified const'ness of various member functions +// 2006/10/17 OMO Removed obsolete check of multiple message indicator +// 2006/11/25 OMO Revised check for presence of GLONASS data +// 2008/03/07 AHA Removed unnecessary failure flag +// 2008/09/01 AHA Harmonization with newest BNC version +// +// (c) DLR/GSOC +// +//------------------------------------------------------------------------------ + +#ifndef INC_RTCM2_H +#define INC_RTCM2_H + +#include +#include +#include +#include + +// +// namespace rtcm2 +// + +namespace rtcm2 { + + +//------------------------------------------------------------------------------ +// +// class thirtyBitWord (specification) +// +// Purpose: +// +// Handling of RTCM2 30bit words +// +//------------------------------------------------------------------------------ + +class ThirtyBitWord { + + public: + + // Constructor and initialization + + ThirtyBitWord(); + + void clear(); + + // Status queries + + bool fail() const; + bool validParity() const; + bool isHeader() const; + + // Access methods + + unsigned int all() const; + unsigned int value() const; + + // Input + + void get(const std::string& buf); + void get(std::istream& inp); + void getHeader(std::string& buf); + void getHeader(std::istream& inp); + + private: + + // Input + + void append(unsigned char c); + + private: + +// bool failure; + + // + // A 32-bit integer is used to store the 30-bit RTCM word as well as 2 + // parity bits retained from the previous word + // + // Bits 31..30 (from left to right) hold the parity bits D29*..D30* of + // the previous 30-bit word + // Bits 29..06 (from left to right) hold the current data bits D01..D24 + // Bits 05..00 (from left to right) hold the current parity bits D25..D30 + // + + unsigned int W; + +}; + + + +//------------------------------------------------------------------------------ +// +// RTCM2packet (class definition) +// +// Purpose: +// +// A class for handling RTCM2 data packets +// +//------------------------------------------------------------------------------ + +class RTCM2packet { + + public: + + // Constructor and initialization + + RTCM2packet(); + + void clear(); + + // Status queries + + bool valid() const; + + // Input + + void getPacket(std::string& buf); + void getPacket(std::istream& inp); + friend std::istream& operator >> (std::istream& is, RTCM2packet& p); + + // + // Access methods + // + + // Header and data words contents (parity corrected) + + unsigned int header1() const; + unsigned int header2() const; + unsigned int dataWord(int i) const; + + // Header information + + unsigned int msgType() const; + unsigned int ID() const { return msgType(); }; + unsigned int stationID() const; + unsigned int modZCount() const; + unsigned int seqNumber() const; + unsigned int nDataWords() const; + unsigned int staHealth() const; + + // Data access + + unsigned int getUnsignedBits (unsigned int start, + unsigned int n ) const; + int getBits (unsigned int start, + unsigned int n ) const; + + private: + + // All input of RTCM data uses a single instance, W, of a 30-bit word + // to maintain parity bits between consecutive inputs. + + ThirtyBitWord W; + + // Two 30-bit words make up the header of an RTCM2 message + // (parity corrected) + + unsigned int H1; + unsigned int H2; + + // Data words (parity corrected) + + std::vector DW; + +}; + + +//------------------------------------------------------------------------------ +// +// RTCM2_03 (class definition) +// +// Purpose: +// +// A class for handling RTCM 2 GPS Reference Station Parameters messages +// +//------------------------------------------------------------------------------ + +class RTCM2_03 { + + public: + // Constructor + RTCM2_03(); + + void extract(const RTCM2packet& P); + + public: + + bool validMsg; // Validity flag + double x,y,z; // Station coordinates + +}; + + +//------------------------------------------------------------------------------ +// +// RTCM2_23 (class definition) +// +// Purpose: +// +// A class for handling RTCM 2 Antenna Type Definition messages +// +//------------------------------------------------------------------------------ + +class RTCM2_23 { + + public: + + void extract(const RTCM2packet& P); + + public: + + bool validMsg; // Validity flag + std::string antType; // Antenna descriptor + std::string antSN ; // Antenna Serial Number + +}; + + +//------------------------------------------------------------------------------ +// +// RTCM2_24 (class definition) +// +// Purpose: +// +// A class for handling RTCM 2 Reference Station Antenna +// Reference Point Parameter messages +// +//------------------------------------------------------------------------------ + +class RTCM2_24 { + + public: + + void extract(const RTCM2packet& P); + + public: + + bool validMsg; // Validity flag + bool isGPS; // Flag for GPS supporting station + bool isGLONASS; // Flag for GLONASS supporting station + double x,y,z; // Station coordinates (ECEF,[m]) + double h; // Antenna height [m] + +}; + + + +//------------------------------------------------------------------------------ +// +// RTCM2_Obs (class definition) +// +// Purpose: +// +// A class for handling blocks of RTCM2 18 & 19 packets that need to be +// combined to get a complete set of measurements +// +//------------------------------------------------------------------------------ + +class RTCM2_Obs { + + public: + + RTCM2_Obs(); // Constructor + + void extract(const RTCM2packet& P); // Packet handler + void clear(); // Initialization + bool valid() const; // 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) + + void resolveEpoch (int refWeek, // Resolve epoch using reference + double refSecs, // epoch (GPS week and secs) + int& epochWeek, + double& epochSecs ) const; + + + public: + + double secs; // Seconds of hour (GPS time) + int nSat; // Number of space vehicles + std::vector PRN; // PRN (satellite number) + std::vector rng_C1; // C/A code pseudorange on L1 [m] + std::vector rng_P1; // P(Y) code pseudorange on L1 [m] + std::vector rng_P2; // Pseudorange on L2 [m] + 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: + + bool anyGPS() const; + bool anyGLONASS() const; + bool allGPS() const; + bool allGLONASS() const; + + private: + + typedef std::bitset<8> msgflags; + + msgflags availability; // Msg availability flags + bool GPSonly; // Flag for GPS-only station + +}; + + +}; // End of namespace rtcm2 + +#endif // include blocker Index: trunk/BNC/src/RTCM/RTCM2Decoder.cpp =================================================================== --- trunk/BNC/src/RTCM/RTCM2Decoder.cpp (revision 4279) +++ trunk/BNC/src/RTCM/RTCM2Decoder.cpp (revision 4279) @@ -0,0 +1,415 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +/* ------------------------------------------------------------------------- + * BKG NTRIP Client + * ------------------------------------------------------------------------- + * + * Class: RTCM2Decoder + * + * Purpose: RTCM2 Decoder + * + * Author: L. Mervart + * + * Created: 24-Aug-2006 + * + * Changes: + * + * -----------------------------------------------------------------------*/ + +#include +#include +#include +#include + +#include "../bncutils.h" +#include "rtcm_utils.h" +#include "GPSDecoder.h" +#include "RTCM2Decoder.h" + +using namespace std; +using namespace rtcm2; + +// +// Constructor +// + +RTCM2Decoder::RTCM2Decoder(const std::string& ID) { + _ID = ID; +} + +// +// Destructor +// + +RTCM2Decoder::~RTCM2Decoder() { +} + + +// +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::getStaCrd(double& xx, double& yy, double& zz, + double& dx1, double& dy1, double& dz1, + double& dx2, double& dy2, double& dz2) { + xx = _msg03.x; + yy = _msg03.y; + zz = _msg03.z; + + dx1 = (_msg22.validMsg ? _msg22.dL1[0] : 0.0); + dy1 = (_msg22.validMsg ? _msg22.dL1[1] : 0.0); + dz1 = (_msg22.validMsg ? _msg22.dL1[2] : 0.0); + + dx2 = (_msg22.validMsg ? _msg22.dL2[0] : 0.0); + dy2 = (_msg22.validMsg ? _msg22.dL2[1] : 0.0); + dz2 = (_msg22.validMsg ? _msg22.dL2[2] : 0.0); + + return success; +} + + +// +t_irc RTCM2Decoder::Decode(char* buffer, int bufLen, vector& errmsg) { + + errmsg.clear(); + + _buffer.append(buffer, bufLen); + int refWeek; + double refSecs; + currentGPSWeeks(refWeek, refSecs); + bool decoded = false; + + while(true) { + _PP.getPacket(_buffer); + if (!_PP.valid()) { + if (decoded) { + return success; + } else { + return failure; + } + } + + // Store message number + _typeList.push_back(_PP.ID()); + + if ( _PP.ID()==18 || _PP.ID()==19 ) { + + _ObsBlock.extract(_PP); + + if (_ObsBlock.valid()) { + decoded = true; + + int epochWeek; + double epochSecs; + _ObsBlock.resolveEpoch(refWeek, refSecs, epochWeek, epochSecs); + + for (int iSat=0; iSat < _ObsBlock.nSat; iSat++) { + t_obs obs; + if (_ObsBlock.PRN[iSat] > 100) { + obs.satNum = _ObsBlock.PRN[iSat] % 100; + obs.satSys = 'R'; + } + else { + obs.satNum = _ObsBlock.PRN[iSat]; + obs.satSys = 'G'; + } + obs.GPSWeek = epochWeek; + obs.GPSWeeks = epochSecs; + obs.C1 = _ObsBlock.rng_C1[iSat]; + obs.P1 = _ObsBlock.rng_P1[iSat]; + obs.P2 = _ObsBlock.rng_P2[iSat]; + obs.L1P = _ObsBlock.resolvedPhase_L1(iSat); + obs.L2P = _ObsBlock.resolvedPhase_L2(iSat); + obs.slip_cnt_L1 = _ObsBlock.slip_L1[iSat]; + obs.slip_cnt_L2 = _ObsBlock.slip_L2[iSat]; + + _obsList.push_back(obs); + } + _ObsBlock.clear(); + } + } + + else if ( _PP.ID() == 20 || _PP.ID() == 21 ) { + _msg2021.extract(_PP); + + if (_msg2021.valid()) { + decoded = true; + translateCorr2Obs(errmsg); + } + } + + else if ( _PP.ID() == 3 ) { + _msg03.extract(_PP); + } + + else if ( _PP.ID() == 22 ) { + _msg22.extract(_PP); + } + + else if ( _PP.ID() == 23 ) { + _msg23.extract(_PP); + } + + else if ( _PP.ID() == 24 ) { + _msg24.extract(_PP); + } + + // Output for RTCM scan + if ( _PP.ID() == 3 ) { + if ( _msg03.validMsg ) { + _antList.push_back(t_antInfo()); + + this->getStaCrd(_antList.back().xx, _antList.back().yy, _antList.back().zz); + + _antList.back().type = t_antInfo::APC; + _antList.back().message = _PP.ID(); + } + } + else if ( _PP.ID() == 23 ) { + if ( _msg23.validMsg ) { + _antType.push_back(_msg23.antType.c_str()); + } + } + else if ( _PP.ID() == 24 ) { + if ( _msg24.validMsg ) { + _antList.push_back(t_antInfo()); + + _antList.back().xx = _msg24.x; + _antList.back().yy = _msg24.y; + _antList.back().zz = _msg24.z; + + _antList.back().height_f = true; + _antList.back().height = _msg24.h; + + _antList.back().type = t_antInfo::ARP; + _antList.back().message = _PP.ID(); + } + } + } + return success; +} + +void RTCM2Decoder::translateCorr2Obs(vector& errmsg) { + + QMutexLocker locker(&_mutex); + + 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; + + // beg test + if ( corr->PRN >= 200 ) { + continue; + } + // end test + + QString prn; + if (corr->PRN < 200) { + prn = 'G' + QString("%1").arg(corr->PRN, 2, 10, QChar('0')); + } + else { + prn = 'R' + QString("%1").arg(corr->PRN - 200, 2, 10, QChar('0')); + } + + const t_ephPair* ePair = ephPair(prn); + + double L1 = 0; + double L2 = 0; + double P1 = 0; + double P2 = 0; + string obsT = ""; + + // new observation + t_obs* new_obs = 0; + + // missing IOD + vector missingIOD; + vector hasIOD; + 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; + } + + // Select corresponding ephemerides + if (ePair) { + if (ePair->last && ePair->last->IOD() == IODcorr) { + eph = ePair->last; + } + else if (ePair->prev && ePair->prev->IOD() == IODcorr) { + eph = ePair->prev; + } + } + + if ( eph ) { + ostringstream msg; + msg << obsT << ':' << setw(3) << eph->IOD(); + hasIOD.push_back(msg.str()); + + + 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->StatID[0] = '\x0'; + new_obs->satSys = (corr->PRN < 200 ? 'G' : 'R'); + new_obs->satNum = (corr->PRN < 200 ? corr->PRN : corr->PRN - 200); + + new_obs->GPSWeek = GPSWeek_rcv; + new_obs->GPSWeeks = GPSWeeks_rcv; + } + + // Store estimated measurements + // ---------------------------- + switch (ii) { + case 0: // --- L1 --- + new_obs->L1P = *obsVal / LAMBDA_1; + new_obs->slip_cnt_L1 = corr->lock1; + break; + case 1: // --- L2 --- + new_obs->L2P = *obsVal / LAMBDA_2; + new_obs->slip_cnt_L2 = corr->lock2; + break; + case 2: // --- C1 / P1 --- + if ( corr->Pind1 ) + new_obs->P1 = *obsVal; + else + new_obs->C1 = *obsVal; + break; + case 3: // --- C2 / P2 --- + if ( corr->Pind2 ) + new_obs->P2 = *obsVal; + else + new_obs->C2 = *obsVal; + break; + default: + continue; + } + } + else if ( IODcorr != 0 ) { + ostringstream msg; + msg << obsT << ':' << setw(3) << IODcorr; + missingIOD.push_back(msg.str()); + } + } // loop over frequencies + + // Error report + if ( missingIOD.size() ) { + ostringstream missingIODstr; + + copy(missingIOD.begin(), missingIOD.end(), ostream_iterator(missingIODstr, " ")); + + errmsg.push_back("missing eph for " + string(prn.toAscii().data()) + " , IODs " + missingIODstr.str()); + } + + // Store new observation + if ( new_obs ) { + _obsList.push_back(*new_obs); + delete new_obs; + } + } +} Index: trunk/BNC/src/RTCM/RTCM2Decoder.h =================================================================== --- trunk/BNC/src/RTCM/RTCM2Decoder.h (revision 4279) +++ trunk/BNC/src/RTCM/RTCM2Decoder.h (revision 4279) @@ -0,0 +1,76 @@ +// Part of BNC, a utility for retrieving decoding and +// converting GNSS data streams from NTRIP broadcasters. +// +// Copyright (C) 2007 +// German Federal Agency for Cartography and Geodesy (BKG) +// http://www.bkg.bund.de +// Czech Technical University Prague, Department of Geodesy +// http://www.fsv.cvut.cz +// +// Email: euref-ip@bkg.bund.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// as published by the Free Software Foundation, version 2. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + +#ifndef INC_RTCM2DECODER_H +#define INC_RTCM2DECODER_H + +#include +#include +#include + +#include "GPSDecoder.h" +#include "RTCM2.h" +#include "RTCM2_2021.h" +#include "rtcm3torinex.h" +#include "ephemeris.h" +#include "bncephuser.h" + +class RTCM2Decoder: public bncEphUser, public GPSDecoder { + + public: + RTCM2Decoder(const std::string& ID); + virtual ~RTCM2Decoder(); + virtual t_irc Decode(char* buffer, int bufLen, std::vector& errmsg); + + t_irc getStaCrd(double& xx, double& yy, double& zz); + + t_irc getStaCrd(double& xx, double& yy, double& zz, + double& dx1, double& dy1, double& dz1, + double& dx2, double& dy2, double& dz2); + + const rtcm2::RTCM2_2021& msg2021() const { return _msg2021; } + + std::string ID() const { return _ID; } + + private: + + void translateCorr2Obs(std::vector& errmsg); + + 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_23 _msg23; + rtcm2::RTCM2_24 _msg24; + rtcm2::RTCM2_2021 _msg2021; +}; + +#endif // include blocker Index: trunk/BNC/src/RTCM/RTCM2_2021.cpp =================================================================== --- trunk/BNC/src/RTCM/RTCM2_2021.cpp (revision 4279) +++ trunk/BNC/src/RTCM/RTCM2_2021.cpp (revision 4279) @@ -0,0 +1,235 @@ +#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 ) { + bool P_CA_Ind = P.getBits (iSat*48 + 25, 1); + 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; + corr->Pind1 = P_CA_Ind; + } + else { + corr-> range2 = (corrVal ? corrVal : ZEROVALUE); + corr->drange2 = dcorrVal; + corr->IODr2 = IOD; + corr->Pind2 = P_CA_Ind; + } + } + } + + 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& 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; + + dL2[0] = 0.0; + dL2[1] = 0.0; + dL2[2] = 0.0; +} + +/////////////////////////////// + Index: trunk/BNC/src/RTCM/RTCM2_2021.h =================================================================== --- trunk/BNC/src/RTCM/RTCM2_2021.h (revision 4279) +++ trunk/BNC/src/RTCM/RTCM2_2021.h (revision 4279) @@ -0,0 +1,81 @@ +#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() { + validMsg = false; + } + + void extract(const RTCM2packet& P); + + bool validMsg; + double dL1[3]; + double dL2[3]; +}; + +}; // end of namespace rtcm2 + + Index: trunk/BNC/src/RTCM/rtcm_utils.cpp =================================================================== --- trunk/BNC/src/RTCM/rtcm_utils.cpp (revision 4279) +++ trunk/BNC/src/RTCM/rtcm_utils.cpp (revision 4279) @@ -0,0 +1,115 @@ +#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 + // ---------------------------------------------------- + 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 + + return 0; +} Index: trunk/BNC/src/RTCM/rtcm_utils.h =================================================================== --- trunk/BNC/src/RTCM/rtcm_utils.h (revision 4279) +++ trunk/BNC/src/RTCM/rtcm_utils.h (revision 4279) @@ -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