source: ntrip/branches/BNC_LM/RTCM/RTCM2_2021.cpp@ 6199

Last change on this file since 6199 was 2588, checked in by stoecker, 14 years ago

fix compiler warning

File size: 6.1 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 bool P_CA_Ind = P.getBits (iSat*48 + 25, 1);
109 double dcorrUnit = ( P.getUnsignedBits(iSat*48 + 32, 1) ? 0.032 : 0.002);
110 double corrUnit = ( P.getUnsignedBits(iSat*48 + 36, 1) ? 0.320 : 0.020);
111 unsigned IOD = P.getUnsignedBits(iSat*48 + 40, 8);
112 double corrVal = P.getBits (iSat*48 + 48, 16) * corrUnit;
113 double dcorrVal = P.getBits (iSat*48 + 64, 8) * dcorrUnit;
114
115 if ( isL1 ) {
116 corr-> range1 = (corrVal ? corrVal : ZEROVALUE);
117 corr->drange1 = dcorrVal;
118 corr->IODr1 = IOD;
119 corr->Pind1 = P_CA_Ind;
120 }
121 else {
122 corr-> range2 = (corrVal ? corrVal : ZEROVALUE);
123 corr->drange2 = dcorrVal;
124 corr->IODr2 = IOD;
125 corr->Pind2 = P_CA_Ind;
126 }
127 }
128 }
129
130 valid_ = !multipleMsgInd;
131}
132
133const RTCM2_2021::HiResCorr* RTCM2_2021::find(unsigned PRN) {
134 std::map<unsigned, const HiResCorr*>::const_iterator ii = data.find(PRN);
135 return (ii != data.end() ? ii->second : 0);
136}
137
138
139RTCM2_2021::HiResCorr* RTCM2_2021::find_i(unsigned PRN) {
140 std::map<unsigned, HiResCorr>::iterator ii = data_i_.find(PRN);
141 return (ii != data_i_.end() ? &(ii->second) : 0);
142}
143
144
145void RTCM2_2021::clear() {
146 tt_ = 0;
147 valid_ = false;
148 for (map<unsigned, HiResCorr>::iterator
149 ii = data_i_.begin(); ii != data_i_.end(); ii++) {
150 ii->second.reset();
151 }
152 data.clear();
153}
154
155
156RTCM2_2021::HiResCorr::HiResCorr() :
157 PRN(0), tt(0),
158 phase1 (0), phase2 (2),
159 lock1 (0), lock2 (0),
160 slip1 (false), slip2 (false),
161 IODp1 (0), IODp2 (0),
162 range1 (0), range2 (0),
163 drange1(0), drange2(0),
164 Pind1 (false), Pind2 (false),
165 IODr1 (0), IODr2 (0) {
166}
167
168void RTCM2_2021::HiResCorr::reset() {
169 // does not reset 'lock' indicators and PRN
170 tt = 0;
171 phase1 = 0;
172 phase2 = 0;
173 slip1 = false;
174 slip2 = false;
175 IODp1 = 0;
176 IODp2 = 0;
177
178 range1 = 0;
179 range2 = 0;
180 drange1 = 0;
181 drange2 = 0;
182 IODr1 = 0;
183 IODr2 = 0;
184 Pind1 = false;
185 Pind2 = false;
186}
187
188std::ostream& operator << (std::ostream& out, const RTCM2_2021::HiResCorr& cc) {
189 out.setf(ios::fixed);
190 out << setw(8) << setprecision(8) << cc.tt
191 << ' ' << setw(2) << cc.PRN
192 << " L1 "
193 << ' ' << setw(8) << setprecision(3) << (cc.phase1 ? cc.phase1 : 9999.999)
194 << ' ' << setw(1) << (cc.phase1 ? (cc.slip1 ? '1' : '0') : '.')
195 << ' ' << setw(2) << (cc.phase1 ? cc.lock1 : 99)
196 << ' ' << setw(3) << (cc.phase1 ? cc.IODp1 : 999)
197 << " L2 "
198 << ' ' << setw(8) << setprecision(3) << (cc.phase2 ? cc.phase2 : 9999.999)
199 << ' ' << setw(1) << (cc.phase2 ? (cc.slip2 ? '1' : '0') : '.')
200 << ' ' << setw(2) << (cc.phase2 ? cc.lock2 : 99)
201 << ' ' << setw(3) << (cc.phase2 ? cc.IODp2 : 999)
202 << " P1 "
203 << ' ' << setw(8) << setprecision(3) << (cc.range1 ? cc.range1 : 9999.999)
204 << ' ' << setw(3) << (cc.range1 ? cc.IODr1 : 999)
205 << " P2 "
206 << ' ' << setw(8) << setprecision(3) << (cc.range2 ? cc.range2 : 9999.999)
207 << ' ' << setw(3) << (cc.phase2 ? cc.IODr2 : 999);
208
209 return out;
210}
211
212
213//////////////////////////////////////////////////////////////////////////////////////////////////////////////
214
215
216void RTCM2_22::extract(const RTCM2packet& P) {
217 if ( P.ID() != 22 ) {
218 return;
219 }
220
221 const double dL1unit = 0.01 / 256;
222
223 validMsg = true;
224
225 dL1[0] = P.getBits( 0, 8) * dL1unit;
226 dL1[1] = P.getBits( 8, 8) * dL1unit;
227 dL1[2] = P.getBits(16, 8) * dL1unit;
228
229 dL2[0] = 0.0;
230 dL2[1] = 0.0;
231 dL2[2] = 0.0;
232}
233
234///////////////////////////////
235
Note: See TracBrowser for help on using the repository browser.