[5852] | 1 |
|
---|
| 2 | #include "ephEncoder.h"
|
---|
[6139] | 3 |
|
---|
| 4 | extern "C" {
|
---|
| 5 | # include "rtcm3torinex.h"
|
---|
| 6 | }
|
---|
| 7 |
|
---|
[5852] | 8 | using namespace std;
|
---|
| 9 |
|
---|
| 10 | // Returns CRC24
|
---|
| 11 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 12 | static unsigned long CRC24(long size, const unsigned char *buf) {
|
---|
| 13 | unsigned long crc = 0;
|
---|
| 14 | int ii;
|
---|
| 15 | while (size--) {
|
---|
| 16 | crc ^= (*buf++) << (16);
|
---|
| 17 | for(ii = 0; ii < 8; ii++) {
|
---|
| 18 | crc <<= 1;
|
---|
| 19 | if (crc & 0x1000000) {
|
---|
| 20 | crc ^= 0x01864cfb;
|
---|
| 21 | }
|
---|
| 22 | }
|
---|
| 23 | }
|
---|
| 24 | return crc;
|
---|
| 25 | }
|
---|
| 26 |
|
---|
| 27 | // build up RTCM3 for GPS
|
---|
| 28 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 29 | #define GPSTOINT(type, value) static_cast<type>(round(value))
|
---|
| 30 |
|
---|
| 31 | #define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
---|
| 32 | |(GPSTOINT(long long,b)&((1ULL<<a)-1)); \
|
---|
| 33 | numbits += (a); \
|
---|
| 34 | while(numbits >= 8) { \
|
---|
| 35 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
---|
| 36 |
|
---|
| 37 | #define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \
|
---|
| 38 | GPSADDBITS(a,i)};
|
---|
| 39 |
|
---|
| 40 | int t_ephEncoder::RTCM3(const t_ephGPS& eph, unsigned char *buffer) {
|
---|
| 41 |
|
---|
| 42 | unsigned char *startbuffer = buffer;
|
---|
| 43 | buffer= buffer+3;
|
---|
| 44 | int size = 0;
|
---|
| 45 | int numbits = 0;
|
---|
| 46 | unsigned long long bitbuffer = 0;
|
---|
[6799] | 47 | eph._ura = indexFromAccuracy(eph._ura, eph.type());
|
---|
[5852] | 48 | GPSADDBITS(12, 1019)
|
---|
[6375] | 49 | if (eph._prn.system() == 'J') {
|
---|
| 50 | GPSADDBITS(6,eph._prn.number() + PRN_QZSS_START - 1)
|
---|
| 51 | }
|
---|
| 52 | else {
|
---|
| 53 | GPSADDBITS(6,eph._prn.number())
|
---|
| 54 | }
|
---|
[5852] | 55 | GPSADDBITS(10, eph._TOC.gpsw())
|
---|
| 56 | GPSADDBITS(4, eph._ura)
|
---|
| 57 | GPSADDBITS(2,eph._L2Codes)
|
---|
| 58 | GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
|
---|
| 59 | /static_cast<double>(1<<13))
|
---|
| 60 | GPSADDBITS(8, eph._IODE)
|
---|
| 61 | GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
|
---|
| 62 | GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
|
---|
| 63 | /static_cast<double>(1<<25))
|
---|
| 64 | GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)
|
---|
| 65 | /static_cast<double>(1<<13))
|
---|
| 66 | GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)
|
---|
| 67 | /static_cast<double>(1<<1))
|
---|
| 68 | GPSADDBITS(10, eph._IODC)
|
---|
| 69 | GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
|
---|
| 70 | GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
|
---|
| 71 | /static_cast<double>(1<<13))
|
---|
| 72 | GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 73 | GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
|
---|
| 74 | GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
---|
| 75 | GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
|
---|
| 76 | GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
|
---|
| 77 | GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
|
---|
| 78 | GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
|
---|
| 79 | GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
|
---|
| 80 | /static_cast<double>(1<<1))
|
---|
| 81 | GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
|
---|
| 82 | GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 83 | GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
|
---|
| 84 | GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
|
---|
| 85 | /static_cast<double>(1<<1))
|
---|
| 86 | GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
|
---|
| 87 | /static_cast<double>(1<<13))
|
---|
| 88 | GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 89 | GPSADDBITS(6, eph._health)
|
---|
| 90 | GPSADDBITS(1, eph._L2PFlag)
|
---|
| 91 | GPSADDBITS(1, 0) /* GPS fit interval */
|
---|
| 92 |
|
---|
| 93 | startbuffer[0]=0xD3;
|
---|
| 94 | startbuffer[1]=(size >> 8);
|
---|
| 95 | startbuffer[2]=size;
|
---|
| 96 | unsigned long i = CRC24(size+3, startbuffer);
|
---|
| 97 | buffer[size++] = i >> 16;
|
---|
| 98 | buffer[size++] = i >> 8;
|
---|
| 99 | buffer[size++] = i;
|
---|
| 100 | size += 3;
|
---|
| 101 | return size;
|
---|
| 102 | }
|
---|
| 103 |
|
---|
| 104 | // build up RTCM3 for GLONASS
|
---|
| 105 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 106 | #define GLONASSTOINT(type, value) static_cast<type>(round(value))
|
---|
| 107 |
|
---|
| 108 | #define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
---|
| 109 | |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \
|
---|
| 110 | numbits += (a); \
|
---|
| 111 | while(numbits >= 8) { \
|
---|
| 112 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
---|
| 113 | #define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \
|
---|
| 114 | if(b < 0.0) \
|
---|
| 115 | { \
|
---|
| 116 | s = 1; \
|
---|
| 117 | i = GLONASSTOINT(long long,(-b)/(c)); \
|
---|
| 118 | if(!i) s = 0; \
|
---|
| 119 | } \
|
---|
| 120 | else \
|
---|
| 121 | { \
|
---|
| 122 | s = 0; \
|
---|
| 123 | i = GLONASSTOINT(long long,(b)/(c)); \
|
---|
| 124 | } \
|
---|
| 125 | GLONASSADDBITS(1,s) \
|
---|
| 126 | GLONASSADDBITS(a-1,i)}
|
---|
| 127 |
|
---|
| 128 | int t_ephEncoder::RTCM3(const t_ephGlo& eph, unsigned char *buffer)
|
---|
| 129 | {
|
---|
| 130 |
|
---|
| 131 | int size = 0;
|
---|
| 132 | int numbits = 0;
|
---|
| 133 | long long bitbuffer = 0;
|
---|
| 134 | unsigned char *startbuffer = buffer;
|
---|
| 135 | buffer= buffer+3;
|
---|
| 136 |
|
---|
| 137 | GLONASSADDBITS(12, 1020)
|
---|
[5853] | 138 | GLONASSADDBITS(6, eph._prn.number())
|
---|
| 139 | GLONASSADDBITS(5, 7+eph._frequency_number)
|
---|
[5852] | 140 | GLONASSADDBITS(1, 0)
|
---|
| 141 | GLONASSADDBITS(1, 0)
|
---|
| 142 | GLONASSADDBITS(2, 0)
|
---|
[5853] | 143 | eph._tki=eph._tki+3*60*60;
|
---|
| 144 | GLONASSADDBITS(5, static_cast<int>(eph._tki)/(60*60))
|
---|
| 145 | GLONASSADDBITS(6, (static_cast<int>(eph._tki)/60)%60)
|
---|
| 146 | GLONASSADDBITS(1, (static_cast<int>(eph._tki)/30)%30)
|
---|
| 147 | GLONASSADDBITS(1, eph._health)
|
---|
[5852] | 148 | GLONASSADDBITS(1, 0)
|
---|
[5853] | 149 | unsigned long long timeofday = (static_cast<int>(eph._tt.gpssec()+3*60*60-eph._gps_utc)%86400);
|
---|
[5852] | 150 | GLONASSADDBITS(7, timeofday/60/15)
|
---|
[5853] | 151 | GLONASSADDBITSFLOATM(24, eph._x_velocity*1000, 1000.0/static_cast<double>(1<<20))
|
---|
| 152 | GLONASSADDBITSFLOATM(27, eph._x_pos*1000, 1000.0/static_cast<double>(1<<11))
|
---|
| 153 | GLONASSADDBITSFLOATM(5, eph._x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
|
---|
| 154 | GLONASSADDBITSFLOATM(24, eph._y_velocity*1000, 1000.0/static_cast<double>(1<<20))
|
---|
| 155 | GLONASSADDBITSFLOATM(27, eph._y_pos*1000, 1000.0/static_cast<double>(1<<11))
|
---|
| 156 | GLONASSADDBITSFLOATM(5, eph._y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
|
---|
| 157 | GLONASSADDBITSFLOATM(24, eph._z_velocity*1000, 1000.0/static_cast<double>(1<<20))
|
---|
| 158 | GLONASSADDBITSFLOATM(27,eph._z_pos*1000, 1000.0/static_cast<double>(1<<11))
|
---|
| 159 | GLONASSADDBITSFLOATM(5, eph._z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
|
---|
[5852] | 160 | GLONASSADDBITS(1, 0)
|
---|
[5853] | 161 | GLONASSADDBITSFLOATM(11, eph._gamma, 1.0/static_cast<double>(1<<30)
|
---|
[5852] | 162 | /static_cast<double>(1<<10))
|
---|
| 163 | GLONASSADDBITS(2, 0) /* GLONASS-M P */
|
---|
| 164 | GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
|
---|
[5853] | 165 | GLONASSADDBITSFLOATM(22, eph._tau, 1.0/static_cast<double>(1<<30))
|
---|
[5852] | 166 | GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
|
---|
[5853] | 167 | GLONASSADDBITS(5, eph._E)
|
---|
[5852] | 168 | GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
|
---|
| 169 | GLONASSADDBITS(4, 0) /* GLONASS-M FT */
|
---|
| 170 | GLONASSADDBITS(11, 0) /* GLONASS-M NT */
|
---|
| 171 | GLONASSADDBITS(2, 0) /* GLONASS-M active? */
|
---|
| 172 | GLONASSADDBITS(1, 0) /* GLONASS additional data */
|
---|
| 173 | GLONASSADDBITS(11, 0) /* GLONASS NA */
|
---|
| 174 | GLONASSADDBITS(32, 0) /* GLONASS tau C */
|
---|
| 175 | GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
|
---|
| 176 | GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
|
---|
| 177 | GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
|
---|
| 178 | GLONASSADDBITS(7, 0) /* Reserved */
|
---|
| 179 |
|
---|
| 180 | startbuffer[0]=0xD3;
|
---|
| 181 | startbuffer[1]=(size >> 8);
|
---|
| 182 | startbuffer[2]=size;
|
---|
| 183 | unsigned long i = CRC24(size+3, startbuffer);
|
---|
| 184 | buffer[size++] = i >> 16;
|
---|
| 185 | buffer[size++] = i >> 8;
|
---|
| 186 | buffer[size++] = i;
|
---|
| 187 | size += 3;
|
---|
| 188 | return size;
|
---|
| 189 | }
|
---|
| 190 |
|
---|
| 191 | // build up RTCM3 for Galileo
|
---|
| 192 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 193 | #define GALILEOTOINT(type, value) static_cast<type>(round(value))
|
---|
| 194 |
|
---|
| 195 | #define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
---|
| 196 | |(GALILEOTOINT(long long,b)&((1LL<<a)-1)); \
|
---|
| 197 | numbits += (a); \
|
---|
| 198 | while(numbits >= 8) { \
|
---|
| 199 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
---|
| 200 | #define GALILEOADDBITSFLOAT(a,b,c) {long long i = GALILEOTOINT(long long,(b)/(c)); \
|
---|
| 201 | GALILEOADDBITS(a,i)};
|
---|
| 202 |
|
---|
| 203 | int t_ephEncoder::RTCM3(const t_ephGal& eph, unsigned char *buffer) {
|
---|
| 204 | int size = 0;
|
---|
| 205 | int numbits = 0;
|
---|
| 206 | long long bitbuffer = 0;
|
---|
| 207 | unsigned char *startbuffer = buffer;
|
---|
| 208 | buffer= buffer+3;
|
---|
| 209 |
|
---|
[6799] | 210 | eph._SISA = indexFromAccuracy(eph._SISA, eph.type());
|
---|
| 211 |
|
---|
[5853] | 212 | bool inav = ( (eph._flags & GALEPHF_INAV) == GALEPHF_INAV );
|
---|
[5852] | 213 | GALILEOADDBITS(12, inav ? 1046 : 1045)
|
---|
[5853] | 214 | GALILEOADDBITS(6, eph._prn.number())
|
---|
| 215 | GALILEOADDBITS(12, eph._TOC.gpsw())
|
---|
| 216 | GALILEOADDBITS(10, eph._IODnav)
|
---|
| 217 | GALILEOADDBITS(8, eph._SISA)
|
---|
| 218 | GALILEOADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
|
---|
[5852] | 219 | /static_cast<double>(1<<13))
|
---|
[5853] | 220 | GALILEOADDBITS(14, eph._TOC.gpssec()/60)
|
---|
| 221 | GALILEOADDBITSFLOAT(6, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
|
---|
[5852] | 222 | /static_cast<double>(1<<29))
|
---|
[5853] | 223 | GALILEOADDBITSFLOAT(21, eph._clock_drift, 1.0/static_cast<double>(1<<30)
|
---|
[5852] | 224 | /static_cast<double>(1<<16))
|
---|
[5853] | 225 | GALILEOADDBITSFLOAT(31, eph._clock_bias, 1.0/static_cast<double>(1<<30)
|
---|
[5852] | 226 | /static_cast<double>(1<<4))
|
---|
[5853] | 227 | GALILEOADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
|
---|
| 228 | GALILEOADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
|
---|
[5852] | 229 | /static_cast<double>(1<<13))
|
---|
[5853] | 230 | GALILEOADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 231 | GALILEOADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
|
---|
| 232 | GALILEOADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
---|
| 233 | GALILEOADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
|
---|
| 234 | GALILEOADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
|
---|
| 235 | GALILEOADDBITS(14, eph._TOEsec/60)
|
---|
| 236 | GALILEOADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
|
---|
| 237 | GALILEOADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
|
---|
[5852] | 238 | /static_cast<double>(1<<1))
|
---|
[5853] | 239 | GALILEOADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
|
---|
| 240 | GALILEOADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 241 | GALILEOADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
|
---|
| 242 | GALILEOADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
|
---|
[5852] | 243 | /static_cast<double>(1<<1))
|
---|
[5853] | 244 | GALILEOADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
|
---|
[5852] | 245 | /static_cast<double>(1<<13))
|
---|
[5853] | 246 | GALILEOADDBITSFLOAT(10, eph._BGD_1_5A, 1.0/static_cast<double>(1<<30)
|
---|
[5852] | 247 | /static_cast<double>(1<<2))
|
---|
| 248 | if(inav)
|
---|
| 249 | {
|
---|
[5853] | 250 | GALILEOADDBITSFLOAT(10, eph._BGD_1_5B, 1.0/static_cast<double>(1<<30)
|
---|
[5852] | 251 | /static_cast<double>(1<<2))
|
---|
[5853] | 252 | GALILEOADDBITS(2, static_cast<int>(eph._E5bHS))
|
---|
| 253 | GALILEOADDBITS(1, eph._flags & GALEPHF_E5BDINVALID)
|
---|
[5852] | 254 | }
|
---|
| 255 | else
|
---|
| 256 | {
|
---|
[5853] | 257 | GALILEOADDBITS(2, static_cast<int>(eph._E5aHS))
|
---|
| 258 | GALILEOADDBITS(1, eph._flags & GALEPHF_E5ADINVALID)
|
---|
[5852] | 259 | }
|
---|
[5853] | 260 | //// eph._TOEsec = 0.9999E9;
|
---|
| 261 | GALILEOADDBITS(20, eph._TOEsec)
|
---|
[5852] | 262 |
|
---|
[6550] | 263 | GALILEOADDBITS((inav ? 1 : 3), 0)
|
---|
[5852] | 264 |
|
---|
| 265 | startbuffer[0]=0xD3;
|
---|
| 266 | startbuffer[1]=(size >> 8);
|
---|
| 267 | startbuffer[2]=size;
|
---|
| 268 | unsigned long i = CRC24(size+3, startbuffer);
|
---|
| 269 | buffer[size++] = i >> 16;
|
---|
| 270 | buffer[size++] = i >> 8;
|
---|
| 271 | buffer[size++] = i;
|
---|
| 272 | size += 3;
|
---|
| 273 | return size;
|
---|
| 274 | }
|
---|
| 275 |
|
---|
[6615] | 276 | // build up RTCM3 for SBAS
|
---|
| 277 | ////////////////////////////////////////////////////////////////////////////
|
---|
[6631] | 278 | #define SBASTOINT(type, value) static_cast<type>(round(value))
|
---|
[6403] | 279 |
|
---|
[6631] | 280 | #define SBASADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
---|
| 281 | |(SBASTOINT(long long,b)&((1ULL<<a)-1)); \
|
---|
| 282 | numbits += (a); \
|
---|
| 283 | while(numbits >= 8) { \
|
---|
| 284 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
---|
| 285 | #define SBASADDBITSFLOAT(a,b,c) {long long i = SBASTOINT(long long,(b)/(c)); \
|
---|
| 286 | SBASADDBITS(a,i)};
|
---|
| 287 |
|
---|
| 288 | int t_ephEncoder::RTCM3(const t_ephSBAS& eph, unsigned char* buffer) {
|
---|
| 289 | int size = 0;
|
---|
| 290 | int numbits = 0;
|
---|
| 291 | long long bitbuffer = 0;
|
---|
| 292 | unsigned char *startbuffer = buffer;
|
---|
| 293 | buffer= buffer+3;
|
---|
| 294 |
|
---|
[6799] | 295 | eph._ura = indexFromAccuracy(eph._ura, eph.type());
|
---|
[6631] | 296 | SBASADDBITS(12, 1043)
|
---|
| 297 | SBASADDBITS(6, eph._prn.number()-20)
|
---|
| 298 | SBASADDBITS(8, eph._IODN)
|
---|
| 299 | SBASADDBITS(13, static_cast<int>(eph._TOC.daysec())>>4)
|
---|
| 300 | SBASADDBITS(4, eph._ura)
|
---|
| 301 | SBASADDBITSFLOAT(30, eph._x_pos, 0.08)
|
---|
| 302 | SBASADDBITSFLOAT(30, eph._y_pos, 0.08)
|
---|
| 303 | SBASADDBITSFLOAT(25, eph._z_pos, 0.4)
|
---|
| 304 | SBASADDBITSFLOAT(17, eph._x_velocity, 0.000625)
|
---|
| 305 | SBASADDBITSFLOAT(17, eph._y_velocity, 0.000625)
|
---|
| 306 | SBASADDBITSFLOAT(18, eph._z_velocity, 0.004)
|
---|
| 307 | SBASADDBITSFLOAT(10, eph._x_acceleration, 0.0000125)
|
---|
| 308 | SBASADDBITSFLOAT(10, eph._y_acceleration, 0.0000125)
|
---|
| 309 | SBASADDBITSFLOAT(10, eph._z_acceleration, 0.0000625)
|
---|
| 310 | SBASADDBITSFLOAT(12, eph._agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 311 | SBASADDBITSFLOAT(8, eph._agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
|
---|
[6635] | 312 | SBASADDBITS(2,0);
|
---|
[6631] | 313 |
|
---|
| 314 | startbuffer[0]=0xD3;
|
---|
| 315 | startbuffer[1]=(size >> 8);
|
---|
| 316 | startbuffer[2]=size;
|
---|
| 317 | unsigned long i = CRC24(size+3, startbuffer);
|
---|
| 318 | buffer[size++] = i >> 16;
|
---|
| 319 | buffer[size++] = i >> 8;
|
---|
| 320 | buffer[size++] = i;
|
---|
| 321 | size += 3;
|
---|
| 322 | return size;
|
---|
[6403] | 323 | }
|
---|
| 324 |
|
---|
[6615] | 325 | // build up RTCM3 for BDS
|
---|
| 326 | ////////////////////////////////////////////////////////////////////////////
|
---|
| 327 | #define BDSTOINT(type, value) static_cast<type>(round(value))
|
---|
[6403] | 328 |
|
---|
[6615] | 329 | #define BDSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
---|
| 330 | |(BDSTOINT(long long,b)&((1ULL<<a)-1)); \
|
---|
| 331 | numbits += (a); \
|
---|
| 332 | while(numbits >= 8) { \
|
---|
| 333 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
---|
| 334 | #define BDSADDBITSFLOAT(a,b,c) {long long i = BDSTOINT(long long,(b)/(c)); \
|
---|
| 335 | BDSADDBITS(a,i)};
|
---|
| 336 |
|
---|
| 337 | int t_ephEncoder::RTCM3(const t_ephBDS& eph, unsigned char* buffer) {
|
---|
| 338 | int size = 0;
|
---|
| 339 | int numbits = 0;
|
---|
| 340 | long long bitbuffer = 0;
|
---|
| 341 | unsigned char *startbuffer = buffer;
|
---|
| 342 | buffer= buffer+3;
|
---|
| 343 |
|
---|
[6799] | 344 | eph._URA = indexFromAccuracy(eph._URA, eph.type());
|
---|
[6615] | 345 | BDSADDBITS(12, RTCM3ID_BDS)
|
---|
| 346 | BDSADDBITS(6, eph._prn.number())
|
---|
[6619] | 347 | BDSADDBITS(13, eph._TOC_bdt.gpsw() - 1356.0)
|
---|
[6799] | 348 | BDSADDBITS(4, eph._URA);
|
---|
[6615] | 349 | BDSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
---|
| 350 | BDSADDBITS(5, eph._AODE)
|
---|
[6619] | 351 | BDSADDBITS(17, static_cast<int>(eph._TOC_bdt.gpssec())>>3)
|
---|
[6622] | 352 | BDSADDBITSFLOAT(11, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
|
---|
| 353 | /static_cast<double>(1<<30)/static_cast<double>(1<<6))
|
---|
[6615] | 354 | BDSADDBITSFLOAT(22, eph._clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<20))
|
---|
| 355 | BDSADDBITSFLOAT(24, eph._clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
---|
| 356 | BDSADDBITS(5, eph._AODC)
|
---|
| 357 | BDSADDBITSFLOAT(18, eph._Crs, 1.0/static_cast<double>(1<<6))
|
---|
| 358 | BDSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
---|
| 359 | BDSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 360 | BDSADDBITSFLOAT(18, eph._Cuc, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 361 | BDSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
|
---|
| 362 | BDSADDBITSFLOAT(18, eph._Cus, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 363 | BDSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
|
---|
[6619] | 364 | BDSADDBITS(17, static_cast<int>(eph._TOE_bdt.gpssec())>>3)
|
---|
[6615] | 365 | BDSADDBITSFLOAT(18, eph._Cic, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 366 | BDSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 367 | BDSADDBITSFLOAT(18, eph._Cis, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 368 | BDSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
[6619] | 369 | BDSADDBITSFLOAT(18, eph._Crc, 1.0/static_cast<double>(1<<6))
|
---|
[6615] | 370 | BDSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
|
---|
| 371 | BDSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
|
---|
| 372 | BDSADDBITSFLOAT(10, eph._TGD1, 0.0000000001)
|
---|
| 373 | BDSADDBITSFLOAT(10, eph._TGD2, 0.0000000001)
|
---|
| 374 | BDSADDBITS(1, eph._SatH1)
|
---|
[6627] | 375 | BDSADDBITS(1, 0) /* reserved bit, fill up 8 bits */
|
---|
[6615] | 376 |
|
---|
| 377 | startbuffer[0]=0xD3;
|
---|
| 378 | startbuffer[1]=(size >> 8);
|
---|
| 379 | startbuffer[2]=size;
|
---|
| 380 | unsigned long i = CRC24(size+3, startbuffer);
|
---|
| 381 | buffer[size++] = i >> 16;
|
---|
| 382 | buffer[size++] = i >> 8;
|
---|
| 383 | buffer[size++] = i;
|
---|
| 384 | size += 3;
|
---|
| 385 | return size;
|
---|
[6403] | 386 | }
|
---|