Changeset 6631 in ntrip


Ignore:
Timestamp:
Mar 4, 2015, 2:47:12 PM (7 years ago)
Author:
stuerze
Message:

initial import of SBAS decoder

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/BNC/src/RTCM3/ephEncoder.cpp

    r6627 r6631  
    323323// build up RTCM3 for SBAS
    324324////////////////////////////////////////////////////////////////////////////
    325 int t_ephEncoder::RTCM3(const t_ephSBAS& /* eph */, unsigned char* /* buffer */) {
    326 
    327   return 0;
     325#define SBASTOINT(type, value) static_cast<type>(round(value))
     326
     327#define SBASADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
     328                       |(SBASTOINT(long long,b)&((1ULL<<a)-1)); \
     329                       numbits += (a); \
     330                       while(numbits >= 8) { \
     331                       buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
     332#define SBASADDBITSFLOAT(a,b,c) {long long i = SBASTOINT(long long,(b)/(c)); \
     333                             SBASADDBITS(a,i)};
     334
     335int t_ephEncoder::RTCM3(const t_ephSBAS& eph, unsigned char* buffer) {
     336  int size = 0;
     337  int numbits = 0;
     338  long long bitbuffer = 0;
     339  unsigned char *startbuffer = buffer;
     340  buffer= buffer+3;
     341
     342  SBASADDBITS(12, 1043)
     343  SBASADDBITS(6, eph._prn.number()-20)
     344  SBASADDBITS(8, eph._IODN)
     345  SBASADDBITS(13, static_cast<int>(eph._TOC.daysec())>>4)
     346  SBASADDBITS(4, eph._ura)
     347  SBASADDBITSFLOAT(30, eph._x_pos, 0.08)
     348  SBASADDBITSFLOAT(30, eph._y_pos, 0.08)
     349  SBASADDBITSFLOAT(25, eph._z_pos, 0.4)
     350  SBASADDBITSFLOAT(17, eph._x_velocity, 0.000625)
     351  SBASADDBITSFLOAT(17, eph._y_velocity, 0.000625)
     352  SBASADDBITSFLOAT(18, eph._z_velocity, 0.004)
     353  SBASADDBITSFLOAT(10, eph._x_acceleration, 0.0000125)
     354  SBASADDBITSFLOAT(10, eph._y_acceleration, 0.0000125)
     355  SBASADDBITSFLOAT(10, eph._z_acceleration, 0.0000625)
     356  SBASADDBITSFLOAT(12, eph._agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
     357  SBASADDBITSFLOAT(8, eph._agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
     358
     359  startbuffer[0]=0xD3;
     360  startbuffer[1]=(size >> 8);
     361  startbuffer[2]=size;
     362  unsigned long i = CRC24(size+3, startbuffer);
     363  buffer[size++] = i >> 16;
     364  buffer[size++] = i >> 8;
     365  buffer[size++] = i;
     366  size += 3;
     367  return size;
    328368}
    329369
Note: See TracChangeset for help on using the changeset viewer.