| [5852] | 1 |
|
|---|
| 2 | #include "ephEncoder.h"
|
|---|
| [6139] | 3 |
|
|---|
| [5852] | 4 | using namespace std;
|
|---|
| 5 |
|
|---|
| 6 |
|
|---|
| 7 | // build up RTCM3 for GPS
|
|---|
| 8 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 9 | int t_ephEncoder::RTCM3(const t_ephGPS& eph, unsigned char *buffer) {
|
|---|
| 10 |
|
|---|
| 11 | unsigned char *startbuffer = buffer;
|
|---|
| 12 | buffer= buffer+3;
|
|---|
| 13 | int size = 0;
|
|---|
| 14 | int numbits = 0;
|
|---|
| 15 | unsigned long long bitbuffer = 0;
|
|---|
| [10587] | 16 | int ura = indexFromAccuracy(eph._ura, eph.system());
|
|---|
| [10215] | 17 | int L5Flag = 0;
|
|---|
| 18 | int SFlag = 0;
|
|---|
| 19 | int fitIntervalFlag = 0;
|
|---|
| [8812] | 20 |
|
|---|
| [10587] | 21 | if (eph.system() == t_eph::QZSS) {
|
|---|
| [6814] | 22 | GPSADDBITS(12, 1044)
|
|---|
| [8812] | 23 | GPSADDBITS(4, eph._prn.number())
|
|---|
| [6819] | 24 | GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
|
|---|
| [8812] | 25 | GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<25))
|
|---|
| 26 | GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 27 | GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [10903] | 28 | GPSADDBITS(8, static_cast<int>(eph._IODE))
|
|---|
| [6819] | 29 | GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
|
|---|
| [8812] | 30 | GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| [6819] | 31 | GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 32 | GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
|
|---|
| 33 | GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
|---|
| 34 | GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
|
|---|
| 35 | GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
|
|---|
| 36 | GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
|
|---|
| 37 | GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
|
|---|
| [8812] | 38 | GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [6819] | 39 | GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
|
|---|
| 40 | GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 41 | GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
|
|---|
| [8812] | 42 | GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 43 | GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 44 | GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| [6819] | 45 | GPSADDBITS(2,eph._L2Codes)
|
|---|
| 46 | GPSADDBITS(10, eph._TOC.gpsw())
|
|---|
| 47 | GPSADDBITS(4, ura)
|
|---|
| 48 | GPSADDBITS(6, eph._health)
|
|---|
| 49 | GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [8853] | 50 | GPSADDBITS(10, eph._IODC)
|
|---|
| [8852] | 51 | (eph._fitInterval == 2.0) ? fitIntervalFlag = 0 : fitIntervalFlag = 1;
|
|---|
| 52 | GPSADDBITS(1, fitIntervalFlag)
|
|---|
| [6819] | 53 | GPSADDBITS(3, 0) /* padding */
|
|---|
| [10619] | 54 | } else if (eph.system() == t_eph::NavIC) {
|
|---|
| [9002] | 55 | GPSADDBITS(12, 1041)
|
|---|
| [8812] | 56 | GPSADDBITS(6, eph._prn.number())
|
|---|
| 57 | GPSADDBITS(10, eph._TOC.gpsw())
|
|---|
| 58 | GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 59 | GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 60 | GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<25))
|
|---|
| 61 | GPSADDBITS(4, ura)
|
|---|
| 62 | GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
|
|---|
| 63 | GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 64 | GPSADDBITSFLOAT(22, eph._Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<11))
|
|---|
| [10903] | 65 | GPSADDBITS(8, static_cast<int>(eph._IODE)) // Issue Of Data Ephemeris and Clock
|
|---|
| [8812] | 66 | GPSADDBITS(10, 0) /* padding */
|
|---|
| 67 | if (eph._health == 0.0) {
|
|---|
| 68 | L5Flag = 0;
|
|---|
| 69 | SFlag = 0;
|
|---|
| 70 | } else if (eph._health == 1.0) {
|
|---|
| 71 | L5Flag = 0;
|
|---|
| 72 | SFlag = 1;
|
|---|
| 73 | } else if (eph._health == 2.0) {
|
|---|
| 74 | L5Flag = 1;
|
|---|
| 75 | SFlag = 0;
|
|---|
| 76 | } else if (eph._health == 3.0) {
|
|---|
| 77 | L5Flag = 1;
|
|---|
| 78 | SFlag = 1;
|
|---|
| 79 | }
|
|---|
| 80 | GPSADDBITS(1, L5Flag)
|
|---|
| 81 | GPSADDBITS(1, SFlag)
|
|---|
| 82 | GPSADDBITSFLOAT(15, eph._Cuc, 1.0/static_cast<double>(1<<28))
|
|---|
| 83 | GPSADDBITSFLOAT(15, eph._Cus, 1.0/static_cast<double>(1<<28))
|
|---|
| 84 | GPSADDBITSFLOAT(15, eph._Cic, 1.0/static_cast<double>(1<<28))
|
|---|
| 85 | GPSADDBITSFLOAT(15, eph._Cis, 1.0/static_cast<double>(1<<28))
|
|---|
| 86 | GPSADDBITSFLOAT(15, eph._Crc, 1.0/static_cast<double>(1<< 4))
|
|---|
| 87 | GPSADDBITSFLOAT(15, eph._Crs, 1.0/static_cast<double>(1<< 4))
|
|---|
| 88 | GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 89 | GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 90 | GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
|
|---|
| 91 | GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
|---|
| 92 | GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
|
|---|
| 93 | GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 94 | GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 95 | GPSADDBITSFLOAT(22, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<11))
|
|---|
| 96 | GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [10903] | 97 | GPSADDBITS(2, static_cast<int>(eph._s_bits_after_IDOT))
|
|---|
| 98 | GPSADDBITS(2, static_cast<int>(eph._s_bits_after_i0))
|
|---|
| 99 | GPSADDBITS(6, 0) /* padding */
|
|---|
| [8812] | 100 | } else {
|
|---|
| [6814] | 101 | GPSADDBITS(12, 1019)
|
|---|
| [6819] | 102 | GPSADDBITS(6,eph._prn.number())
|
|---|
| 103 | GPSADDBITS(10, eph._TOC.gpsw())
|
|---|
| 104 | GPSADDBITS(4, ura)
|
|---|
| 105 | GPSADDBITS(2,eph._L2Codes)
|
|---|
| [8812] | 106 | GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| [10903] | 107 | GPSADDBITS(8, static_cast<int>(eph._IODE))
|
|---|
| [6819] | 108 | GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
|
|---|
| [8812] | 109 | GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<25))
|
|---|
| 110 | GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 111 | GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [6819] | 112 | GPSADDBITS(10, eph._IODC)
|
|---|
| 113 | GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
|
|---|
| [8812] | 114 | GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| [6819] | 115 | GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 116 | GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
|
|---|
| 117 | GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
|---|
| 118 | GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
|
|---|
| 119 | GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
|
|---|
| 120 | GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
|
|---|
| 121 | GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
|
|---|
| [8812] | 122 | GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [6819] | 123 | GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
|
|---|
| 124 | GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 125 | GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
|
|---|
| [8812] | 126 | GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 127 | GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| [6819] | 128 | GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [7844] | 129 | GPSADDBITS(6, eph._health)
|
|---|
| [6819] | 130 | GPSADDBITS(1, eph._L2PFlag)
|
|---|
| [8852] | 131 | (eph._fitInterval == 4.0) ? fitIntervalFlag = 0 : fitIntervalFlag = 1;
|
|---|
| 132 | GPSADDBITS(1, fitIntervalFlag)
|
|---|
| [6814] | 133 | }
|
|---|
| [5852] | 134 |
|
|---|
| 135 | startbuffer[0]=0xD3;
|
|---|
| 136 | startbuffer[1]=(size >> 8);
|
|---|
| 137 | startbuffer[2]=size;
|
|---|
| 138 | unsigned long i = CRC24(size+3, startbuffer);
|
|---|
| 139 | buffer[size++] = i >> 16;
|
|---|
| 140 | buffer[size++] = i >> 8;
|
|---|
| 141 | buffer[size++] = i;
|
|---|
| 142 | size += 3;
|
|---|
| 143 | return size;
|
|---|
| 144 | }
|
|---|
| 145 |
|
|---|
| 146 | // build up RTCM3 for GLONASS
|
|---|
| 147 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 148 | int t_ephEncoder::RTCM3(const t_ephGlo& eph, unsigned char *buffer)
|
|---|
| 149 | {
|
|---|
| 150 |
|
|---|
| 151 | int size = 0;
|
|---|
| 152 | int numbits = 0;
|
|---|
| 153 | long long bitbuffer = 0;
|
|---|
| 154 | unsigned char *startbuffer = buffer;
|
|---|
| 155 | buffer= buffer+3;
|
|---|
| 156 |
|
|---|
| 157 | GLONASSADDBITS(12, 1020)
|
|---|
| [5853] | 158 | GLONASSADDBITS(6, eph._prn.number())
|
|---|
| [10587] | 159 | GLONASSADDBITS(5, 7+eph._frq_num)
|
|---|
| [8182] | 160 | GLONASSADDBITS(1, eph._almanac_health)
|
|---|
| 161 | GLONASSADDBITS(1, eph._almanac_health_availablility_indicator)
|
|---|
| 162 | GLONASSADDBITS(2, eph._P1)
|
|---|
| [7844] | 163 | int tki = static_cast<int>(eph._tki)+3*60*60;
|
|---|
| [10559] | 164 | if (tki >= 86400) {tki -= 86400;}
|
|---|
| 165 | GLONASSADDBITS(5, static_cast<int>(tki)/(60*60))
|
|---|
| [7849] | 166 | GLONASSADDBITS(6, (static_cast<int>(tki)/60)%60)
|
|---|
| 167 | GLONASSADDBITS(1, (static_cast<int>(tki)/30)%30)
|
|---|
| [7844] | 168 | GLONASSADDBITS(1, eph._health)
|
|---|
| [8182] | 169 | GLONASSADDBITS(1, eph._P2)
|
|---|
| [5853] | 170 | unsigned long long timeofday = (static_cast<int>(eph._tt.gpssec()+3*60*60-eph._gps_utc)%86400);
|
|---|
| [5852] | 171 | GLONASSADDBITS(7, timeofday/60/15)
|
|---|
| [10587] | 172 | GLONASSADDBITSFLOATM(24, eph._x_vel*1000, 1000.0/static_cast<double>(1<<20))
|
|---|
| [5853] | 173 | GLONASSADDBITSFLOATM(27, eph._x_pos*1000, 1000.0/static_cast<double>(1<<11))
|
|---|
| [10587] | 174 | GLONASSADDBITSFLOATM(5, eph._x_acc*1000, 1000.0/static_cast<double>(1<<30))
|
|---|
| 175 | GLONASSADDBITSFLOATM(24, eph._y_vel*1000, 1000.0/static_cast<double>(1<<20))
|
|---|
| [5853] | 176 | GLONASSADDBITSFLOATM(27, eph._y_pos*1000, 1000.0/static_cast<double>(1<<11))
|
|---|
| [10587] | 177 | GLONASSADDBITSFLOATM(5, eph._y_acc*1000, 1000.0/static_cast<double>(1<<30))
|
|---|
| 178 | GLONASSADDBITSFLOATM(24, eph._z_vel*1000, 1000.0/static_cast<double>(1<<20))
|
|---|
| [5853] | 179 | GLONASSADDBITSFLOATM(27,eph._z_pos*1000, 1000.0/static_cast<double>(1<<11))
|
|---|
| [10587] | 180 | GLONASSADDBITSFLOATM(5, eph._z_acc*1000, 1000.0/static_cast<double>(1<<30))
|
|---|
| [8182] | 181 | GLONASSADDBITS(1, eph._P3)
|
|---|
| 182 | GLONASSADDBITSFLOATM(11, eph._gamma, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
|
|---|
| 183 | GLONASSADDBITS(2, eph._M_P) /* GLONASS-M P */
|
|---|
| 184 | GLONASSADDBITS(1, eph._M_l3) /* GLONASS-M ln(3) */
|
|---|
| [5853] | 185 | GLONASSADDBITSFLOATM(22, eph._tau, 1.0/static_cast<double>(1<<30))
|
|---|
| [8182] | 186 | GLONASSADDBITSFLOATM(5, eph._M_delta_tau, 1.0/static_cast<double>(1<<30)) /* GLONASS-M delta tau */
|
|---|
| [5853] | 187 | GLONASSADDBITS(5, eph._E)
|
|---|
| [8182] | 188 | GLONASSADDBITS(1, eph._M_P4) /* GLONASS-M P4 */
|
|---|
| 189 | GLONASSADDBITS(4, eph._M_FT) /* GLONASS-M FT */
|
|---|
| 190 | GLONASSADDBITS(11, eph._M_NT) /* GLONASS-M NT */
|
|---|
| 191 | GLONASSADDBITS(2, eph._M_M) /* GLONASS-M active? */
|
|---|
| 192 | GLONASSADDBITS(1, eph._additional_data_availability) /* GLONASS additional data */
|
|---|
| [10587] | 193 | GLONASSADDBITS(11, eph._M_NA) /* GLONASS NA */
|
|---|
| [8182] | 194 | GLONASSADDBITSFLOATM(32, eph._tauC, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1)) /* GLONASS tau C */
|
|---|
| 195 | GLONASSADDBITS(5, eph._M_N4) /* GLONASS-M N4 */
|
|---|
| 196 | GLONASSADDBITSFLOATM(22, eph._M_tau_GPS, 1.0/static_cast<double>(1<<30)) /* GLONASS-M tau GPS */
|
|---|
| 197 | GLONASSADDBITS(1, eph._M_l5) /* GLONASS-M ln(5) */
|
|---|
| [5852] | 198 | GLONASSADDBITS(7, 0) /* Reserved */
|
|---|
| 199 |
|
|---|
| 200 | startbuffer[0]=0xD3;
|
|---|
| 201 | startbuffer[1]=(size >> 8);
|
|---|
| 202 | startbuffer[2]=size;
|
|---|
| 203 | unsigned long i = CRC24(size+3, startbuffer);
|
|---|
| 204 | buffer[size++] = i >> 16;
|
|---|
| 205 | buffer[size++] = i >> 8;
|
|---|
| 206 | buffer[size++] = i;
|
|---|
| 207 | size += 3;
|
|---|
| 208 | return size;
|
|---|
| 209 | }
|
|---|
| 210 |
|
|---|
| 211 | // build up RTCM3 for Galileo
|
|---|
| 212 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 213 | int t_ephEncoder::RTCM3(const t_ephGal& eph, unsigned char *buffer) {
|
|---|
| 214 | int size = 0;
|
|---|
| 215 | int numbits = 0;
|
|---|
| 216 | long long bitbuffer = 0;
|
|---|
| 217 | unsigned char *startbuffer = buffer;
|
|---|
| 218 | buffer= buffer+3;
|
|---|
| 219 |
|
|---|
| [10587] | 220 | int SISA = indexFromAccuracy(eph._SISA, eph.system());
|
|---|
| [6799] | 221 |
|
|---|
| [6812] | 222 | GALILEOADDBITS(12, eph._inav ? 1046 : 1045)
|
|---|
| [5853] | 223 | GALILEOADDBITS(6, eph._prn.number())
|
|---|
| [6869] | 224 | GALILEOADDBITS(12, eph._TOC.galw())
|
|---|
| [5853] | 225 | GALILEOADDBITS(10, eph._IODnav)
|
|---|
| [6819] | 226 | GALILEOADDBITS(8, SISA)
|
|---|
| [8812] | 227 | GALILEOADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| [5853] | 228 | GALILEOADDBITS(14, eph._TOC.gpssec()/60)
|
|---|
| [8812] | 229 | GALILEOADDBITSFLOAT(6, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<29))
|
|---|
| 230 | GALILEOADDBITSFLOAT(21, eph._clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<16))
|
|---|
| 231 | GALILEOADDBITSFLOAT(31, eph._clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<4))
|
|---|
| [5853] | 232 | GALILEOADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
|
|---|
| [8812] | 233 | GALILEOADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| [5853] | 234 | GALILEOADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 235 | GALILEOADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
|
|---|
| 236 | GALILEOADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
|---|
| 237 | GALILEOADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
|
|---|
| 238 | GALILEOADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
|
|---|
| 239 | GALILEOADDBITS(14, eph._TOEsec/60)
|
|---|
| 240 | GALILEOADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
|
|---|
| [8812] | 241 | GALILEOADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [5853] | 242 | GALILEOADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
|
|---|
| 243 | GALILEOADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 244 | GALILEOADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
|
|---|
| [8812] | 245 | GALILEOADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 246 | GALILEOADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 247 | GALILEOADDBITSFLOAT(10, eph._BGD_1_5A, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<2))
|
|---|
| [8813] | 248 | if(eph._inav) {
|
|---|
| [8812] | 249 | GALILEOADDBITSFLOAT(10, eph._BGD_1_5B, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<2))
|
|---|
| [10587] | 250 | GALILEOADDBITS(2, static_cast<int>(eph._E5b_HS))
|
|---|
| 251 | GALILEOADDBITS(1, eph._E5b_DataInvalid ? 1 : 0)
|
|---|
| 252 | GALILEOADDBITS(2, static_cast<int>(eph._E1B_HS))
|
|---|
| 253 | GALILEOADDBITS(1, eph._E1B_DataInvalid ? 1 : 0)
|
|---|
| [8812] | 254 | } else {
|
|---|
| [10587] | 255 | GALILEOADDBITS(2, static_cast<int>(eph._E5a_HS))
|
|---|
| 256 | GALILEOADDBITS(1, eph._E5a_DataInvalid ? 1 : 0)
|
|---|
| [5852] | 257 | }
|
|---|
| 258 |
|
|---|
| [6818] | 259 | GALILEOADDBITS((eph._inav ? 2 : 7), 0)
|
|---|
| [5852] | 260 |
|
|---|
| 261 | startbuffer[0]=0xD3;
|
|---|
| 262 | startbuffer[1]=(size >> 8);
|
|---|
| 263 | startbuffer[2]=size;
|
|---|
| 264 | unsigned long i = CRC24(size+3, startbuffer);
|
|---|
| 265 | buffer[size++] = i >> 16;
|
|---|
| 266 | buffer[size++] = i >> 8;
|
|---|
| 267 | buffer[size++] = i;
|
|---|
| 268 | size += 3;
|
|---|
| 269 | return size;
|
|---|
| 270 | }
|
|---|
| 271 |
|
|---|
| [6615] | 272 | // build up RTCM3 for SBAS
|
|---|
| 273 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| [6631] | 274 | int t_ephEncoder::RTCM3(const t_ephSBAS& eph, unsigned char* buffer) {
|
|---|
| 275 | int size = 0;
|
|---|
| 276 | int numbits = 0;
|
|---|
| 277 | long long bitbuffer = 0;
|
|---|
| 278 | unsigned char *startbuffer = buffer;
|
|---|
| 279 | buffer= buffer+3;
|
|---|
| 280 |
|
|---|
| [10587] | 281 | int ura = indexFromAccuracy(eph._ura, eph.system());
|
|---|
| [6631] | 282 | SBASADDBITS(12, 1043)
|
|---|
| 283 | SBASADDBITS(6, eph._prn.number()-20)
|
|---|
| 284 | SBASADDBITS(8, eph._IODN)
|
|---|
| 285 | SBASADDBITS(13, static_cast<int>(eph._TOC.daysec())>>4)
|
|---|
| [6819] | 286 | SBASADDBITS(4, ura)
|
|---|
| [6631] | 287 | SBASADDBITSFLOAT(30, eph._x_pos, 0.08)
|
|---|
| 288 | SBASADDBITSFLOAT(30, eph._y_pos, 0.08)
|
|---|
| 289 | SBASADDBITSFLOAT(25, eph._z_pos, 0.4)
|
|---|
| [10587] | 290 | SBASADDBITSFLOAT(17, eph._x_vel, 0.000625)
|
|---|
| 291 | SBASADDBITSFLOAT(17, eph._y_vel, 0.000625)
|
|---|
| 292 | SBASADDBITSFLOAT(18, eph._z_vel, 0.004)
|
|---|
| 293 | SBASADDBITSFLOAT(10, eph._x_acc, 0.0000125)
|
|---|
| 294 | SBASADDBITSFLOAT(10, eph._y_acc, 0.0000125)
|
|---|
| 295 | SBASADDBITSFLOAT(10, eph._z_acc, 0.0000625)
|
|---|
| [6631] | 296 | SBASADDBITSFLOAT(12, eph._agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 297 | SBASADDBITSFLOAT(8, eph._agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
|
|---|
| [6635] | 298 | SBASADDBITS(2,0);
|
|---|
| [6631] | 299 |
|
|---|
| 300 | startbuffer[0]=0xD3;
|
|---|
| 301 | startbuffer[1]=(size >> 8);
|
|---|
| 302 | startbuffer[2]=size;
|
|---|
| 303 | unsigned long i = CRC24(size+3, startbuffer);
|
|---|
| 304 | buffer[size++] = i >> 16;
|
|---|
| 305 | buffer[size++] = i >> 8;
|
|---|
| 306 | buffer[size++] = i;
|
|---|
| 307 | size += 3;
|
|---|
| 308 | return size;
|
|---|
| [6403] | 309 | }
|
|---|
| 310 |
|
|---|
| [6615] | 311 | // build up RTCM3 for BDS
|
|---|
| 312 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 313 | int t_ephEncoder::RTCM3(const t_ephBDS& eph, unsigned char* buffer) {
|
|---|
| 314 | int size = 0;
|
|---|
| 315 | int numbits = 0;
|
|---|
| 316 | long long bitbuffer = 0;
|
|---|
| 317 | unsigned char *startbuffer = buffer;
|
|---|
| 318 | buffer= buffer+3;
|
|---|
| 319 |
|
|---|
| [10587] | 320 | int URAI = indexFromAccuracy(eph._ura, eph.system());
|
|---|
| [9000] | 321 | BDSADDBITS(12, 1042)
|
|---|
| [6615] | 322 | BDSADDBITS(6, eph._prn.number())
|
|---|
| [8208] | 323 | BDSADDBITS(13, eph._TOC.bdsw())
|
|---|
| [10587] | 324 | BDSADDBITS(4, URAI);
|
|---|
| [6615] | 325 | BDSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 326 | BDSADDBITS(5, eph._AODE)
|
|---|
| [6812] | 327 | BDSADDBITS(17, static_cast<int>(eph._TOC.bdssec())>>3)
|
|---|
| [8812] | 328 | BDSADDBITSFLOAT(11, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<30)/static_cast<double>(1<<6))
|
|---|
| [6615] | 329 | BDSADDBITSFLOAT(22, eph._clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<20))
|
|---|
| 330 | BDSADDBITSFLOAT(24, eph._clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
|---|
| 331 | BDSADDBITS(5, eph._AODC)
|
|---|
| 332 | BDSADDBITSFLOAT(18, eph._Crs, 1.0/static_cast<double>(1<<6))
|
|---|
| 333 | BDSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 334 | BDSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 335 | BDSADDBITSFLOAT(18, eph._Cuc, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 336 | BDSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
|---|
| 337 | BDSADDBITSFLOAT(18, eph._Cus, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 338 | BDSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
|
|---|
| [6812] | 339 | BDSADDBITS(17, static_cast<int>(eph._TOE.bdssec())>>3)
|
|---|
| [6615] | 340 | BDSADDBITSFLOAT(18, eph._Cic, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 341 | BDSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 342 | BDSADDBITSFLOAT(18, eph._Cis, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 343 | BDSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| [6619] | 344 | BDSADDBITSFLOAT(18, eph._Crc, 1.0/static_cast<double>(1<<6))
|
|---|
| [6615] | 345 | BDSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
|---|
| 346 | BDSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
|---|
| 347 | BDSADDBITSFLOAT(10, eph._TGD1, 0.0000000001)
|
|---|
| 348 | BDSADDBITSFLOAT(10, eph._TGD2, 0.0000000001)
|
|---|
| 349 | BDSADDBITS(1, eph._SatH1)
|
|---|
| [6627] | 350 | BDSADDBITS(1, 0) /* reserved bit, fill up 8 bits */
|
|---|
| [6615] | 351 |
|
|---|
| 352 | startbuffer[0]=0xD3;
|
|---|
| 353 | startbuffer[1]=(size >> 8);
|
|---|
| 354 | startbuffer[2]=size;
|
|---|
| 355 | unsigned long i = CRC24(size+3, startbuffer);
|
|---|
| 356 | buffer[size++] = i >> 16;
|
|---|
| 357 | buffer[size++] = i >> 8;
|
|---|
| 358 | buffer[size++] = i;
|
|---|
| 359 | size += 3;
|
|---|
| 360 | return size;
|
|---|
| [6403] | 361 | }
|
|---|
| [7053] | 362 |
|
|---|