| 1 | #include <iostream>
|
|---|
| 2 | #include <iomanip>
|
|---|
| 3 | #include <algorithm>
|
|---|
| 4 |
|
|---|
| 5 | #include "RTCM2_2021.h"
|
|---|
| 6 |
|
|---|
| 7 | using namespace rtcm2;
|
|---|
| 8 | using namespace std;
|
|---|
| 9 |
|
|---|
| 10 | const double ZEROVALUE = 1e-100;
|
|---|
| 11 |
|
|---|
| 12 | RTCM2_2021::RTCM2_2021() { }
|
|---|
| 13 |
|
|---|
| 14 |
|
|---|
| 15 | void 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 |
|
|---|
| 133 | const 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 |
|
|---|
| 139 | RTCM2_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 |
|
|---|
| 145 | void 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 |
|
|---|
| 156 | RTCM2_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 |
|
|---|
| 168 | void 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 |
|
|---|
| 188 | std::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 |
|
|---|
| 216 | void 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 |
|
|---|
