//------------------------------------------------------------------------------
//
// 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 <bitset>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#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<buf.size() ) {
// Process byte
append(buf[i]);
// Increment count
i++;
};
// Remove processed bytes from buffer. Retain also the previous word to
// allow a resync if getHeader() is called repeatedly on the same buffer.
if (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<n; i++) {
W.get(buf.substr((spare+2+i)*wordLen,buf.size()-(spare+2+i)*wordLen));
DW[i] = W.value();
if (!W.validParity()) {
// Invalid data word; delete first 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 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<n; i++) {
W.get(inp);
DW[i] = W.value();
if (inp.fail() || !W.validParity()) { clear(); return; }
};
return;
};
//
// Input operator
//
// Reads an RTCM2 packet from the input stream.
//
istream& operator >> (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<iLast ) {
bitField = tmp;
for (unsigned int iWord=iFirst+1; iWord<iLast; iWord++) {
tmp = (DW[iWord]>>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<nad;i++)
antType += (char)P.getUnsignedBits(nF1+i*8,8);
// Optional antenna serial numbers
if (P.getUnsignedBits(2,1)==1) {
// Check number of data words (can nas be read in?)
validMsg = ( P.nDataWords() >=
(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<nas;i++)
antSN += (char)P.getUnsignedBits(nF1+8*nad+nF2+i*8,8);
};
};
//------------------------------------------------------------------------------
//
// RTCM2_24 (class implementation)
//
// Purpose:
//
// A class for handling RTCM 2 Reference Station Antenna
// Reference Point Parameter messages
//
//------------------------------------------------------------------------------
void RTCM2_24::extract(const RTCM2packet& P) {
double dx,dy,dz;
// Check validity, packet type and number of data words
validMsg = (P.valid());
if (!validMsg) return;
validMsg = (P.ID()==24);
if (!validMsg) return;
validMsg = (P.nDataWords()==6);
if (!validMsg) return;
// System indicator
isGPS = (P.getUnsignedBits(118,1)==0);
isGLONASS = (P.getUnsignedBits(118,1)==1);
// Antenna reference point coordinates
x = 64.0*P.getBits( 0,32);
y = 64.0*P.getBits(40,32);
z = 64.0*P.getBits(80,32);
dx = P.getUnsignedBits( 32,6);
dy = P.getUnsignedBits( 72,6);
dz = P.getUnsignedBits(112,6);
x = 0.0001*( x + (x<0? -dx:+dx) );
y = 0.0001*( y + (y<0? -dy:+dy) );
z = 0.0001*( z + (z<0? -dz:+dz) );
// Antenna Height
if (P.getUnsignedBits(119,1)==1) {
h= P.getUnsignedBits(120,18)*0.0001;
};
};
//------------------------------------------------------------------------------
//
// 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
//
// Notes:
//
// The class collects L1/L2 code and phase measurements for GPS and GLONASS.
// Since the Multiple Message Indicator is inconsistently handled by various
// receivers we simply require code and phase on L1 and L2 for a complete
// set ob observations at a given epoch. GLONASS observations are optional,
// but all four types (code+phase,L1+L2) must be provided, if at least one
// is given. Also, the GLONASS message must follow the corresponding GPS
// message.
//
//------------------------------------------------------------------------------
// Constructor
RTCM2_Obs::RTCM2_Obs() {
clear();
};
// Reset entire block
void RTCM2_Obs::clear() {
GPSonly = true;
secs=0.0; // Seconds of hour (GPS time)
nSat=0; // Number of space vehicles
PRN.resize(0); // space vehicles
rng_C1.resize(0); // Pseudorange [m]
rng_P1.resize(0); // Pseudorange [m]
rng_P2.resize(0); // Pseudorange [m]
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
};
// Availability checks
bool RTCM2_Obs::anyGPS() const {
return availability.test(bit_L1rngGPS) ||
availability.test(bit_L2rngGPS) ||
availability.test(bit_L1cphGPS) ||
availability.test(bit_L2cphGPS);
};
bool RTCM2_Obs::anyGLONASS() const {
return availability.test(bit_L1rngGLO) ||
availability.test(bit_L2rngGLO) ||
availability.test(bit_L1cphGLO) ||
availability.test(bit_L2cphGLO);
};
bool RTCM2_Obs::allGPS() const {
return availability.test(bit_L1rngGPS) &&
availability.test(bit_L2rngGPS) &&
availability.test(bit_L1cphGPS) &&
availability.test(bit_L2cphGPS);
};
bool RTCM2_Obs::allGLONASS() const {
return availability.test(bit_L1rngGLO) &&
availability.test(bit_L2rngGLO) &&
availability.test(bit_L1cphGLO) &&
availability.test(bit_L2cphGLO);
};
// Validity
bool RTCM2_Obs::valid() const {
return ( allGPS() && ( GPSonly || allGLONASS() ) );
};
//
// Extract RTCM2 18 & 19 messages and store relevant data for future use
//
void RTCM2_Obs::extract(const RTCM2packet& P) {
bool isGPS,isCAcode,isL1,isOth;
int NSat,idx;
int sid,prn,slip_cnt;
double t,rng,cph;
// Check validity and packet type
if ( ! ( P.valid() &&
(P.ID()==18 || P.ID()==19) ) ) return;
// Check number of data words, message starts with 1 DW for epoch, then each
// satellite brings 2 DW,
// Do not start decoding if less than 3 DW are in package
if ( P.nDataWords()<3 ) {
#if ( DEBUG > 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;iSat<NSat;iSat++){
// Code type
isCAcode = ( P.getUnsignedBits(iSat*48+25,1)==0 );
// Satellite
sid = P.getUnsignedBits(iSat*48+27,5);
if (sid==0) sid=32;
prn = (isGPS? sid : sid+200 );
// 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?
idx=-1;
for (unsigned int i=0;i<PRN.size();i++) {
if (PRN[i]==prn) { idx=i; break; };
};
if (idx==-1) {
// Insert new sat at end of list
nSat++; idx = nSat-1;
PRN.push_back(prn);
rng_C1.push_back(0.0);
rng_P1.push_back(0.0);
rng_P2.push_back(0.0);
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;
slip_L1[idx] = slip_cnt;
}
else {
cph_L2[idx] = cph;
slip_L2[idx] = slip_cnt;
};
};
};
// Process pseudorange message
if ( P.ID()==19 ) {
// 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;
#if (FIX_TRIMBLE_4000SSI==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.
int nGPS=0;
for(int iSat=0; iSat<NSat; iSat++){
// Constellation (for each satellite in message)
isGPS = ( P.getUnsignedBits(iSat*48+26,1)==0 );
if(isGPS) nGPS++;
};
isGPS = (2*nGPS>NSat);
#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;iSat<NSat;iSat++){
// Code type
isCAcode = ( P.getUnsignedBits(iSat*48+25,1)==0 );
// Satellite
sid = P.getUnsignedBits(iSat*48+27,5);
if (sid==0) sid=32;
prn = (isGPS? sid : sid+200 );
// Pseudorange measurement [m]
rng = P.getUnsignedBits(iSat*48+40,32)*0.02;
// Is this a new PRN?
idx=-1;
for (unsigned int i=0;i<PRN.size();i++) {
if (PRN[i]==prn) { idx=i; break; };
};
if (idx==-1) {
// Insert new sat at end of list
nSat++; idx = nSat-1;
PRN.push_back(prn);
rng_C1.push_back(0.0);
rng_P1.push_back(0.0);
rng_P2.push_back(0.0);
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) {
if (isCAcode) {
rng_C1[idx] = rng;
}
else {
rng_P1[idx] = rng;
}
}
else {
rng_P2[idx] = rng;
};
};
};
};
//
// Resolution of 2^24 cy carrier phase ambiguity
// caused by 32-bit data field restrictions
//
// Note: the RTCM standard specifies an ambiguity of +/-2^23 cy.
// However, numerous receivers generate data in the +/-2^22 cy range.
// A reduced ambiguity of 2^23 cy appears compatible with both cases.
//
double RTCM2_Obs::resolvedPhase_L1(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_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