Changeset 5852 in ntrip for trunk


Ignore:
Timestamp:
Aug 7, 2014, 1:05:36 PM (10 years ago)
Author:
mervart
Message:
 
Location:
trunk/BNC/src
Files:
2 added
3 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/src/ephemeris.cpp

    r5851 r5852  
    1616
    1717using namespace std;
    18 
    19 // Returns CRC24
    20 ////////////////////////////////////////////////////////////////////////////
    21 static unsigned long CRC24(long size, const unsigned char *buf) {
    22   unsigned long crc = 0;
    23   int ii;
    24   while (size--) {
    25     crc ^= (*buf++) << (16);
    26     for(ii = 0; ii < 8; ii++) {
    27       crc <<= 1;
    28       if (crc & 0x1000000) {
    29         crc ^= 0x01864cfb;
    30       }
    31     }
    32   }
    33   return crc;
    34 }
    3518
    3619// Constructor
    … …  
    228211}
    229212
    230 // build up RTCM3 for GPS
    231 ////////////////////////////////////////////////////////////////////////////
    232 #define GPSTOINT(type, value) static_cast<type>(round(value))
    233 
    234 #define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
    235                        |(GPSTOINT(long long,b)&((1ULL<<a)-1)); \
    236                        numbits += (a); \
    237                        while(numbits >= 8) { \
    238                        buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
    239 
    240 #define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \
    241                              GPSADDBITS(a,i)};
    242 
    243 int t_ephGPS::RTCM3(unsigned char *buffer) {
    244 
    245   unsigned char *startbuffer = buffer;
    246   buffer= buffer+3;
    247   int size = 0;
    248   int numbits = 0;
    249   unsigned long long bitbuffer = 0;
    250   if (_ura <= 2.40){
    251     _ura = 0;
    252   }
    253   else if (_ura <= 3.40){
    254     _ura = 1;
    255   }
    256   else if (_ura <= 6.85){
    257     _ura = 2;
    258   }
    259   else if (_ura <= 9.65){
    260     _ura = 3;
    261   }
    262   else if (_ura <= 13.65){
    263     _ura = 4;
    264   }
    265   else if (_ura <= 24.00){
    266     _ura = 5;
    267   }
    268   else if (_ura <= 48.00){
    269     _ura = 6;
    270   }
    271   else if (_ura <= 96.00){
    272     _ura = 7;
    273   }
    274   else if (_ura <= 192.00){
    275     _ura = 8;
    276   }
    277   else if (_ura <= 384.00){
    278     _ura = 9;
    279   }
    280   else if (_ura <= 768.00){
    281     _ura = 10;
    282   }
    283   else if (_ura <= 1536.00){
    284     _ura = 11;
    285   }
    286   else if (_ura <= 1536.00){
    287     _ura = 12;
    288   }
    289   else if (_ura <= 2072.00){
    290     _ura = 13;
    291   }
    292   else if (_ura <= 6144.00){
    293     _ura = 14;
    294   }
    295   else{
    296     _ura = 15;
    297   }
    298 
    299   GPSADDBITS(12, 1019)
    300   GPSADDBITS(6,_prn.number())
    301   GPSADDBITS(10, _TOC.gpsw())
    302   GPSADDBITS(4, _ura)
    303   GPSADDBITS(2,_L2Codes)
    304   GPSADDBITSFLOAT(14, _IDOT, M_PI/static_cast<double>(1<<30)
    305   /static_cast<double>(1<<13))
    306   GPSADDBITS(8, _IODE)
    307   GPSADDBITS(16, static_cast<int>(_TOC.gpssec())>>4)
    308   GPSADDBITSFLOAT(8, _clock_driftrate, 1.0/static_cast<double>(1<<30)
    309   /static_cast<double>(1<<25))
    310   GPSADDBITSFLOAT(16, _clock_drift, 1.0/static_cast<double>(1<<30)
    311   /static_cast<double>(1<<13))
    312   GPSADDBITSFLOAT(22, _clock_bias, 1.0/static_cast<double>(1<<30)
    313   /static_cast<double>(1<<1))
    314   GPSADDBITS(10, _IODC)
    315   GPSADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
    316   GPSADDBITSFLOAT(16, _Delta_n, M_PI/static_cast<double>(1<<30)
    317   /static_cast<double>(1<<13))
    318   GPSADDBITSFLOAT(32, _M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    319   GPSADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
    320   GPSADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
    321   GPSADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
    322   GPSADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
    323   GPSADDBITS(16, static_cast<int>(_TOEsec)>>4)
    324   GPSADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
    325   GPSADDBITSFLOAT(32, _OMEGA0, M_PI/static_cast<double>(1<<30)
    326   /static_cast<double>(1<<1))
    327   GPSADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
    328   GPSADDBITSFLOAT(32, _i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    329   GPSADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
    330   GPSADDBITSFLOAT(32, _omega, M_PI/static_cast<double>(1<<30)
    331   /static_cast<double>(1<<1))
    332   GPSADDBITSFLOAT(24, _OMEGADOT, M_PI/static_cast<double>(1<<30)
    333   /static_cast<double>(1<<13))
    334   GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    335   GPSADDBITS(6, _health)
    336   GPSADDBITS(1, _L2PFlag)
    337   GPSADDBITS(1, 0) /* GPS fit interval */
    338 
    339   startbuffer[0]=0xD3;
    340   startbuffer[1]=(size >> 8);
    341   startbuffer[2]=size;
    342   unsigned long  i = CRC24(size+3, startbuffer);
    343   buffer[size++] = i >> 16;
    344   buffer[size++] = i >> 8;
    345   buffer[size++] = i;
    346   size += 3;
    347   return size;
    348 }
    349 
    350213// Derivative of the state vector using a simple force model (static)
    351214////////////////////////////////////////////////////////////////////////////
    … …  
    527390
    528391  _ok = true;
    529 }
    530 
    531 // build up RTCM3 for GLONASS
    532 ////////////////////////////////////////////////////////////////////////////
    533 #define GLONASSTOINT(type, value) static_cast<type>(round(value))
    534 
    535 #define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
    536                        |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \
    537                        numbits += (a); \
    538                        while(numbits >= 8) { \
    539                        buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
    540 #define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \
    541                        if(b < 0.0) \
    542                        { \
    543                          s = 1; \
    544                          i = GLONASSTOINT(long long,(-b)/(c)); \
    545                          if(!i) s = 0; \
    546                        } \
    547                        else \
    548                        { \
    549                          s = 0; \
    550                          i = GLONASSTOINT(long long,(b)/(c)); \
    551                        } \
    552                        GLONASSADDBITS(1,s) \
    553                        GLONASSADDBITS(a-1,i)}
    554 
    555 int t_ephGlo::RTCM3(unsigned char *buffer)
    556 {
    557 
    558   int size = 0;
    559   int numbits = 0;
    560   long long bitbuffer = 0;
    561   unsigned char *startbuffer = buffer;
    562   buffer= buffer+3;
    563 
    564   GLONASSADDBITS(12, 1020)
    565   GLONASSADDBITS(6, _prn.number())
    566   GLONASSADDBITS(5, 7+_frequency_number)
    567   GLONASSADDBITS(1, 0)
    568   GLONASSADDBITS(1, 0)
    569   GLONASSADDBITS(2, 0)
    570   _tki=_tki+3*60*60;
    571   GLONASSADDBITS(5, static_cast<int>(_tki)/(60*60))
    572   GLONASSADDBITS(6, (static_cast<int>(_tki)/60)%60)
    573   GLONASSADDBITS(1, (static_cast<int>(_tki)/30)%30)
    574   GLONASSADDBITS(1, _health)
    575   GLONASSADDBITS(1, 0)
    576   unsigned long long timeofday = (static_cast<int>(_tt.gpssec()+3*60*60-_gps_utc)%86400);
    577   GLONASSADDBITS(7, timeofday/60/15)
    578   GLONASSADDBITSFLOATM(24, _x_velocity*1000, 1000.0/static_cast<double>(1<<20))
    579   GLONASSADDBITSFLOATM(27, _x_pos*1000, 1000.0/static_cast<double>(1<<11))
    580   GLONASSADDBITSFLOATM(5, _x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
    581   GLONASSADDBITSFLOATM(24, _y_velocity*1000, 1000.0/static_cast<double>(1<<20))
    582   GLONASSADDBITSFLOATM(27, _y_pos*1000, 1000.0/static_cast<double>(1<<11))
    583   GLONASSADDBITSFLOATM(5, _y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
    584   GLONASSADDBITSFLOATM(24, _z_velocity*1000, 1000.0/static_cast<double>(1<<20))
    585   GLONASSADDBITSFLOATM(27,_z_pos*1000, 1000.0/static_cast<double>(1<<11))
    586   GLONASSADDBITSFLOATM(5, _z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
    587   GLONASSADDBITS(1, 0)
    588   GLONASSADDBITSFLOATM(11, _gamma, 1.0/static_cast<double>(1<<30)
    589   /static_cast<double>(1<<10))
    590   GLONASSADDBITS(2, 0) /* GLONASS-M P */
    591   GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
    592   GLONASSADDBITSFLOATM(22, _tau, 1.0/static_cast<double>(1<<30))
    593   GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
    594   GLONASSADDBITS(5, _E)
    595   GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
    596   GLONASSADDBITS(4, 0) /* GLONASS-M FT */
    597   GLONASSADDBITS(11, 0) /* GLONASS-M NT */
    598   GLONASSADDBITS(2, 0) /* GLONASS-M active? */
    599   GLONASSADDBITS(1, 0) /* GLONASS additional data */
    600   GLONASSADDBITS(11, 0) /* GLONASS NA */
    601   GLONASSADDBITS(32, 0) /* GLONASS tau C */
    602   GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
    603   GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
    604   GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
    605   GLONASSADDBITS(7, 0) /* Reserved */
    606 
    607   startbuffer[0]=0xD3;
    608   startbuffer[1]=(size >> 8);
    609   startbuffer[2]=size;
    610   unsigned long i = CRC24(size+3, startbuffer);
    611   buffer[size++] = i >> 16;
    612   buffer[size++] = i >> 8;
    613   buffer[size++] = i;
    614   size += 3;
    615   return size;
    616392}
    617393
    … …  
    749525}
    750526
    751 // build up RTCM3 for Galileo
    752 ////////////////////////////////////////////////////////////////////////////
    753 #define GALILEOTOINT(type, value) static_cast<type>(round(value))
    754 
    755 #define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
    756                        |(GALILEOTOINT(long long,b)&((1LL<<a)-1)); \
    757                        numbits += (a); \
    758                        while(numbits >= 8) { \
    759                        buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
    760 #define GALILEOADDBITSFLOAT(a,b,c) {long long i = GALILEOTOINT(long long,(b)/(c)); \
    761                              GALILEOADDBITS(a,i)};
    762 
    763 int t_ephGal::RTCM3(unsigned char *buffer) {
    764   int size = 0;
    765   int numbits = 0;
    766   long long bitbuffer = 0;
    767   unsigned char *startbuffer = buffer;
    768   buffer= buffer+3;
    769 
    770   bool inav = ( (_flags & GALEPHF_INAV) == GALEPHF_INAV );
    771 
    772   GALILEOADDBITS(12, inav ? 1046 : 1045)
    773   GALILEOADDBITS(6, _prn.number())
    774   GALILEOADDBITS(12, _TOC.gpsw())
    775   GALILEOADDBITS(10, _IODnav)
    776   GALILEOADDBITS(8, _SISA)
    777   GALILEOADDBITSFLOAT(14, _IDOT, M_PI/static_cast<double>(1<<30)
    778   /static_cast<double>(1<<13))
    779   GALILEOADDBITS(14, _TOC.gpssec()/60)
    780   GALILEOADDBITSFLOAT(6, _clock_driftrate, 1.0/static_cast<double>(1<<30)
    781   /static_cast<double>(1<<29))
    782   GALILEOADDBITSFLOAT(21, _clock_drift, 1.0/static_cast<double>(1<<30)
    783   /static_cast<double>(1<<16))
    784   GALILEOADDBITSFLOAT(31, _clock_bias, 1.0/static_cast<double>(1<<30)
    785   /static_cast<double>(1<<4))
    786   GALILEOADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
    787   GALILEOADDBITSFLOAT(16, _Delta_n, M_PI/static_cast<double>(1<<30)
    788   /static_cast<double>(1<<13))
    789   GALILEOADDBITSFLOAT(32, _M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    790   GALILEOADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
    791   GALILEOADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
    792   GALILEOADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
    793   GALILEOADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
    794   GALILEOADDBITS(14, _TOEsec/60)
    795   GALILEOADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
    796   GALILEOADDBITSFLOAT(32, _OMEGA0, M_PI/static_cast<double>(1<<30)
    797   /static_cast<double>(1<<1))
    798   GALILEOADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
    799   GALILEOADDBITSFLOAT(32, _i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
    800   GALILEOADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
    801   GALILEOADDBITSFLOAT(32, _omega, M_PI/static_cast<double>(1<<30)
    802   /static_cast<double>(1<<1))
    803   GALILEOADDBITSFLOAT(24, _OMEGADOT, M_PI/static_cast<double>(1<<30)
    804   /static_cast<double>(1<<13))
    805   GALILEOADDBITSFLOAT(10, _BGD_1_5A, 1.0/static_cast<double>(1<<30)
    806   /static_cast<double>(1<<2))
    807   if(inav)
    808   {
    809     GALILEOADDBITSFLOAT(10, _BGD_1_5B, 1.0/static_cast<double>(1<<30)
    810     /static_cast<double>(1<<2))
    811     GALILEOADDBITS(2, static_cast<int>(_E5bHS))
    812     GALILEOADDBITS(1, _flags & GALEPHF_E5BDINVALID)
    813   }
    814   else
    815   {
    816     GALILEOADDBITS(2, static_cast<int>(_E5aHS))
    817     GALILEOADDBITS(1, _flags & GALEPHF_E5ADINVALID)
    818   }
    819   _TOEsec = 0.9999E9;
    820   GALILEOADDBITS(20, _TOEsec)
    821 
    822   GALILEOADDBITS(inav ? 1 : 3, 0)
    823 
    824   startbuffer[0]=0xD3;
    825   startbuffer[1]=(size >> 8);
    826   startbuffer[2]=size;
    827   unsigned long i = CRC24(size+3, startbuffer);
    828   buffer[size++] = i >> 16;
    829   buffer[size++] = i >> 8;
    830   buffer[size++] = i;
    831   size += 3;
    832   return size;
    833 }
    834 
    835527// Constructor
    836528//////////////////////////////////////////////////////////////////////////////

  • trunk/BNC/src/ephemeris.h

    r5815 r5852  
    2020class t_eph {
    2121 public:
    22 
    2322  enum e_type {unknown, GPS, GLONASS, Galileo};
    2423
    … …  
    3534                        double* xc, double* vv) const = 0;
    3635  virtual int  IOD() const = 0;
    37   virtual int  RTCM3(unsigned char *) = 0;
    3836
    3937  bool ok() const {return _ok;}
    … …  
    8078
    8179class t_ephGPS : public t_eph {
     80 friend class t_ephEncoder;
    8281 public:
    8382  t_ephGPS() { }
    … …  
    9695
    9796  virtual int  IOD() const { return static_cast<int>(_IODC); }
    98 
    99   virtual int  RTCM3(unsigned char *);
    10097
    10198  double TGD() const {return _TGD;} // Timing Group Delay (P1-P2 DCB)
    … …  
    131128  double  _L2PFlag;         // L2 P data flag
    132129
    133   double  _ura;             // SV accuracy
     130  mutable double  _ura;             // SV accuracy
    134131  double  _health;          // SV health
    135132  double  _TGD;             // [s]   
    … …  
    156153
    157154  virtual int  IOD() const;
    158 
    159   virtual int  RTCM3(unsigned char *);
    160155
    161156  void set(const glonassephemeris* ee);
    … …  
    208203
    209204  virtual int  IOD() const { return static_cast<int>(_IODnav); }
    210 
    211   virtual int  RTCM3(unsigned char *);
    212205
    213206 private:

  • trunk/BNC/src/src.pri

    r5824 r5852  
    5656          RTCM/RTCM2_2021.h RTCM/rtcm_utils.h                         \
    5757          RTCM3/RTCM3Decoder.h RTCM3/rtcm3torinex/rtcm3torinex.h      \
    58           RTCM3/RTCM3coDecoder.h                                      \
     58          RTCM3/RTCM3coDecoder.h RTCM3/ephEncoder.h                   \
    5959          RTCM3/clock_and_orbit/clock_orbit_rtcm.h                    \
    6060          PPP/pppInclude.h   PPP/pppOptions.h   PPP/pppMain.h         \
    … …  
    8888          RTCM/RTCM2_2021.cpp RTCM/rtcm_utils.cpp                     \
    8989          RTCM3/RTCM3Decoder.cpp RTCM3/rtcm3torinex/rtcm3torinex.c    \
    90           RTCM3/RTCM3coDecoder.cpp                                    \
     90          RTCM3/RTCM3coDecoder.cpp RTCM3/ephEncoder.cpp               \
    9191          RTCM3/clock_and_orbit/clock_orbit_rtcm.c                    \
    9292                             PPP/pppOptions.cpp PPP/pppMain.cpp       \
Note: See TracChangeset for help on using the changeset viewer.