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