Changeset 1829 in ntrip for trunk


Ignore:
Timestamp:
May 6, 2009, 4:02:39 PM (16 years ago)
Author:
stoecker
Message:

fixed clock and orbit calls

Location:
trunk/BNC/RTCM3
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/RTCM3/RTCM3coDecoder.cpp

    r1828 r1829  
    123123  errmsg.clear();
    124124
     125//printf("Start with %d new bytes and %d old bytes\n", bufLen, _buffer.size());
     126
    125127  _buffer.append(QByteArray(buffer,bufLen));
    126128
    127129  t_irc retCode = failure;
    128130
    129   while (true) {
    130    
     131  while(_buffer.size())
     132  {
    131133    int bytesused = 0;
    132     GCOB_RETURN irc = GetClockOrbitBias(&_co, &_bias, _buffer.data(),
     134    struct ClockOrbit cox;
     135    memcpy(&cox, &_co, sizeof(cox)); /* save state */
     136
     137    GCOB_RETURN irc = GetClockOrbitBias(&_co, &_bias, _buffer.data(),
    133138                                        _buffer.size(), &bytesused);
    134139
    135     // Not enough Data
    136     // ---------------
    137     if      (irc == GCOBR_SHORTBUFFER         ||
    138              irc == GCOBR_MESSAGEEXCEEDSBUFFER) {
     140//printf("used %4d buffer %4d return %4d first byte %2x\n", bytesused, _buffer.size(), irc, (unsigned char)_buffer.data()[0]);
     141    if(irc <= -30) /* not enough data found */
     142    {
     143      /* copy previous state back - necessary in case of MESSAGEFOLLOWS with
     144         incomplete second block */
     145      memcpy(&_co, &cox, sizeof(cox));
    139146      if (retCode != success) {
    140147        _GPSweeks = -1.0;
    … …  
    142149      return retCode;
    143150    }
    144 
    145     // Second part of the message follows
    146     // ----------------------------------
    147     else if (irc == GCOBR_MESSAGEFOLLOWS) {
     151    else if(irc >= 0)
     152    {
    148153      _buffer = _buffer.mid(bytesused);
    149     }
    150    
    151     // Message correctly decoded
    152     // -------------------------
    153     else if (bytesused > 0 && irc == GCOBR_OK) {
    154 
    155       reopen();
    156 
    157       int    GPSweek;
    158       currentGPSWeeks(GPSweek, _GPSweeks);
    159       if (_co.NumberOfGPSSat > 0) {
    160         if      (_GPSweeks > _co.GPSEpochTime + 86400.0) {
    161           GPSweek += 1;
    162         }
    163         else if (_GPSweeks < _co.GPSEpochTime - 86400.0) {
    164           GPSweek -= 1;
    165         }
    166         _GPSweeks = _co.GPSEpochTime;
     154
     155      if(irc == GCOBR_OK) /* correctly and complete decoded */
     156      {
     157        reopen();
     158
     159//printf("TIME: gps %d glonass %d gps_tod %d\n", _co.GPSEpochTime, _co.GLONASSEpochTime, _co.GPSEpochTime%86400);
     160
     161        int    GPSweek;
     162        currentGPSWeeks(GPSweek, _GPSweeks);
     163        if (_co.NumberOfGPSSat > 0) {
     164          if      (_GPSweeks > _co.GPSEpochTime + 86400.0) {
     165            GPSweek += 1;
     166          }
     167          else if (_GPSweeks < _co.GPSEpochTime - 86400.0) {
     168            GPSweek -= 1;
     169          }
     170          _GPSweeks = _co.GPSEpochTime;
     171        }
     172        else {
     173          double GPSdaysec = fmod(_GPSweeks, 86400.0);
     174          int    weekDay   = int((_GPSweeks - GPSdaysec) / 86400.0);
     175          if      (GPSdaysec > _co.GLONASSEpochTime + 3600.0) {
     176            weekDay += 1;
     177            if (weekDay > 6) {
     178              weekDay = 0;
     179              GPSweek += 1;
     180            }
     181          }
     182          else if (GPSdaysec < _co.GLONASSEpochTime - 3600.0) {
     183            weekDay -= 1;
     184            if (weekDay < 0) {
     185              weekDay = 6;
     186              GPSweek -= 1;
     187            }
     188          }
     189          _GPSweeks = weekDay * 86400.0 + _co.GLONASSEpochTime;
     190        }
     191
     192        for(int ii = 0; ii < _co.NumberOfGPSSat; ++ii) {
     193          QString line;
     194          line.sprintf("%d %.1f G%2.2d   %3d   %8.3f   %8.3f %8.3f %8.3f",
     195                  GPSweek, _GPSweeks, _co.Sat[ii].ID, _co.Sat[ii].IOD,
     196                  _co.Sat[ii].Clock.DeltaA0,
     197                  _co.Sat[ii].Orbit.DeltaRadial,
     198                  _co.Sat[ii].Orbit.DeltaAlongTrack,
     199                  _co.Sat[ii].Orbit.DeltaCrossTrack);
     200          long coTime = GPSweek * 7*24*3600 + long(floor(_GPSweeks+0.5));
     201          printLine(line, coTime);
     202        }
     203        for(int ii = CLOCKORBIT_NUMGPS;
     204            ii < CLOCKORBIT_NUMGPS + _co.NumberOfGLONASSSat; ++ii) {
     205          QString line;
     206          line.sprintf("%d %.1f R%2.2d   %3d   %8.3f   %8.3f %8.3f %8.3f",
     207                  GPSweek, _GPSweeks, _co.Sat[ii].ID, _co.Sat[ii].IOD,
     208                  _co.Sat[ii].Clock.DeltaA0,
     209                  _co.Sat[ii].Orbit.DeltaRadial,
     210                  _co.Sat[ii].Orbit.DeltaAlongTrack,
     211                  _co.Sat[ii].Orbit.DeltaCrossTrack);
     212          long coTime = GPSweek * 7*24*3600 + long(floor(_GPSweeks+0.5));
     213          printLine(line, coTime);
     214        }
     215        retCode = success;
     216        memset(&_co, 0, sizeof(_co));
    167217      }
    168       else {
    169         double GPSdaysec = fmod(_GPSweeks, 86400.0);
    170         int    weekDay   = int((_GPSweeks - GPSdaysec) / 86400.0);
    171         if      (GPSdaysec > _co.GLONASSEpochTime + 3600.0) {
    172           weekDay += 1;
    173           if (weekDay > 6) {
    174             weekDay = 0;
    175             GPSweek += 1;
    176           }
    177         }
    178         else if (GPSdaysec < _co.GLONASSEpochTime - 3600.0) {
    179           weekDay -= 1;
    180           if (weekDay < 0) {
    181             weekDay = 6;
    182             GPSweek -= 1;
    183           }
    184         }
    185         _GPSweeks = weekDay * 86400.0 + _co.GLONASSEpochTime;
    186       }
    187 
    188       for(int ii = 0; ii < _co.NumberOfGPSSat; ++ii) {
    189         QString line;
    190         line.sprintf("%d %.1f G%2.2d   %3d   %8.3f   %8.3f %8.3f %8.3f",
    191                GPSweek, _GPSweeks, _co.Sat[ii].ID, _co.Sat[ii].IOD,
    192                _co.Sat[ii].Clock.DeltaA0,
    193                _co.Sat[ii].Orbit.DeltaRadial,
    194                _co.Sat[ii].Orbit.DeltaAlongTrack,
    195                _co.Sat[ii].Orbit.DeltaCrossTrack);
    196         long coTime = GPSweek * 7*24*3600 + long(floor(_GPSweeks+0.5));
    197         printLine(line, coTime);
    198       }
    199       for(int ii = CLOCKORBIT_NUMGPS;
    200           ii < CLOCKORBIT_NUMGPS + _co.NumberOfGLONASSSat; ++ii) {
    201         QString line;
    202         line.sprintf("%d %.1f R%2.2d   %3d   %8.3f   %8.3f %8.3f %8.3f",
    203                GPSweek, _GPSweeks, _co.Sat[ii].ID, _co.Sat[ii].IOD,
    204                _co.Sat[ii].Clock.DeltaA0,
    205                _co.Sat[ii].Orbit.DeltaRadial,
    206                _co.Sat[ii].Orbit.DeltaAlongTrack,
    207                _co.Sat[ii].Orbit.DeltaCrossTrack);
    208         long coTime = GPSweek * 7*24*3600 + long(floor(_GPSweeks+0.5));
    209         printLine(line, coTime);
    210       }
    211       _buffer = _buffer.mid(bytesused);
    212       retCode = success;
     218    }
     219    else /* error  - skip 1 byte and retry */
     220    {
    213221      memset(&_co, 0, sizeof(_co));
    214     }
    215 
    216     // All other Cases
    217     // ---------------
    218     else {
    219222      _buffer = _buffer.mid(1);
    220223    }
    221224  }
     225//printf("Return with %d (success = %d) bytes %d\n", retCode, success, _buffer.size());
     226  return retCode;
    222227}
    223228

  • trunk/BNC/RTCM3/clock_orbit_rtcm.c

    r1827 r1829  
    33        Name:           clock_orbit_rtcm.c
    44        Project:        RTCM3
    5         Version:        $Id: clock_orbit_rtcm.c,v 1.6 2009/05/04 12:53:42 stoecker Exp $
     5        Version:        $Id: clock_orbit_rtcm.c,v 1.12 2009/05/05 07:00:40 mervart Exp $
    66        Authors:        Dirk Stöcker
    77        Description:    state space approach for RTCM3
    … …  
    618618    G_RESERVED5
    619619    G_NO_OF_SATELLITES(co->NumberOfGPSSat)
    620     if(co->OrbitDataSupplied)
    621       return GCOBR_DATAMISMATCH;
    622     co->OrbitDataSupplied = 1;
     620    if(co->OrbitDataSupplied&1)
     621      return GCOBR_DATAMISMATCH;
     622    co->OrbitDataSupplied |= 1;
    623623#ifdef DEBUG
    624624fprintf(stderr, "epochtime %d ui %d mmi %d sats %d\n",co->GPSEpochTime,co->UpdateInterval,mmi,co->NumberOfGPSSat);
    … …  
    663663    G_RESERVED5
    664664    G_NO_OF_SATELLITES(co->NumberOfGPSSat)
    665     if(co->ClockDataSupplied)
    666       return GCOBR_DATAMISMATCH;
    667     co->ClockDataSupplied = 1;
     665    if(co->ClockDataSupplied & 1)
     666      return GCOBR_DATAMISMATCH;
     667    co->ClockDataSupplied |= 1;
    668668    for(i = 0; i < co->NumberOfGPSSat; ++i)
    669669    {
    … …  
    683683    G_RESERVED5
    684684    G_NO_OF_SATELLITES(co->NumberOfGPSSat)
    685     if(co->ClockDataSupplied || co->OrbitDataSupplied)
    686       return GCOBR_DATAMISMATCH;
    687     co->OrbitDataSupplied = 1;
    688     co->ClockDataSupplied = 1;
     685    if((co->ClockDataSupplied&1) || (co->OrbitDataSupplied&1))
     686      return GCOBR_DATAMISMATCH;
     687    co->OrbitDataSupplied |= 1;
     688    co->ClockDataSupplied |= 1;
    689689    for(i = 0; i < co->NumberOfGPSSat; ++i)
    690690    {
    … …  
    715715    G_RESERVED5
    716716    G_NO_OF_SATELLITES(co->NumberOfGPSSat)
    717     if(co->URADataSupplied)
    718       return GCOBR_DATAMISMATCH;
    719     co->URADataSupplied = 1;
     717    if(co->URADataSupplied&1)
     718      return GCOBR_DATAMISMATCH;
     719    co->URADataSupplied |= 1;
    720720    for(i = 0; i < co->NumberOfGPSSat; ++i)
    721721    {
    … …  
    733733    G_RESERVED5
    734734    G_NO_OF_SATELLITES(co->NumberOfGPSSat)
    735     if(co->HRDataSupplied)
    736       return GCOBR_DATAMISMATCH;
    737     co->HRDataSupplied = 1;
     735    if(co->HRDataSupplied&1)
     736      return GCOBR_DATAMISMATCH;
     737    co->HRDataSupplied |= 1;
    738738    for(i = 0; i < co->NumberOfGPSSat; ++i)
    739739    {
    … …  
    749749    G_RESERVED5
    750750    G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    751     if(co->OrbitDataSupplied)
    752       return GCOBR_DATAMISMATCH;
    753     co->OrbitDataSupplied = 1;
     751    if(co->OrbitDataSupplied&2)
     752      return GCOBR_DATAMISMATCH;
     753    co->OrbitDataSupplied |= 2;
    754754    for(i = CLOCKORBIT_NUMGPS;
    755755    i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
    … …  
    777777    G_RESERVED5
    778778    G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    779     if(co->ClockDataSupplied)
    780       return GCOBR_DATAMISMATCH;
    781     co->ClockDataSupplied = 1;
     779    if(co->ClockDataSupplied&2)
     780      return GCOBR_DATAMISMATCH;
     781    co->ClockDataSupplied |= 2;
    782782    for(i = CLOCKORBIT_NUMGPS;
    783783    i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
    … …  
    796796    G_RESERVED5
    797797    G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    798     if(co->ClockDataSupplied || co->OrbitDataSupplied)
    799       return GCOBR_DATAMISMATCH;
    800     co->OrbitDataSupplied = 1;
    801     co->ClockDataSupplied = 1;
     798    if((co->ClockDataSupplied&2) || (co->OrbitDataSupplied&2))
     799      return GCOBR_DATAMISMATCH;
     800    co->OrbitDataSupplied |= 2;
     801    co->ClockDataSupplied |= 2;
    802802    for(i = CLOCKORBIT_NUMGPS;
    803803    i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
    … …  
    827827    G_RESERVED5
    828828    G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    829     if(co->URADataSupplied)
    830       return GCOBR_DATAMISMATCH;
    831     co->URADataSupplied = 1;
     829    if(co->URADataSupplied&2)
     830      return GCOBR_DATAMISMATCH;
     831    co->URADataSupplied |= 2;
    832832    for(i = CLOCKORBIT_NUMGPS;
    833833    i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
    … …  
    844844    G_RESERVED5
    845845    G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    846     if(co->HRDataSupplied)
    847       return GCOBR_DATAMISMATCH;
    848     co->HRDataSupplied = 1;
     846    if(co->HRDataSupplied&2)
     847      return GCOBR_DATAMISMATCH;
     848    co->HRDataSupplied |= 2;
    849849    for(i = CLOCKORBIT_NUMGPS;
    850850    i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
    … …  
    900900    OLD_G_RESERVED6
    901901    OLD_G_NO_OF_SATELLITES(co->NumberOfGPSSat)
    902     if(co->OrbitDataSupplied)
    903       return GCOBR_DATAMISMATCH;
    904     co->OrbitDataSupplied = 1;
     902    if(co->OrbitDataSupplied&1)
     903      return GCOBR_DATAMISMATCH;
     904    co->OrbitDataSupplied |= 1;
    905905    for(i = 0; i < co->NumberOfGPSSat; ++i)
    906906    {
    … …  
    928928    OLD_G_RESERVED6
    929929    OLD_G_NO_OF_SATELLITES(co->NumberOfGPSSat)
    930     if(co->ClockDataSupplied)
    931       return GCOBR_DATAMISMATCH;
    932     co->ClockDataSupplied = 1;
     930    if(co->ClockDataSupplied&1)
     931      return GCOBR_DATAMISMATCH;
     932    co->ClockDataSupplied |= 1;
    933933    for(i = 0; i < co->NumberOfGPSSat; ++i)
    934934    {
    … …  
    948948    OLD_G_RESERVED6
    949949    OLD_G_NO_OF_SATELLITES(co->NumberOfGPSSat)
    950     if(co->ClockDataSupplied || co->OrbitDataSupplied)
    951       return GCOBR_DATAMISMATCH;
    952     co->OrbitDataSupplied = 1;
    953     co->ClockDataSupplied = 1;
     950    if((co->ClockDataSupplied&1) || (co->OrbitDataSupplied&1))
     951      return GCOBR_DATAMISMATCH;
     952    co->OrbitDataSupplied |= 1;
     953    co->ClockDataSupplied |= 1;
    954954    for(i = 0; i < co->NumberOfGPSSat; ++i)
    955955    {
    … …  
    978978    OLD_G_RESERVED6
    979979    OLD_G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    980     if(co->OrbitDataSupplied)
    981       return GCOBR_DATAMISMATCH;
    982     co->OrbitDataSupplied = 1;
     980    if(co->OrbitDataSupplied&2)
     981      return GCOBR_DATAMISMATCH;
     982    co->OrbitDataSupplied |= 2;
    983983    for(i = CLOCKORBIT_NUMGPS;
    984984    i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
    … …  
    10051005    OLD_G_RESERVED6
    10061006    OLD_G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    1007     if(co->ClockDataSupplied)
    1008       return GCOBR_DATAMISMATCH;
    1009     co->ClockDataSupplied = 1;
     1007    if(co->ClockDataSupplied&2)
     1008      return GCOBR_DATAMISMATCH;
     1009    co->ClockDataSupplied |= 2;
    10101010    for(i = CLOCKORBIT_NUMGPS;
    10111011    i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
    … …  
    10241024    OLD_G_RESERVED6
    10251025    OLD_G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    1026     if(co->ClockDataSupplied || co->OrbitDataSupplied)
    1027       return GCOBR_DATAMISMATCH;
    1028     co->OrbitDataSupplied = 1;
    1029     co->ClockDataSupplied = 1;
     1026    if((co->ClockDataSupplied&2) || (co->OrbitDataSupplied&2))
     1027      return GCOBR_DATAMISMATCH;
     1028    co->OrbitDataSupplied |= 2;
     1029    co->ClockDataSupplied |= 2;
    10301030    for(i = CLOCKORBIT_NUMGPS;
    10311031    i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
Note: See TracChangeset for help on using the changeset viewer.