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


Ignore:
Timestamp:
Apr 14, 2010, 3:18:56 PM (15 years ago)
Author:
mervart
Message:

* empty log message *

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/RTCM3/clock_orbit_rtcm.c

    r2325 r2425  
    33        Name:           clock_orbit_rtcm.c
    44        Project:        RTCM3
    5         Version:        $Id: clock_orbit_rtcm.c,v 1.9 2010/02/22 13:42:26 stoecker Exp $
     5        Version:        $Id: clock_orbit_rtcm.c,v 1.19 2010/04/14 13:11:26 mervart 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(1, 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
    … …  
    244261      T_MULTIPLE_MESSAGE_INDICATOR(/*mmi || */ left ? 1 : 0)
    245262      --mmi;
    246       T_RESERVED5
     263      T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     264      T_RESERVED4
    247265      T_NO_OF_SATELLITES(nums)
    248266      for(i = start; i < start+nums; ++i)
    … …  
    256274        T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    257275        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)
    263276        T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
    264277        T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
    … …  
    300313    {
    301314      T_GPS_SATELLITE_ID(co->Sat[i].ID)
    302       T_SSR_URA(co->Sat[i].URA)
     315      T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
    303316    }
    304317    ENDBLOCK
    … …  
    312325    T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
    313326    --mmi;
    314     T_RESERVED5
     327    T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     328    T_RESERVED4
    315329    T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    316330    for(i = CLOCKORBIT_NUMGPS;
    … …  
    325339      T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    326340      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)
    332341    }
    333342    ENDBLOCK
    … …  
    362371    T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
    363372    --mmi;
    364     T_RESERVED5
     373    T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     374    T_RESERVED4
    365375    T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
    366376    for(i = CLOCKORBIT_NUMGPS;
    … …  
    375385      T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
    376386      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)
    382387      T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
    383388      T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
    … …  
    417422    {
    418423      T_GPS_SATELLITE_ID(co->Sat[i].ID)
    419       T_SSR_URA(co->Sat[i].URA)
     424      T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
    420425    }
    421426    ENDBLOCK
    … …  
    532537#define G_SIZE(a)                        GETBITS(a, 10)
    533538#define G_MESSAGE_NUMBER(a)              GETBITS(a, 12) /* DF002 */
     539#define G_RESERVED4                      SKIPBITS(4)    /* DF001 */
    534540#define G_RESERVED5                      SKIPBITS(5)    /* DF001 */
    535541#define G_GPS_SATELLITE_ID(a)            GETBITS(a, 6)  /* DF068 */
    … …  
    544550#define G_DELTA_DOT_ALONG_TRACK(a)       GETFLOATSIGN(a, 19, 1/250000.0)
    545551#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)
    550552
    551553#define G_SATELLITE_REFERENCE_DATUM(a)   GETBITS(a, 1)
    … …  
    556558#define G_GPS_SIGNAL_IDENTIFIER(a)       GETBITS(a, 5)
    557559#define G_GLONASS_SIGNAL_IDENTIFIER(a)   GETBITS(a, 5)
    558 #define G_GALILEO_SIGNAL_IDENTIFIER(a)   GETBITS(a, 5)
    559560#define G_CODE_BIAS(a)                   GETFLOATSIGN(a, 14, 1/100.0)
    560561#define G_GLONASS_SATELLITE_ID(a)        GETBITS(a, 5)
    … …  
    566567#define G_NO_OF_SATELLITES(a)            GETBITS(a, 6)
    567568#define G_MULTIPLE_MESSAGE_INDICATOR(a)  GETBITS(a, 1)
    568 #define G_SSR_URA(a)                     GETBITS(a, 4)
     569#define G_SSR_URA(a)                     {int temp; GETBITS(temp, 6) \
     570 (a) = URAToValue(temp);}
    569571#define G_HR_CLOCK_CORRECTION(a)         GETFLOATSIGN(a, 22, 1/10000.0)
    570572#define G_SSR_UPDATE_INTERVAL(a)         GETBITS(a, 4)
    … …  
    614616    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    615617    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    616     G_RESERVED5
     618    G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     619    G_RESERVED4
    617620    G_NO_OF_SATELLITES(nums)
    618621    co->OrbitDataSupplied |= 1;
    619622#ifdef DEBUG
    620 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
    621 co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
     623fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GPSEpochTime,
     624co->UpdateInterval,mmi,co->NumberOfGPSSat,nums, co->SatRefDatum);
    622625#endif
    623626    for(i = 0; i < nums; ++i)
    … …  
    637640      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    638641      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",
     642#ifdef DEBUG
     643fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
    646644co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
    647645co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
    648646co->Sat[pos].Orbit.DotDeltaRadial,
    649647co->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);
     648co->Sat[pos].Orbit.DotDeltaCrossTrack);
    656649#endif
    657650    }
    … …  
    699692    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    700693    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    701     G_RESERVED5
     694    G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     695    G_RESERVED4
    702696    G_NO_OF_SATELLITES(nums)
    703697    co->OrbitDataSupplied |= 1;
    … …  
    719713      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    720714      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)
    726715      G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
    727716      G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
    … …  
    748737      co->Sat[pos].ID = id;
    749738
    750       G_SSR_URA(co->Sat[pos].URA)
     739      G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
    751740    }
    752741    break;
    … …  
    780769    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    781770    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    782     G_RESERVED5
     771    G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     772    G_RESERVED4
    783773    G_NO_OF_SATELLITES(nums)
    784774    co->OrbitDataSupplied |= 2;
    785775#ifdef DEBUG
    786 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
    787 co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
     776fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GLONASSEpochTime,
     777co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums, co->SatRefDatum);
    788778#endif
    789779    for(i = 0; i < nums; ++i)
    … …  
    803793      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    804794      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",
     795#ifdef DEBUG
     796fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
    812797co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
    813798co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
    814799co->Sat[pos].Orbit.DotDeltaRadial,
    815800co->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);
     801co->Sat[pos].Orbit.DotDeltaCrossTrack);
    822802#endif
    823803    }
    … …  
    861841    G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
    862842    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
    863     G_RESERVED5
     843    G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
     844    G_RESERVED4
    864845    G_NO_OF_SATELLITES(nums)
    865846    co->OrbitDataSupplied |= 2;
    … …  
    881862      G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
    882863      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)
    888864      G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
    889865      G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
    … …  
    908884      co->Sat[pos].ID = id;
    909885
    910       G_SSR_URA(co->Sat[pos].URA)
     886      G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
    911887    }
    912888    break;
Note: See TracChangeset for help on using the changeset viewer.