| 1 | // Part of BNC, a utility for retrieving decoding and
|
|---|
| 2 | // converting GNSS data streams from NTRIP broadcasters.
|
|---|
| 3 | //
|
|---|
| 4 | // Copyright (C) 2015
|
|---|
| 5 | // German Federal Agency for Cartography and Geodesy (BKG)
|
|---|
| 6 | // http://www.bkg.bund.de
|
|---|
| 7 | // Alberding GmbH
|
|---|
| 8 | // http://www.alberding.eu
|
|---|
| 9 | //
|
|---|
| 10 | // Email: euref-ip@bkg.bund.de
|
|---|
| 11 | //
|
|---|
| 12 | // This program is free software; you can redistribute it and/or
|
|---|
| 13 | // modify it under the terms of the GNU General Public License
|
|---|
| 14 | // as published by the Free Software Foundation, version 2.
|
|---|
| 15 | //
|
|---|
| 16 | // This program is distributed in the hope that it will be useful,
|
|---|
| 17 | // but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|---|
| 18 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|---|
| 19 | // GNU General Public License for more details.
|
|---|
| 20 | //
|
|---|
| 21 | // You should have received a copy of the GNU General Public License
|
|---|
| 22 | // along with this program; if not, write to the Free Software
|
|---|
| 23 | // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
|---|
| 24 |
|
|---|
| 25 | #ifndef BITS_H
|
|---|
| 26 | #define BITS_H
|
|---|
| 27 |
|
|---|
| 28 | #define LOADBITS(a) { \
|
|---|
| 29 | while((a) > numbits) { \
|
|---|
| 30 | if(!size--) return false; \
|
|---|
| 31 | bitfield = (bitfield<<8)|*(data++); \
|
|---|
| 32 | numbits += 8; \
|
|---|
| 33 | } \
|
|---|
| 34 | }
|
|---|
| 35 |
|
|---|
| 36 | /* extract bits from data stream
|
|---|
| 37 | b = variable to store result, a = number of bits */
|
|---|
| 38 | #define GETBITS64(b, a) { \
|
|---|
| 39 | if(((a) > 56) && ((a)-56) > numbits) { \
|
|---|
| 40 | uint64_t x; \
|
|---|
| 41 | GETBITS(x, 56) \
|
|---|
| 42 | LOADBITS((a)-56) \
|
|---|
| 43 | b = ((x<<((a)-56)) | (bitfield<<(sizeof(bitfield)*8-numbits)) \
|
|---|
| 44 | >>(sizeof(bitfield)*8-((a)-56))); \
|
|---|
| 45 | numbits -= ((a)-56); \
|
|---|
| 46 | } \
|
|---|
| 47 | else { \
|
|---|
| 48 | GETBITS(b, a) \
|
|---|
| 49 | } \
|
|---|
| 50 | }
|
|---|
| 51 |
|
|---|
| 52 | /* extract bits from data stream
|
|---|
| 53 | b = variable to store result, a = number of bits */
|
|---|
| 54 | #define GETBITS(b, a) { \
|
|---|
| 55 | LOADBITS(a) \
|
|---|
| 56 | b = (bitfield<<(64-numbits))>>(64-(a)); \
|
|---|
| 57 | numbits -= (a); \
|
|---|
| 58 | }
|
|---|
| 59 |
|
|---|
| 60 | /* extract bits from data stream
|
|---|
| 61 | b = variable to store result, a = number of bits */
|
|---|
| 62 | #define GETBITSFACTOR(b, a, c) { \
|
|---|
| 63 | LOADBITS(a) \
|
|---|
| 64 | b = ((bitfield<<(sizeof(bitfield)*8-numbits))>>(sizeof(bitfield)*8-(a)))*(c); \
|
|---|
| 65 | numbits -= (a); \
|
|---|
| 66 | }
|
|---|
| 67 |
|
|---|
| 68 | /* extract floating value from data stream
|
|---|
| 69 | b = variable to store result, a = number of bits */
|
|---|
| 70 | #define GETFLOAT(b, a, c) { \
|
|---|
| 71 | LOADBITS(a) \
|
|---|
| 72 | b = ((double)((bitfield<<(64-numbits))>>(64-(a))))*(c); \
|
|---|
| 73 | numbits -= (a); \
|
|---|
| 74 | }
|
|---|
| 75 |
|
|---|
| 76 | /* extract signed floating value from data stream
|
|---|
| 77 | b = variable to store result, a = number of bits */
|
|---|
| 78 | #define GETFLOATSIGN(b, a, c) { \
|
|---|
| 79 | LOADBITS(a) \
|
|---|
| 80 | b = ((double)(((int64_t)(bitfield<<(64-numbits)))>>(64-(a))))*(c); \
|
|---|
| 81 | numbits -= (a); \
|
|---|
| 82 | }
|
|---|
| 83 |
|
|---|
| 84 | /* extract bits from data stream
|
|---|
| 85 | b = variable to store result, a = number of bits */
|
|---|
| 86 | #define GETBITSSIGN(b, a) { \
|
|---|
| 87 | LOADBITS(a) \
|
|---|
| 88 | b = ((int64_t)(bitfield<<(64-numbits)))>>(64-(a)); \
|
|---|
| 89 | numbits -= (a); \
|
|---|
| 90 | }
|
|---|
| 91 |
|
|---|
| 92 | #define GETFLOATSIGNM(b, a, c) { \
|
|---|
| 93 | int l; \
|
|---|
| 94 | LOADBITS(a) \
|
|---|
| 95 | l = (bitfield<<(64-numbits))>>(64-1); \
|
|---|
| 96 | b = ((double)(((bitfield<<(64-(numbits-1))))>>(64-(a-1))))*(c); \
|
|---|
| 97 | numbits -= (a); \
|
|---|
| 98 | if(l) b *= -1.0; \
|
|---|
| 99 | }
|
|---|
| 100 |
|
|---|
| 101 | #define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
|
|---|
| 102 |
|
|---|
| 103 | /* extract byte-aligned byte from data stream,
|
|---|
| 104 | b = variable to store size, s = variable to store string pointer */
|
|---|
| 105 | #define GETSTRING(b, s) { \
|
|---|
| 106 | b = *(data++); \
|
|---|
| 107 | s = (char *) data; \
|
|---|
| 108 | data += b; \
|
|---|
| 109 | size -= b+1; \
|
|---|
| 110 | }
|
|---|
| 111 |
|
|---|
| 112 | #define STARTDATA \
|
|---|
| 113 | size_t ressize=0; \
|
|---|
| 114 | char *blockstart=0; \
|
|---|
| 115 | int numbits=0; \
|
|---|
| 116 | uint64_t bitbuffer=0;
|
|---|
| 117 |
|
|---|
| 118 |
|
|---|
| 119 | #define STOREBITS \
|
|---|
| 120 | while(numbits >= 8) { \
|
|---|
| 121 | if(!size) return 0; \
|
|---|
| 122 | *(buffer++) = bitbuffer>>(numbits-8); \
|
|---|
| 123 | numbits -= 8; \
|
|---|
| 124 | ++ressize; \
|
|---|
| 125 | --size; \
|
|---|
| 126 | }
|
|---|
| 127 |
|
|---|
| 128 | #define ADDBITS(a, b) { \
|
|---|
| 129 | bitbuffer = (bitbuffer<<(a))|((b)&((1<<a)-1)); \
|
|---|
| 130 | numbits += (a); \
|
|---|
| 131 | STOREBITS \
|
|---|
| 132 | }
|
|---|
| 133 |
|
|---|
| 134 |
|
|---|
| 135 | #define INITBLOCK \
|
|---|
| 136 | numbits = 0; \
|
|---|
| 137 | blockstart = buffer; \
|
|---|
| 138 | ADDBITS(8, 0xD3) \
|
|---|
| 139 | ADDBITS(6, 0) \
|
|---|
| 140 | ADDBITS(10, 0)
|
|---|
| 141 |
|
|---|
| 142 | #define ENDBLOCK \
|
|---|
| 143 | if(numbits) { ADDBITS((8-numbits), 0) } { \
|
|---|
| 144 | int len = buffer-blockstart-3; \
|
|---|
| 145 | blockstart[1] |= len>>8; \
|
|---|
| 146 | blockstart[2] = len; \
|
|---|
| 147 | if(len > 1023) \
|
|---|
| 148 | return 0; \
|
|---|
| 149 | len = CRC24(len+3, (const unsigned char *) blockstart); \
|
|---|
| 150 | ADDBITS(24, len) \
|
|---|
| 151 | }
|
|---|
| 152 |
|
|---|
| 153 | #define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(c > 0 ? b*c+0.5 : b*c-0.5))
|
|---|
| 154 |
|
|---|
| 155 |
|
|---|
| 156 | // RTCM3 CRS encoding
|
|---|
| 157 | //////////////////////////////////////////////////////////
|
|---|
| 158 | #define CRSTOINT(type, value) static_cast<type>(round(value))
|
|---|
| 159 |
|
|---|
| 160 | #define CRSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
|---|
| 161 | |(CRSTOINT(long long,b)&((1ULL<<a)-1)); \
|
|---|
| 162 | numbits += (a); \
|
|---|
| 163 | while(numbits >= 8) { \
|
|---|
| 164 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
|---|
| 165 |
|
|---|
| 166 | #define CRSADDBITSFLOAT(a,b,c) {long long i = CRSTOINT(long long,(b)/(c)); \
|
|---|
| 167 | CRSADDBITS(a,i)};
|
|---|
| 168 |
|
|---|
| 169 | // RTCM3 GPS EPH encoding
|
|---|
| 170 | //////////////////////////////////////////////////////////
|
|---|
| 171 | #define GPSTOINT(type, value) static_cast<type>(round(value))
|
|---|
| 172 |
|
|---|
| 173 | #define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
|---|
| 174 | |(GPSTOINT(long long,b)&((1ULL<<a)-1)); \
|
|---|
| 175 | numbits += (a); \
|
|---|
| 176 | while(numbits >= 8) { \
|
|---|
| 177 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
|---|
| 178 |
|
|---|
| 179 | #define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \
|
|---|
| 180 | GPSADDBITS(a,i)};
|
|---|
| 181 |
|
|---|
| 182 | // RTCM3 GLONASS EPH encoding
|
|---|
| 183 | //////////////////////////////////////////////////////////
|
|---|
| 184 | #define GLONASSTOINT(type, value) static_cast<type>(round(value))
|
|---|
| 185 |
|
|---|
| 186 | #define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
|---|
| 187 | |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \
|
|---|
| 188 | numbits += (a); \
|
|---|
| 189 | while(numbits >= 8) { \
|
|---|
| 190 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
|---|
| 191 |
|
|---|
| 192 | #define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \
|
|---|
| 193 | if(b < 0.0) \
|
|---|
| 194 | { \
|
|---|
| 195 | s = 1; \
|
|---|
| 196 | i = GLONASSTOINT(long long,(-b)/(c)); \
|
|---|
| 197 | if(!i) s = 0; \
|
|---|
| 198 | } \
|
|---|
| 199 | else \
|
|---|
| 200 | { \
|
|---|
| 201 | s = 0; \
|
|---|
| 202 | i = GLONASSTOINT(long long,(b)/(c)); \
|
|---|
| 203 | } \
|
|---|
| 204 | GLONASSADDBITS(1,s) \
|
|---|
| 205 | GLONASSADDBITS(a-1,i)}
|
|---|
| 206 |
|
|---|
| 207 | // RTCM3 Galileo EPH encoding
|
|---|
| 208 | //////////////////////////////////////////////////////////
|
|---|
| 209 | #define GALILEOTOINT(type, value) static_cast<type>(round(value))
|
|---|
| 210 |
|
|---|
| 211 | #define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
|---|
| 212 | |(GALILEOTOINT(long long,b)&((1LL<<a)-1)); \
|
|---|
| 213 | numbits += (a); \
|
|---|
| 214 | while(numbits >= 8) { \
|
|---|
| 215 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
|---|
| 216 |
|
|---|
| 217 | #define GALILEOADDBITSFLOAT(a,b,c) {long long i = GALILEOTOINT(long long,(b)/(c)); \
|
|---|
| 218 | GALILEOADDBITS(a,i)};
|
|---|
| 219 |
|
|---|
| 220 | // RTCM3 BDS EPH encoding
|
|---|
| 221 | //////////////////////////////////////////////////////////
|
|---|
| 222 | #define BDSTOINT(type, value) static_cast<type>(round(value))
|
|---|
| 223 |
|
|---|
| 224 | #define BDSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
|---|
| 225 | |(BDSTOINT(long long,b)&((1ULL<<a)-1)); \
|
|---|
| 226 | numbits += (a); \
|
|---|
| 227 | while(numbits >= 8) { \
|
|---|
| 228 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
|---|
| 229 |
|
|---|
| 230 | #define BDSADDBITSFLOAT(a,b,c) {long long i = BDSTOINT(long long,(b)/(c)); \
|
|---|
| 231 | BDSADDBITS(a,i)};
|
|---|
| 232 |
|
|---|
| 233 | // RTCM3 SBAS EPH encoding
|
|---|
| 234 | //////////////////////////////////////////////////////////
|
|---|
| 235 | #define SBASTOINT(type, value) static_cast<type>(round(value))
|
|---|
| 236 |
|
|---|
| 237 | #define SBASADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
|
|---|
| 238 | |(SBASTOINT(long long,b)&((1ULL<<a)-1)); \
|
|---|
| 239 | numbits += (a); \
|
|---|
| 240 | while(numbits >= 8) { \
|
|---|
| 241 | buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
|
|---|
| 242 |
|
|---|
| 243 | #define SBASADDBITSFLOAT(a,b,c) {long long i = SBASTOINT(long long,(b)/(c)); \
|
|---|
| 244 | SBASADDBITS(a,i)};
|
|---|
| 245 |
|
|---|
| 246 | #endif /* BITS_H */
|
|---|
