source: ntrip/trunk/BNC/src/RTCM/RTCM2_2021.cpp@ 8154

Last change on this file since 8154 was 7629, checked in by stuerze, 9 years ago

some value initialization in constructor is added

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