source: ntrip/trunk/BNC/RTCM/RTCM2_2021.cpp@ 1034

Last change on this file since 1034 was 1024, checked in by zdenek, 16 years ago

Zdenek Lukes: added logic for decoding of message 20/21 from RTCM 2.3 streams
File size: 5.9 KB
Line 
1#include <iostream>
2#include <iomanip>
3#include <algorithm>
4
5#include "RTCM2_2021.h"
6
7using namespace rtcm2;
8using namespace std;
9
10const double ZEROVALUE = 1e-100;
11
12RTCM2_2021::RTCM2_2021() { }
13
14
15void RTCM2_2021::extract(const RTCM2packet& P) {
16 if ( !P.valid() || P.ID() != 20 && P.ID() != 21 ) {
17 return;
18 }
19
20 // Error: at least 4 data words
21 if ( P.nDataWords()<5 ) {
22#if ( DEBUG > 0 )
23 cerr << "Error in RTCM2_Obs::extract(): less than 3 DW ("
24 << P.nDataWords() << ") detected" << endl;
25#endif
26 return;
27 };
28
29 // Error: number of data words has to be odd number
30 if ( P.nDataWords()%2==0 ){
31#if ( DEBUG > 0 )
32 cerr << "Error in RTCM2_Obs::extract(): odd number of DW ("
33 << P.nDataWords() << ") detected" << endl;
34#endif
35 return;
36 };
37
38 // Current epoch (mod 3600 sec)
39 double tt = 0.6*P.modZCount()
40 + P.getUnsignedBits(4,20)*1.0e-6;
41
42 // Clear old epoch
43 if ( tt != tt_ || valid_ ) {
44 clear();
45 tt_ = tt;
46 valid_ = false;
47 }
48
49
50 // Frequency (exit if neither L1 nor L2)
51 bool isL1 = ( P.getUnsignedBits(0,1)==0 );
52 if ( P.getUnsignedBits(1,1)==1 ) {
53 return;
54 }
55
56 // Number of satellites
57 unsigned nSat = (P.nDataWords() - 1) / 2;
58
59 double multipleMsgInd = true;
60 for (unsigned iSat = 0; iSat < nSat; iSat++) {
61 bool multInd = P.getBits (iSat*48 + 24, 1);
62 bool isGPS = ( P.getUnsignedBits(iSat*48 + 26, 1)==0 );
63 unsigned PRN = P.getUnsignedBits(iSat*48 + 27, 5);
64
65 multipleMsgInd = multipleMsgInd && multInd;
66
67 if ( !isGPS ) {
68 PRN += 200;
69 }
70 if ( PRN == 0 ) {
71 PRN = 32;
72 }
73
74 HiResCorr* corr = 0;
75 if ( !(corr = find_i(PRN)) ) {
76 data_i_[PRN] = HiResCorr();
77 corr = &(data_i_[PRN]);
78 }
79 if ( !find(PRN) ) {
80 data[PRN] = corr;
81 }
82
83 corr->PRN = PRN;
84 corr->tt = tt_;
85
86 // Message number 20
87 if ( P.ID() == 20 ) {
88 unsigned lossLock = P.getUnsignedBits(iSat*48 + 35, 5);
89 unsigned IOD = P.getUnsignedBits(iSat*48 + 40, 8);
90 double corrVal = P.getBits (iSat*48 + 48, 24) / 256.0;
91
92 if ( isL1 ) {
93 corr->phase1 = (corrVal ? corrVal : ZEROVALUE);
94 corr->slip1 = (corr->lock1 != lossLock);
95 corr->lock1 = lossLock;
96 corr->IODp1 = IOD;
97 }
98 else {
99 corr->phase2 = (corrVal ? corrVal : ZEROVALUE);
100 corr->slip2 = (corr->lock2 != lossLock);
101 corr->lock2 = lossLock;
102 corr->IODp2 = IOD;
103 }
104 }
105
106 // Message number 21
107 else if ( P.ID() == 21 ) {
108 double dcorrUnit = ( P.getUnsignedBits(iSat*48 + 32, 1) ? 0.032 : 0.002);
109 double corrUnit = ( P.getUnsignedBits(iSat*48 + 36, 1) ? 0.320 : 0.020);
110 unsigned IOD = P.getUnsignedBits(iSat*48 + 40, 8);
111 double corrVal = P.getBits (iSat*48 + 48, 16) * corrUnit;
112 double dcorrVal = P.getBits (iSat*48 + 64, 8) * dcorrUnit;
113
114 if ( isL1 ) {
115 corr-> range1 = (corrVal ? corrVal : ZEROVALUE);
116 corr->drange1 = dcorrVal;
117 corr->IODr1 = IOD;
118 }
119 else {
120 corr-> range2 = (corrVal ? corrVal : ZEROVALUE);
121 corr->drange2 = dcorrVal;
122 corr->IODr2 = IOD;
123 }
124 }
125 }
126
127 valid_ = !multipleMsgInd;
128}
129
130const RTCM2_2021::HiResCorr* RTCM2_2021::find(unsigned PRN) {
131 std::map<unsigned, const HiResCorr*>::const_iterator ii = data.find(PRN);
132 return (ii != data.end() ? ii->second : 0);
133}
134
135
136RTCM2_2021::HiResCorr* RTCM2_2021::find_i(unsigned PRN) {
137 std::map<unsigned, HiResCorr>::iterator ii = data_i_.find(PRN);
138 return (ii != data_i_.end() ? &(ii->second) : 0);
139}
140
141
142void RTCM2_2021::clear() {
143 tt_ = 0;
144 valid_ = false;
145 for (map<unsigned, HiResCorr>::iterator
146 ii = data_i_.begin(); ii != data_i_.end(); ii++) {
147 ii->second.reset();
148 }
149 data.clear();
150}
151
152
153RTCM2_2021::HiResCorr::HiResCorr() :
154 PRN(0), tt(0),
155 phase1 (0), phase2 (2),
156 lock1 (0), lock2 (0),
157 slip1 (false), slip2 (false),
158 IODp1 (0), IODp2 (0),
159 range1 (0), range2 (0),
160 drange1(0), drange2(0),
161 Pind1 (false), Pind2 (false),
162 IODr1 (0), IODr2 (0) {
163}
164
165void RTCM2_2021::HiResCorr::reset() {
166 // does not reset 'lock' indicators and PRN
167 tt = 0;
168 phase1 = 0;
169 phase2 = 0;
170 slip1 = false;
171 slip2 = false;
172 IODp1 = 0;
173 IODp2 = 0;
174
175 range1 = 0;
176 range2 = 0;
177 drange1 = 0;
178 drange2 = 0;
179 IODr1 = 0;
180 IODr2 = 0;
181 Pind1 = false;
182 Pind2 = false;
183}
184
185std::ostream& rtcm2::operator << (std::ostream& out, const RTCM2_2021::HiResCorr& cc) {
186 out.setf(ios::fixed);
187 out << setw(8) << setprecision(8) << cc.tt
188 << ' ' << setw(2) << cc.PRN
189 << " L1 "
190 << ' ' << setw(8) << setprecision(3) << (cc.phase1 ? cc.phase1 : 9999.999)
191 << ' ' << setw(1) << (cc.phase1 ? (cc.slip1 ? '1' : '0') : '.')
192 << ' ' << setw(2) << (cc.phase1 ? cc.lock1 : 99)
193 << ' ' << setw(3) << (cc.phase1 ? cc.IODp1 : 999)
194 << " L2 "
195 << ' ' << setw(8) << setprecision(3) << (cc.phase2 ? cc.phase2 : 9999.999)
196 << ' ' << setw(1) << (cc.phase2 ? (cc.slip2 ? '1' : '0') : '.')
197 << ' ' << setw(2) << (cc.phase2 ? cc.lock2 : 99)
198 << ' ' << setw(3) << (cc.phase2 ? cc.IODp2 : 999)
199 << " P1 "
200 << ' ' << setw(8) << setprecision(3) << (cc.range1 ? cc.range1 : 9999.999)
201 << ' ' << setw(3) << (cc.range1 ? cc.IODr1 : 999)
202 << " P2 "
203 << ' ' << setw(8) << setprecision(3) << (cc.range2 ? cc.range2 : 9999.999)
204 << ' ' << setw(3) << (cc.phase2 ? cc.IODr2 : 999);
205
206 return out;
207}
208
209
210//////////////////////////////////////////////////////////////////////////////////////////////////////////////
211
212
213void RTCM2_22::extract(const RTCM2packet& P) {
214 if ( P.ID() != 22 ) {
215 return;
216 }
217
218 const double dL1unit = 0.01 / 256;
219
220 validMsg = true;
221
222 dL1[0] = P.getBits( 0, 8) * dL1unit;
223 dL1[1] = P.getBits( 8, 8) * dL1unit;
224 dL1[2] = P.getBits(16, 8) * dL1unit;
225}
226
227///////////////////////////////
228
Note: See TracBrowser for help on using the repository browser.