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