Index: branches/BNC_2.12/src/RTCM3/RTCM3Decoder.cpp =================================================================== --- branches/BNC_2.12/src/RTCM3/RTCM3Decoder.cpp (revision 8804) +++ branches/BNC_2.12/src/RTCM3/RTCM3Decoder.cpp (revision 8805) @@ -243,5 +243,5 @@ {GPS_WAVELENGTH_L1, "1P"}, {GPS_WAVELENGTH_L1, "1W"}, - {0.0, 0}/*{GPS_WAVELENGTH_L1,"1Y"}*/, + {0.0, 0}, {0.0, 0}, {0.0, 0}, @@ -249,5 +249,5 @@ {GPS_WAVELENGTH_L2, "2P"}, {GPS_WAVELENGTH_L2, "2W"}, - {0.0, 0}/*{GPS_WAVELENGTH_L2,"2Y"}*/, + {0.0, 0}, {0.0, 0}, {0.0, 0}, @@ -288,13 +288,13 @@ {1.0, "2C"}, {1.0, "2P"}, - {0.0, 0}, - {0.0, 0}, - {0.0, 0}, - {0.0, 0}, - {0.0, 0}, - {0.0, 0}, - {0.0, 0}, - {0.0, 0}, - {0.0, 0}, + {GLO_WAVELENGTH_L1a, "4A"}, + {GLO_WAVELENGTH_L1a, "4B"}, + {GLO_WAVELENGTH_L1a, "4X"}, + {GLO_WAVELENGTH_L2a, "6A"}, + {GLO_WAVELENGTH_L2a, "6B"}, + {GLO_WAVELENGTH_L2a, "6X"}, + {GLO_WAVELENGTH_L3, "3I"}, + {GLO_WAVELENGTH_L3, "3Q"}, + {GLO_WAVELENGTH_L3, "3X"}, {0.0, 0}, {0.0, 0}, @@ -346,5 +346,5 @@ {0.0, 0}, {0.0, 0}, - {0.0, 0}, + {0.0, 0} }; @@ -421,10 +421,49 @@ }; +/** + * MSM signal types for IRNSS + * + * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later! + */ +static struct CodeData irn[RTCM3_MSM_NUMSIG] = { + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {IRNSS_WAVELENGTH_S, "9A"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {IRNSS_WAVELENGTH_L5, "5A"}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0}, + {0.0, 0} + }; + #define UINT64(c) c ## ULL // //////////////////////////////////////////////////////////////////////////// -bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size) - { +bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size) { bool decoded = false; int type, syncf, i; @@ -438,5 +477,5 @@ /* id */ char sys; - if (type >= 1121) + if (type >= 1121) sys = 'C'; else if (type >= 1111) @@ -448,6 +487,8 @@ else if (type >= 1081) sys = 'R'; - else + else if (type >= 1071) sys = 'G'; + else if (type >= 21) // test + sys = 'I'; bncTime CurrentObsTime; @@ -483,11 +524,16 @@ int sigmask, numsat = 0, numsig = 0; uint64_t satmask, cellmask, ui; - double rrmod[RTCM3_MSM_NUMSAT]; - int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT], - extsat[RTCM3_MSM_NUMSAT]; - int ll[RTCM3_MSM_NUMCELLS]/*, hc[RTCM3_MSM_NUMCELLS]*/; - double cnr[RTCM3_MSM_NUMCELLS]; - double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS], - dop[RTCM3_MSM_NUMCELLS]; + // satellite data + double rrmod[RTCM3_MSM_NUMSAT]; // GNSS sat rough ranges modulo 1 millisecond + int rrint[RTCM3_MSM_NUMSAT]; // number of integer msecs in GNSS sat rough ranges + int rdop[RTCM3_MSM_NUMSAT]; // GNSS sat rough phase range rates + int extsat[RTCM3_MSM_NUMSAT];// extended sat info + // signal data + int ll[RTCM3_MSM_NUMCELLS]; // lock time indicator + /*int hc[RTCM3_MSM_NUMCELLS];*/ // half cycle ambiguity indicator + double cnr[RTCM3_MSM_NUMCELLS]; // signal cnr + double cp[RTCM3_MSM_NUMCELLS]; // fine phase range data + double psr[RTCM3_MSM_NUMCELLS]; // fine psr + double dop[RTCM3_MSM_NUMCELLS]; // fine phase range rates SKIPBITS(3 + 7 + 2 + 2 + 1 + 3) @@ -505,5 +551,5 @@ i = numsat * numsig; GETBITS64(cellmask, (unsigned )i) - + // satellite data switch (type % 10) { case 1: @@ -533,5 +579,5 @@ break; } - + // signal data int numcells = numsat * numsig; /** Drop anything which exceeds our cell limit. Increase limit definition @@ -684,15 +730,18 @@ { int k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1]; - if (extsat[numsat] < 14) { - k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1] = 100 + extsat[numsat] - - 7; + if (extsat[numsat] < 14) { // channel number is available as extended info for MSM5/7 + k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1] = 100 + extsat[numsat] - 7; } - if (k) - cd.wl = ( - cd.wl == 0.0 ? - GLO_WAVELENGTH_L1(k - 100) : - GLO_WAVELENGTH_L2(k - 100)); - else + if (k) { + if (cd.wl == 0.0) { + cd.wl = GLO_WAVELENGTH_L1(k - 100); + } + else if (cd.wl == 1.0) { + cd.wl = GLO_WAVELENGTH_L2(k - 100); + } + } + else if (!k && cd.wl <= 1) { cd.code = 0; + } } break; @@ -700,4 +749,7 @@ cd = gal[RTCM3_MSM_NUMSIG - j - 1]; break; + case 'I': + cd = irn[RTCM3_MSM_NUMSIG - j - 1]; + break; } if (cd.code) { @@ -748,6 +800,5 @@ if (cp[count] > -1.0 / (1 << 8)) { frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl - + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 - / cd.wl; + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl; frqObs->_phaseValid = true; frqObs->_lockTime = lti2sec(type,ll[count]); @@ -768,6 +819,5 @@ if (cp[count] > -1.0 / (1 << 8)) { frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl - + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 - / cd.wl; + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl; frqObs->_phaseValid = true; frqObs->_lockTime = lti2sec(type,ll[count]); @@ -793,6 +843,5 @@ if (cp[count] > -1.0 / (1 << 8)) { frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl - + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 - / cd.wl; + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl; frqObs->_phaseValid = true; frqObs->_lockTime = lti2sec(type,ll[count]); @@ -813,6 +862,5 @@ if (cp[count] > -1.0 / (1 << 8)) { frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl - + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 - / cd.wl; + + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl; frqObs->_phaseValid = true; frqObs->_lockTime = lti2sec(type,ll[count]); @@ -834,6 +882,7 @@ } } - if (CurrentObs._obs.size() > 0) + if (CurrentObs._obs.size() > 0) { _CurrentObsList.push_back(CurrentObs); + } } } @@ -1002,10 +1051,8 @@ i <<= 4; eph._TOC.set(i * 1000); - GETFLOATSIGN(eph._clock_driftrate, 8, - 1.0 / (double )(1 << 30) / (double )(1 << 25)) - GETFLOATSIGN(eph._clock_drift, 16, - 1.0 / (double )(1 << 30) / (double )(1 << 13)) - GETFLOATSIGN(eph._clock_bias, 22, - 1.0 / (double )(1 << 30) / (double )(1 << 1)) + GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25)) + GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13)) + GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1)) + GETBITS(eph._IODC, 10) GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) @@ -1076,5 +1123,8 @@ GETBITS(i, 1) tk += i * 30; - eph._tki = tk < 3 * 60 * 60 ? tk - 3 * 60 * 60 + 86400 : tk - 3 * 60 * 60; + eph._tki = tk - 3*60*60; + if(eph._tki < 0.0) { + eph._tki += 86400.0; + } GETBITS(eph._health, 1) /* MSB of Bn*/ GETBITS(eph._P2, 1) /* P2 */ @@ -1153,10 +1203,8 @@ eph._TOC.set(i * 1000); - GETFLOATSIGN(eph._clock_driftrate, 8, - 1.0 / (double )(1 << 30) / (double )(1 << 25)) - GETFLOATSIGN(eph._clock_drift, 16, - 1.0 / (double )(1 << 30) / (double )(1 << 13)) - GETFLOATSIGN(eph._clock_bias, 22, - 1.0 / (double )(1 << 30) / (double )(1 << 1)) + GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25)) + GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13)) + GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1)) + GETBITS(eph._IODE, 8) GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) @@ -1199,5 +1247,86 @@ GETBITS(eph._fitInterval, 1) eph._TOT = 0.9999e9; - eph._L2PFlag = 0; /* does not exist for QZSS */ + + emit newGPSEph(eph); + decoded = true; + } + return decoded; +} + +// +//////////////////////////////////////////////////////////////////////////// +bool RTCM3Decoder::DecodeIRNSSEphemeris(unsigned char* data, int size) { + bool decoded = false; + + if (size == 67) { + t_ephGPS eph; + int i, week, L5Flag, SFlag; + uint64_t numbits = 0, bitfield = 0; + + data += 3; /* header */ + size -= 6; /* header + crc */ + SKIPBITS(12) + + eph._receptDateTime = currentDateAndTimeGPS(); + + GETBITS(i, 6) + eph._prn.set('I', i); + GETBITS(week, 10) + GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1)) + GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13)) + GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25)) + GETBITS(i, 4) + eph._ura = accuracyFromIndex(i, eph.type()); + GETBITS(i, 16) + i <<= 4; + eph._TOC.set(i * 1000); + GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1)) + GETFLOATSIGN(eph._Delta_n, 22, R2R_PI/(double)(1<<30)/(double)(1 << 11)) + // IODCE + GETBITS(eph._IODE, 8) + eph._IODC = eph._IODE; + SKIPBITS(10) + GETBITS(L5Flag, 1) + GETBITS(SFlag, 1) + if (L5Flag == 0 && SFlag == 0) { + eph._health = 0.0; + } + else if (L5Flag == 0 && SFlag == 1) { + eph._health = 1.0; + } + else if (L5Flag == 1 && SFlag == 0) { + eph._health = 2.0; + } + else if (L5Flag == 1 && SFlag == 1) { + eph._health = 3.0; + } + GETFLOATSIGN(eph._Cuc, 15, 1.0 / (double )(1 << 28)) + GETFLOATSIGN(eph._Cus, 15, 1.0 / (double )(1 << 28)) + GETFLOATSIGN(eph._Cic, 15, 1.0 / (double )(1 << 28)) + GETFLOATSIGN(eph._Cis, 15, 1.0 / (double )(1 << 28)) + GETFLOATSIGN(eph._Crc, 15, 1.0 / (double )(1 << 4)) + GETFLOATSIGN(eph._Crs, 15, 1.0 / (double )(1 << 4)) + GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13)) + GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1)) + GETBITS(i, 16) + i <<= 4; + eph._TOEsec = i; + bncTime t; + t.set(i * 1000); + eph._TOEweek = t.gpsw(); + int numOfRollOvers = int(floor(t.gpsw()/1024.0)); + week += (numOfRollOvers * 1024); + /* week from HOW, differs from TOC, TOE week, we use adapted value instead */ + if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */ + return false; + GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3)) + GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19)) + GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1)) + GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1)) + GETFLOATSIGN(eph._OMEGADOT, 22, R2R_PI/(double)(1<<30)/(double)(1<<11)) + GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1)) + SKIPBITS(2) + SKIPBITS(2) + eph._TOT = 0.9999e9; emit newGPSEph(eph); @@ -1273,5 +1402,5 @@ eph._prn.set('E', i, eph._inav ? 1 : 0); - GETBITS(eph._TOEweek, 12) + GETBITS(eph._TOEweek, 12) //FIXME: roll-over after week 4095!! GETBITS(eph._IODnav, 10) GETBITS(i, 8) @@ -1280,10 +1409,7 @@ GETBITSFACTOR(i, 14, 60) eph._TOC.set(1024 + eph._TOEweek, i); - GETFLOATSIGN(eph._clock_driftrate, 6, - 1.0 / (double )(1 << 30) / (double )(1 << 29)) - GETFLOATSIGN(eph._clock_drift, 21, - 1.0 / (double )(1 << 30) / (double )(1 << 16)) - GETFLOATSIGN(eph._clock_bias, 31, - 1.0 / (double )(1 << 30) / (double )(1 << 4)) + GETFLOATSIGN(eph._clock_driftrate, 6, 1.0 / (double )(1 << 30) / (double )(1 << 29)) + GETFLOATSIGN(eph._clock_drift, 21, 1.0 / (double )(1 << 30) / (double )(1 << 16)) + GETFLOATSIGN(eph._clock_bias, 31, 1.0 / (double )(1 << 30) / (double )(1 << 4)) GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5)) GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13)) @@ -1310,6 +1436,5 @@ eph._e5aDataInValid = false; - GETFLOATSIGN(eph._BGD_1_5B, 10, - 1.0 / (double )(1 << 30) / (double )(1 << 2)) +GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0 / (double )(1 << 30) / (double )(1 << 2)) GETBITS(eph._E5bHS, 2) GETBITS(eph._e5bDataInValid, 1) @@ -1364,10 +1489,7 @@ i <<= 3; eph._TOC.setBDS(i * 1000); - GETFLOATSIGN(eph._clock_driftrate, 11, - 1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6)) - GETFLOATSIGN(eph._clock_drift, 22, - 1.0 / (double )(1 << 30) / (double )(1 << 20)) - GETFLOATSIGN(eph._clock_bias, 24, - 1.0 / (double )(1 << 30) / (double )(1 << 3)) + GETFLOATSIGN(eph._clock_driftrate, 11, 1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6)) + GETFLOATSIGN(eph._clock_drift, 22, 1.0 / (double )(1 << 30) / (double )(1 << 20)) + GETFLOATSIGN(eph._clock_bias, 24, 1.0 / (double )(1 << 30) / (double )(1 << 3)) GETBITS(eph._AODC, 5) GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6)) @@ -1511,5 +1633,6 @@ } } - else if (id >= 1070 && id <= 1229) /* MSM */ { + else if ((id >= 1070 && id <= 1229) || + (id >= 21 && id <= 27)) /* MSM */ { if (DecodeRTCM3MSM(_Message, _BlockSize)) decoded = true; @@ -1555,4 +1678,8 @@ decoded = true; break; + case 29: + if (DecodeIRNSSEphemeris(_Message, _BlockSize)) + decoded = true; + break; case 1045: case 1046: Index: branches/BNC_2.12/src/RTCM3/RTCM3Decoder.h =================================================================== --- branches/BNC_2.12/src/RTCM3/RTCM3Decoder.h (revision 8804) +++ branches/BNC_2.12/src/RTCM3/RTCM3Decoder.h (revision 8805) @@ -132,4 +132,11 @@ bool DecodeQZSSEphemeris(unsigned char* buffer, int bufLen); /** + * Extract ephemeris data from 29 (allocated for testing) RTCM3 messages. + * @param buffer the buffer containing an 29 RTCM block + * @param bufLen the length of the buffer (the message length including header+crc) + * @return true when data block was decodable + */ + bool DecodeIRNSSEphemeris(unsigned char* buffer, int bufLen); + /** * Extract ephemeris data from 1045 and 1046 RTCM3 messages. * @param buffer the buffer containing an 1045 and 1046 RTCM block Index: branches/BNC_2.12/src/RTCM3/clock_and_orbit/clock_orbit_rtcm.h =================================================================== --- branches/BNC_2.12/src/RTCM3/clock_and_orbit/clock_orbit_rtcm.h (revision 8804) +++ branches/BNC_2.12/src/RTCM3/clock_and_orbit/clock_orbit_rtcm.h (revision 8805) @@ -108,5 +108,5 @@ CLOCKORBIT_NUMQZSS=10, CLOCKORBIT_NUMSBAS=38, - CLOCKORBIT_NUMBDS=37, + CLOCKORBIT_NUMBDS=63, CLOCKORBIT_NUMBIAS=100, CLOCKORBIT_NUMIONOLAYERS=4, @@ -236,5 +236,8 @@ CODETYPE_BDS_B2a_D = 12, CODETYPE_BDS_B2a_P = 13, - CODETYPE_BDS_B2a_DP = 14 + CODETYPE_BDS_B2a_DP = 14, + + CODETYPE_IRNSS_S_SPS = 8, + CODETYPE_IRNSS_L5_SPS = 22 }; Index: branches/BNC_2.12/src/bncutils.cpp =================================================================== --- branches/BNC_2.12/src/bncutils.cpp (revision 8804) +++ branches/BNC_2.12/src/bncutils.cpp (revision 8805) @@ -173,5 +173,4 @@ } } - } @@ -463,5 +462,5 @@ // //////////////////////////////////////////////////////////////////////////// -int factorial(int n) { +double factorial(int n) { if (n == 0) { return 1; @@ -809,45 +808,64 @@ double accuracyFromIndex(int index, t_eph::e_type type) { - - if (type == t_eph::GPS || type == t_eph::BDS || type == t_eph::SBAS - || type == t_eph::QZSS) { - +double accuracy = -1.0; + + if (type == t_eph::GPS || + type == t_eph::BDS || + type == t_eph::SBAS|| + type == t_eph::QZSS) { if ((index >= 0) && (index <= 6)) { if (index == 3) { - return ceil(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0; + accuracy = ceil(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0; } else { - return floor(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0; + accuracy = floor(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0; } } else if ((index > 6) && (index <= 15)) { - return (10.0 * pow(2.0, (double(index) - 2.0))) / 10.0; + accuracy = (10.0 * pow(2.0, (double(index) - 2.0))) / 10.0; } else { - return 8192.0; - } - } - - if (type == t_eph::Galileo) { - + accuracy = 8192.0; + } + } + else if (type == t_eph::Galileo) { if ((index >= 0) && (index <= 49)) { - return (double(index) / 100.0); + accuracy = (double(index) / 100.0); } else if ((index > 49) && (index <= 74)) { - return (50.0 + (double(index) - 50.0) * 2.0) / 100.0; + accuracy = (50.0 + (double(index) - 50.0) * 2.0) / 100.0; } else if ((index > 74) && (index <= 99)) { - return 1.0 + (double(index) - 75.0) * 0.04; + accuracy = 1.0 + (double(index) - 75.0) * 0.04; } else if ((index > 99) && (index <= 125)) { - return 2.0 + (double(index) - 100.0) * 0.16; + accuracy = 2.0 + (double(index) - 100.0) * 0.16; } else { - return -1.0; - } - } - - return double(index); -} + accuracy = -1.0; + } + } + else if (type == t_eph::IRNSS) { + if ((index >= 0) && (index <= 6)) { + if (index == 1) { + accuracy = 2.8; + } + else if (index == 3) { + accuracy = 5.7; + } + else if (index == 5) { + accuracy = 11.3; + } + else { + accuracy = pow(2, 1 + index / 2); + } + } + else if ((index > 6) && (index <= 15)) { + accuracy = pow(2, index - 2); + } + } + return accuracy; +} + int indexFromAccuracy(double accuracy, t_eph::e_type type) { Index: branches/BNC_2.12/src/bncutils.h =================================================================== --- branches/BNC_2.12/src/bncutils.h (revision 8804) +++ branches/BNC_2.12/src/bncutils.h (revision 8805) @@ -140,5 +140,5 @@ double associatedLegendreFunction(int n, int m, double t); -int factorial(int n); +double factorial(int n); /** Convert RTCM3 lock-time indicator to lock time in seconds