Changeset 2433 in ntrip for trunk/BNC/RTCM3/clock_orbit_rtcm.c


Ignore:
Timestamp:
Apr 16, 2010, 3:35:50 PM (14 years ago)
Author:
mervart
Message:

* empty log message *

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/RTCM3/clock_orbit_rtcm.c

    r2427 r2433  
    33        Name:           clock_orbit_rtcm.c
    44        Project:        RTCM3
    5         Version:        $Id: clock_orbit_rtcm.c,v 1.20 2010/04/14 14:13:45 mervart Exp $
     5        Version:        $Id: clock_orbit_rtcm.c,v 1.11 2010/04/16 06:14:00 stoecker Exp $
    66        Authors:        Dirk Stöcker
    77        Description:    state space approach for RTCM3
    88*/
    99
     10#include <math.h>
    1011#include <stdio.h>
    1112#include <string.h>
    … …  
    9394/* standard values */
    9495#define T_MESSAGE_NUMBER(a)              ADDBITS(12, a) /* DF002 */
     96#define T_RESERVED4                      ADDBITS(4, 0)  /* DF001 */
    9597#define T_RESERVED5                      ADDBITS(5, 0)  /* DF001 */
    9698#define T_GPS_SATELLITE_ID(a)            ADDBITS(6, a)  /* DF068 */
    … …  
    105107#define T_DELTA_DOT_ALONG_TRACK(a)       SCALEADDBITS(19,   250000.0, a)
    106108#define T_DELTA_DOT_CROSS_TRACK(a)       SCALEADDBITS(19,   250000.0, a)
    107 #define T_DELTA_DOT_DOT_RADIAL(a)        SCALEADDBITS(27, 50000000.0, a)
    108 #define T_DELTA_DOT_DOT_ALONG_TRACK(a)   SCALEADDBITS(25, 12500000.0, a)
    109 #define T_DELTA_DOT_DOT_CROSS_TRACK(a)   SCALEADDBITS(25, 12500000.0, a)
    110 #define T_SATELLITE_REFERENCE_POINT(a)   ADDBITS(1, a)
    111109
    112110#define T_SATELLITE_REFERENCE_DATUM(a)   ADDBITS(1, a)
    … …  
    117115#define T_GPS_SIGNAL_IDENTIFIER(a)       ADDBITS(5, a)
    118116#define T_GLONASS_SIGNAL_IDENTIFIER(a)   ADDBITS(5, a)
    119 #define T_GALILEO_SIGNAL_IDENTIFIER(a)   ADDBITS(5, a)
    120117#define T_CODE_BIAS(a)                   SCALEADDBITS(14,      100.0, a)
    121118#define T_GLONASS_SATELLITE_ID(a)        ADDBITS(5, a)
    … …  
    125122#define T_NO_OF_SATELLITES(a)            ADDBITS(6, a)
    126123#define T_MULTIPLE_MESSAGE_INDICATOR(a)  ADDBITS(1, a)
    127 #define T_SSR_URA(a)                     ADDBITS(4, a)
     124#define T_SSR_URA(a)                     ADDBITS(6, a)
    128125#define T_HR_CLOCK_CORRECTION(a)         SCALEADDBITS(22,    10000.0, a)
    129126#define T_SSR_UPDATE_INTERVAL(a)         ADDBITS(4, a)
     127
     128static double URAToValue(int ura)
     129{
     130  int urac, urav;
     131  urac = ura >> 3;
     132  urav = ura & 7;
     133  if(!ura)
     134    return 0;
     135  else if(ura == 63)
     136    return SSR_MAXURA;
     137  return (pow(3,urac)*(1.0+urav/4.0)-1.0)/1000.0;
     138}
     139
     140static int ValueToURA(double val)
     141{
     142  int ura;
     143  if(!val)
     144    return 0;
     145  else if(val > 5.4665)
     146    return 63;
     147  for(ura = 1; ura < 63 && val > URAToValue(ura); ++ura)
     148    ;
     149  return ura;
     150}
    130151
    131152size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
    … …  
    184205    T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
    185206    --mmi;
    186     T_RESERVED5
     207    T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     208    T_RESERVED4
    187209    T_NO_OF_SATELLITES(co->NumberOfGPSSat)
    188210    for(i = 0; i < co->NumberOfGPSSat; ++i)
    … …  
    196218      T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    197219      T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
    198       T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
    199       T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
    200       T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
    201       T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
    202       T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    203220    }
    204221    ENDBLOCK
    … …  
    225242  if(gpsco)
    226243  {
    227     int nums, left, start = 0;
    228     nums = co->NumberOfGPSSat;
     244#ifdef SPLITBLOCK
     245    int nums = co->NumberOfGPSSat;
     246    int left, start = 0;
    229247    if(nums > 28) /* split block when more than 28 sats */
    230248    {
    … …  
    238256    while(nums)
    239257    {
     258#endif
    240259      INITBLOCK
    241260      T_MESSAGE_NUMBER(COTYPE_GPSCOMBINED)
    242261      T_GPS_EPOCH_TIME(co->GPSEpochTime)
    243262      T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
     263#ifdef SPLITBLOCK
    244264      T_MULTIPLE_MESSAGE_INDICATOR(/*mmi || */ left ? 1 : 0)
     265#else
     266      T_MULTIPLE_MESSAGE_INDICATOR(/*mmi || */ 0)
     267#endif
    245268      --mmi;
    246       T_RESERVED5
     269      T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     270      T_RESERVED4
     271#ifdef SPLITBLOCK
    247272      T_NO_OF_SATELLITES(nums)
    248273      for(i = start; i < start+nums; ++i)
     274#else
     275      T_NO_OF_SATELLITES(co->NumberOfGPSSat)
     276      for(i = 0; i < co->NumberOfGPSSat; ++i)
     277#endif
    249278      {
    250279        T_GPS_SATELLITE_ID(co->Sat[i].ID)
    … …  
    256285        T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    257286        T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
    258         T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
    259         T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
    260         T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
    261         T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
    262         T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    263287        T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
    264288        T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
    … …  
    266290      }
    267291      ENDBLOCK
     292#ifdef SPLITBLOCK
    268293      start += nums;
    269294      nums = left;
    270295      left = 0;
    271296    }
     297#endif
    272298  }
    273299  if(gpshr)
    … …  
    300326    {
    301327      T_GPS_SATELLITE_ID(co->Sat[i].ID)
    302       T_SSR_URA(co->Sat[i].URA)
     328      T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
    303329    }
    304330    ENDBLOCK
    … …  
    312338    T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
    313339    --mmi;
    314     T_RESERVED5
     340    T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     341    T_RESERVED4
    315342    T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    316343    for(i = CLOCKORBIT_NUMGPS;
    … …  
    325352      T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    326353      T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
    327       T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
    328       T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
    329       T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
    330       T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
    331       T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    332354    }
    333355    ENDBLOCK
    … …  
    362384    T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
    363385    --mmi;
    364     T_RESERVED5
     386    T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     387    T_RESERVED4
    365388    T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    366389    for(i = CLOCKORBIT_NUMGPS;
    … …  
    375398      T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    376399      T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
    377       T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
    378       T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
    379       T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
    380       T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
    381       T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    382400      T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
    383401      T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
    … …  
    417435    {
    418436      T_GPS_SATELLITE_ID(co->Sat[i].ID)
    419       T_SSR_URA(co->Sat[i].URA)
     437      T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
    420438    }
    421439    ENDBLOCK
    … …  
    532550#define G_SIZE(a)                        GETBITS(a, 10)
    533551#define G_MESSAGE_NUMBER(a)              GETBITS(a, 12) /* DF002 */
     552#define G_RESERVED4                      SKIPBITS(4)    /* DF001 */
    534553#define G_RESERVED5                      SKIPBITS(5)    /* DF001 */
    535554#define G_GPS_SATELLITE_ID(a)            GETBITS(a, 6)  /* DF068 */
    … …  
    544563#define G_DELTA_DOT_ALONG_TRACK(a)       GETFLOATSIGN(a, 19, 1/250000.0)
    545564#define G_DELTA_DOT_CROSS_TRACK(a)       GETFLOATSIGN(a, 19, 1/250000.0)
    546 #define G_DELTA_DOT_DOT_RADIAL(a)        GETFLOATSIGN(a, 27, 1/50000000.0)
    547 #define G_DELTA_DOT_DOT_ALONG_TRACK(a)   GETFLOATSIGN(a, 25, 1/12500000.0)
    548 #define G_DELTA_DOT_DOT_CROSS_TRACK(a)   GETFLOATSIGN(a, 25, 1/12500000.0)
    549 #define G_SATELLITE_REFERENCE_POINT(a)   GETBITS(a, 1)
    550565
    551566#define G_SATELLITE_REFERENCE_DATUM(a)   GETBITS(a, 1)
    … …  
    556571#define G_GPS_SIGNAL_IDENTIFIER(a)       GETBITS(a, 5)
    557572#define G_GLONASS_SIGNAL_IDENTIFIER(a)   GETBITS(a, 5)
    558 #define G_GALILEO_SIGNAL_IDENTIFIER(a)   GETBITS(a, 5)
    559573#define G_CODE_BIAS(a)                   GETFLOATSIGN(a, 14, 1/100.0)
    560574#define G_GLONASS_SATELLITE_ID(a)        GETBITS(a, 5)
    … …  
    566580#define G_NO_OF_SATELLITES(a)            GETBITS(a, 6)
    567581#define G_MULTIPLE_MESSAGE_INDICATOR(a)  GETBITS(a, 1)
    568 #define G_SSR_URA(a)                     GETBITS(a, 4)
     582#define G_SSR_URA(a)                     {int temp; GETBITS(temp, 6) \
     583 (a) = URAToValue(temp);}
    569584#define G_HR_CLOCK_CORRECTION(a)         GETFLOATSIGN(a, 22, 1/10000.0)
    570585#define G_SSR_UPDATE_INTERVAL(a)         GETBITS(a, 4)
    … …  
    614629    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    615630    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    616     G_RESERVED5
     631    G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     632    G_RESERVED4
    617633    G_NO_OF_SATELLITES(nums)
    618634    co->OrbitDataSupplied |= 1;
    619635#ifdef DEBUG
    620 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
    621 co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
     636fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GPSEpochTime,
     637co->UpdateInterval,mmi,co->NumberOfGPSSat,nums, co->SatRefDatum);
    622638#endif
    623639    for(i = 0; i < nums; ++i)
    … …  
    637653      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    638654      G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
    639       G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
    640       G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
    641       G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
    642       G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
    643       G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    644 #ifdef DEBUG
    645 fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f rp %d rd %d\n",
     655#ifdef DEBUG
     656fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
    646657co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
    647658co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
    648659co->Sat[pos].Orbit.DotDeltaRadial,
    649660co->Sat[pos].Orbit.DotDeltaAlongTrack,
    650 co->Sat[pos].Orbit.DotDeltaCrossTrack,
    651 co->Sat[pos].Orbit.DotDotDeltaRadial,
    652 co->Sat[pos].Orbit.DotDotDeltaAlongTrack,
    653 co->Sat[pos].Orbit.DotDotDeltaCrossTrack,
    654 co->SatRefPoint,
    655 co->SatRefDatum);
     661co->Sat[pos].Orbit.DotDeltaCrossTrack);
    656662#endif
    657663    }
    … …  
    699705    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    700706    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    701     G_RESERVED5
     707    G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     708    G_RESERVED4
    702709    G_NO_OF_SATELLITES(nums)
    703710    co->OrbitDataSupplied |= 1;
    … …  
    719726      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    720727      G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
    721       G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
    722       G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
    723       G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
    724       G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
    725       G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    726728      G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
    727729      G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
    … …  
    748750      co->Sat[pos].ID = id;
    749751
    750       G_SSR_URA(co->Sat[pos].URA)
     752      G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
    751753    }
    752754    break;
    … …  
    780782    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    781783    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    782     G_RESERVED5
     784    G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     785    G_RESERVED4
    783786    G_NO_OF_SATELLITES(nums)
    784787    co->OrbitDataSupplied |= 2;
    785788#ifdef DEBUG
    786 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
    787 co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
     789fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GLONASSEpochTime,
     790co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums, co->SatRefDatum);
    788791#endif
    789792    for(i = 0; i < nums; ++i)
    … …  
    803806      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    804807      G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
    805       G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
    806       G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
    807       G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
    808       G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
    809       G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    810 #ifdef DEBUG
    811 fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f rp %d rd %d\n",
     808#ifdef DEBUG
     809fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
    812810co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
    813811co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
    814812co->Sat[pos].Orbit.DotDeltaRadial,
    815813co->Sat[pos].Orbit.DotDeltaAlongTrack,
    816 co->Sat[pos].Orbit.DotDeltaCrossTrack,
    817 co->Sat[pos].Orbit.DotDotDeltaRadial,
    818 co->Sat[pos].Orbit.DotDotDeltaAlongTrack,
    819 co->Sat[pos].Orbit.DotDotDeltaCrossTrack,
    820 co->SatRefPoint,
    821 co->SatRefDatum);
     814co->Sat[pos].Orbit.DotDeltaCrossTrack);
    822815#endif
    823816    }
    … …  
    861854    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    862855    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    863     G_RESERVED5
     856    G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     857    G_RESERVED4
    864858    G_NO_OF_SATELLITES(nums)
    865859    co->OrbitDataSupplied |= 2;
    … …  
    881875      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    882876      G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
    883       G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
    884       G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
    885       G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
    886       G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
    887       G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    888877      G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
    889878      G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
    … …  
    908897      co->Sat[pos].ID = id;
    909898
    910       G_SSR_URA(co->Sat[pos].URA)
     899      G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
    911900    }
    912901    break;
Note: See TracChangeset for help on using the changeset viewer.