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


Ignore:
Timestamp:
Apr 14, 2010, 4:16:23 PM (14 years ago)
Author:
mervart
Message:

* empty log message *

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/RTCM3/clock_orbit_rtcm.c

    r2425 r2427  
    33        Name:           clock_orbit_rtcm.c
    44        Project:        RTCM3
    5         Version:        $Id: clock_orbit_rtcm.c,v 1.19 2010/04/14 13:11:26 mervart Exp $
     5        Version:        $Id: clock_orbit_rtcm.c,v 1.20 2010/04/14 14:13:45 mervart Exp $
    66        Authors:        Dirk Stöcker
    77        Description:    state space approach for RTCM3
    88*/
    99
    10 #include <math.h>
    1110#include <stdio.h>
    1211#include <string.h>
    … …  
    9493/* standard values */
    9594#define T_MESSAGE_NUMBER(a)              ADDBITS(12, a) /* DF002 */
    96 #define T_RESERVED4                      ADDBITS(1, 0)  /* DF001 */
    9795#define T_RESERVED5                      ADDBITS(5, 0)  /* DF001 */
    9896#define T_GPS_SATELLITE_ID(a)            ADDBITS(6, a)  /* DF068 */
    … …  
    107105#define T_DELTA_DOT_ALONG_TRACK(a)       SCALEADDBITS(19,   250000.0, a)
    108106#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)
    109111
    110112#define T_SATELLITE_REFERENCE_DATUM(a)   ADDBITS(1, a)
    … …  
    115117#define T_GPS_SIGNAL_IDENTIFIER(a)       ADDBITS(5, a)
    116118#define T_GLONASS_SIGNAL_IDENTIFIER(a)   ADDBITS(5, a)
     119#define T_GALILEO_SIGNAL_IDENTIFIER(a)   ADDBITS(5, a)
    117120#define T_CODE_BIAS(a)                   SCALEADDBITS(14,      100.0, a)
    118121#define T_GLONASS_SATELLITE_ID(a)        ADDBITS(5, a)
    … …  
    122125#define T_NO_OF_SATELLITES(a)            ADDBITS(6, a)
    123126#define T_MULTIPLE_MESSAGE_INDICATOR(a)  ADDBITS(1, a)
    124 #define T_SSR_URA(a)                     ADDBITS(6, a)
     127#define T_SSR_URA(a)                     ADDBITS(4, a)
    125128#define T_HR_CLOCK_CORRECTION(a)         SCALEADDBITS(22,    10000.0, a)
    126129#define T_SSR_UPDATE_INTERVAL(a)         ADDBITS(4, a)
    127 
    128 static 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 
    140 static 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 }
    151130
    152131size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
    … …  
    205184    T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
    206185    --mmi;
    207     T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    208     T_RESERVED4
     186    T_RESERVED5
    209187    T_NO_OF_SATELLITES(co->NumberOfGPSSat)
    210188    for(i = 0; i < co->NumberOfGPSSat; ++i)
    … …  
    218196      T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    219197      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)
    220203    }
    221204    ENDBLOCK
    … …  
    261244      T_MULTIPLE_MESSAGE_INDICATOR(/*mmi || */ left ? 1 : 0)
    262245      --mmi;
    263       T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    264       T_RESERVED4
     246      T_RESERVED5
    265247      T_NO_OF_SATELLITES(nums)
    266248      for(i = start; i < start+nums; ++i)
    … …  
    274256        T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    275257        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)
    276263        T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
    277264        T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
    … …  
    313300    {
    314301      T_GPS_SATELLITE_ID(co->Sat[i].ID)
    315       T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
     302      T_SSR_URA(co->Sat[i].URA)
    316303    }
    317304    ENDBLOCK
    … …  
    325312    T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
    326313    --mmi;
    327     T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    328     T_RESERVED4
     314    T_RESERVED5
    329315    T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    330316    for(i = CLOCKORBIT_NUMGPS;
    … …  
    339325      T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    340326      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)
    341332    }
    342333    ENDBLOCK
    … …  
    371362    T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
    372363    --mmi;
    373     T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    374     T_RESERVED4
     364    T_RESERVED5
    375365    T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    376366    for(i = CLOCKORBIT_NUMGPS;
    … …  
    385375      T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    386376      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)
    387382      T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
    388383      T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
    … …  
    422417    {
    423418      T_GPS_SATELLITE_ID(co->Sat[i].ID)
    424       T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
     419      T_SSR_URA(co->Sat[i].URA)
    425420    }
    426421    ENDBLOCK
    … …  
    537532#define G_SIZE(a)                        GETBITS(a, 10)
    538533#define G_MESSAGE_NUMBER(a)              GETBITS(a, 12) /* DF002 */
    539 #define G_RESERVED4                      SKIPBITS(4)    /* DF001 */
    540534#define G_RESERVED5                      SKIPBITS(5)    /* DF001 */
    541535#define G_GPS_SATELLITE_ID(a)            GETBITS(a, 6)  /* DF068 */
    … …  
    550544#define G_DELTA_DOT_ALONG_TRACK(a)       GETFLOATSIGN(a, 19, 1/250000.0)
    551545#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)
    552550
    553551#define G_SATELLITE_REFERENCE_DATUM(a)   GETBITS(a, 1)
    … …  
    558556#define G_GPS_SIGNAL_IDENTIFIER(a)       GETBITS(a, 5)
    559557#define G_GLONASS_SIGNAL_IDENTIFIER(a)   GETBITS(a, 5)
     558#define G_GALILEO_SIGNAL_IDENTIFIER(a)   GETBITS(a, 5)
    560559#define G_CODE_BIAS(a)                   GETFLOATSIGN(a, 14, 1/100.0)
    561560#define G_GLONASS_SATELLITE_ID(a)        GETBITS(a, 5)
    … …  
    567566#define G_NO_OF_SATELLITES(a)            GETBITS(a, 6)
    568567#define G_MULTIPLE_MESSAGE_INDICATOR(a)  GETBITS(a, 1)
    569 #define G_SSR_URA(a)                     {int temp; GETBITS(temp, 6) \
    570  (a) = URAToValue(temp);}
     568#define G_SSR_URA(a)                     GETBITS(a, 4)
    571569#define G_HR_CLOCK_CORRECTION(a)         GETFLOATSIGN(a, 22, 1/10000.0)
    572570#define G_SSR_UPDATE_INTERVAL(a)         GETBITS(a, 4)
    … …  
    616614    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    617615    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    618     G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    619     G_RESERVED4
     616    G_RESERVED5
    620617    G_NO_OF_SATELLITES(nums)
    621618    co->OrbitDataSupplied |= 1;
    622619#ifdef DEBUG
    623 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GPSEpochTime,
    624 co->UpdateInterval,mmi,co->NumberOfGPSSat,nums, co->SatRefDatum);
     620fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
     621co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
    625622#endif
    626623    for(i = 0; i < nums; ++i)
    … …  
    640637      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    641638      G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
    642 #ifdef DEBUG
    643 fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
     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
     645fprintf(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",
    644646co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
    645647co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
    646648co->Sat[pos].Orbit.DotDeltaRadial,
    647649co->Sat[pos].Orbit.DotDeltaAlongTrack,
    648 co->Sat[pos].Orbit.DotDeltaCrossTrack);
     650co->Sat[pos].Orbit.DotDeltaCrossTrack,
     651co->Sat[pos].Orbit.DotDotDeltaRadial,
     652co->Sat[pos].Orbit.DotDotDeltaAlongTrack,
     653co->Sat[pos].Orbit.DotDotDeltaCrossTrack,
     654co->SatRefPoint,
     655co->SatRefDatum);
    649656#endif
    650657    }
    … …  
    692699    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    693700    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    694     G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    695     G_RESERVED4
     701    G_RESERVED5
    696702    G_NO_OF_SATELLITES(nums)
    697703    co->OrbitDataSupplied |= 1;
    … …  
    713719      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    714720      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)
    715726      G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
    716727      G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
    … …  
    737748      co->Sat[pos].ID = id;
    738749
    739       G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
     750      G_SSR_URA(co->Sat[pos].URA)
    740751    }
    741752    break;
    … …  
    769780    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    770781    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    771     G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    772     G_RESERVED4
     782    G_RESERVED5
    773783    G_NO_OF_SATELLITES(nums)
    774784    co->OrbitDataSupplied |= 2;
    775785#ifdef DEBUG
    776 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GLONASSEpochTime,
    777 co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums, co->SatRefDatum);
     786fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
     787co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
    778788#endif
    779789    for(i = 0; i < nums; ++i)
    … …  
    793803      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    794804      G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
    795 #ifdef DEBUG
    796 fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
     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
     811fprintf(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",
    797812co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
    798813co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
    799814co->Sat[pos].Orbit.DotDeltaRadial,
    800815co->Sat[pos].Orbit.DotDeltaAlongTrack,
    801 co->Sat[pos].Orbit.DotDeltaCrossTrack);
     816co->Sat[pos].Orbit.DotDeltaCrossTrack,
     817co->Sat[pos].Orbit.DotDotDeltaRadial,
     818co->Sat[pos].Orbit.DotDotDeltaAlongTrack,
     819co->Sat[pos].Orbit.DotDotDeltaCrossTrack,
     820co->SatRefPoint,
     821co->SatRefDatum);
    802822#endif
    803823    }
    … …  
    841861    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    842862    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    843     G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
    844     G_RESERVED4
     863    G_RESERVED5
    845864    G_NO_OF_SATELLITES(nums)
    846865    co->OrbitDataSupplied |= 2;
    … …  
    862881      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    863882      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)
    864888      G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
    865889      G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
    … …  
    884908      co->Sat[pos].ID = id;
    885909
    886       G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
     910      G_SSR_URA(co->Sat[pos].URA)
    887911    }
    888912    break;
Note: See TracChangeset for help on using the changeset viewer.