Ignore:
Timestamp:
Sep 20, 2019, 2:51:58 PM (5 years ago)
Author:
stuerze
Message:

IRNSS support is added in RTCM3 decoder, RTCM signal mapping IDs for GLONASS and BDS are updated/extended

File:
1 edited

Legend:

Unmodified
Added
Removed

  • branches/BNC_2.12/src/RTCM3/RTCM3Decoder.cpp

    r8795 r8805  
    243243        {GPS_WAVELENGTH_L1, "1P"},
    244244        {GPS_WAVELENGTH_L1, "1W"},
    245         {0.0, 0}/*{GPS_WAVELENGTH_L1,"1Y"}*/,
     245        {0.0, 0},
    246246        {0.0, 0},
    247247        {0.0, 0},
    … …  
    249249        {GPS_WAVELENGTH_L2, "2P"},
    250250        {GPS_WAVELENGTH_L2, "2W"},
    251         {0.0, 0}/*{GPS_WAVELENGTH_L2,"2Y"}*/,
     251        {0.0, 0},
    252252        {0.0, 0},
    253253        {0.0, 0},
    … …  
    288288        {1.0, "2C"},
    289289        {1.0, "2P"},
    290         {0.0, 0},
    291         {0.0, 0},
    292         {0.0, 0},
    293         {0.0, 0},
    294         {0.0, 0},
    295         {0.0, 0},
    296         {0.0, 0},
    297         {0.0, 0},
    298         {0.0, 0},
     290        {GLO_WAVELENGTH_L1a, "4A"},
     291        {GLO_WAVELENGTH_L1a, "4B"},
     292        {GLO_WAVELENGTH_L1a, "4X"},
     293        {GLO_WAVELENGTH_L2a, "6A"},
     294        {GLO_WAVELENGTH_L2a, "6B"},
     295        {GLO_WAVELENGTH_L2a, "6X"},
     296        {GLO_WAVELENGTH_L3,  "3I"},
     297        {GLO_WAVELENGTH_L3,  "3Q"},
     298        {GLO_WAVELENGTH_L3,  "3X"},
    299299        {0.0, 0},
    300300        {0.0, 0},
    … …  
    346346        {0.0, 0},
    347347        {0.0, 0},
    348         {0.0, 0},
     348        {0.0, 0}
    349349    };
    350350
    … …  
    421421    };
    422422
     423/**
     424 * MSM signal types for IRNSS
     425 *
     426 * NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!
     427 */
     428static struct CodeData irn[RTCM3_MSM_NUMSIG] = {
     429        {0.0, 0},
     430        {0.0, 0},
     431        {0.0, 0},
     432        {0.0, 0},
     433        {0.0, 0},
     434        {0.0, 0},
     435        {0.0, 0},
     436        {IRNSS_WAVELENGTH_S, "9A"},
     437        {0.0, 0},
     438        {0.0, 0},
     439        {0.0, 0},
     440        {0.0, 0},
     441        {0.0, 0},
     442        {0.0, 0},
     443        {0.0, 0},
     444        {0.0, 0},
     445        {0.0, 0},
     446        {0.0, 0},
     447        {0.0, 0},
     448        {0.0, 0},
     449        {0.0, 0},
     450        {IRNSS_WAVELENGTH_L5, "5A"},
     451        {0.0, 0},
     452        {0.0, 0},
     453        {0.0, 0},
     454        {0.0, 0},
     455        {0.0, 0},
     456        {0.0, 0},
     457        {0.0, 0},
     458        {0.0, 0},
     459        {0.0, 0},
     460        {0.0, 0}
     461    };
     462
    423463#define UINT64(c) c ## ULL
    424464
    425465//
    426466////////////////////////////////////////////////////////////////////////////
    427 bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size)
    428     {
     467bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size) {
    429468  bool decoded = false;
    430469  int type, syncf, i;
    … …  
    438477  /* id */
    439478  char sys;
    440   if (type >= 1121)
     479  if      (type >= 1121)
    441480    sys = 'C';
    442481  else if (type >= 1111)
    … …  
    448487  else if (type >= 1081)
    449488    sys = 'R';
    450   else
     489  else if (type >= 1071)
    451490    sys = 'G';
     491  else if (type >=   21) // test
     492    sys = 'I';
    452493
    453494  bncTime CurrentObsTime;
    … …  
    483524    int sigmask, numsat = 0, numsig = 0;
    484525    uint64_t satmask, cellmask, ui;
    485     double rrmod[RTCM3_MSM_NUMSAT];
    486     int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT],
    487         extsat[RTCM3_MSM_NUMSAT];
    488     int ll[RTCM3_MSM_NUMCELLS]/*, hc[RTCM3_MSM_NUMCELLS]*/;
    489     double cnr[RTCM3_MSM_NUMCELLS];
    490     double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS],
    491         dop[RTCM3_MSM_NUMCELLS];
     526    // satellite data
     527    double rrmod[RTCM3_MSM_NUMSAT]; // GNSS sat rough ranges modulo 1 millisecond
     528    int    rrint[RTCM3_MSM_NUMSAT]; // number of integer msecs in GNSS sat rough ranges
     529    int    rdop[RTCM3_MSM_NUMSAT];  // GNSS sat rough phase range rates
     530    int    extsat[RTCM3_MSM_NUMSAT];// extended sat info
     531    // signal data
     532    int    ll[RTCM3_MSM_NUMCELLS];  // lock time indicator
     533    /*int    hc[RTCM3_MSM_NUMCELLS];*/  // half cycle ambiguity indicator
     534    double cnr[RTCM3_MSM_NUMCELLS]; // signal cnr
     535    double cp[RTCM3_MSM_NUMCELLS];  // fine phase range data
     536    double psr[RTCM3_MSM_NUMCELLS]; // fine psr
     537    double dop[RTCM3_MSM_NUMCELLS]; // fine phase range rates
    492538
    493539    SKIPBITS(3 + 7 + 2 + 2 + 1 + 3)
    … …  
    505551    i = numsat * numsig;
    506552    GETBITS64(cellmask, (unsigned )i)
    507 
     553    // satellite data
    508554    switch (type % 10) {
    509555      case 1:
    … …  
    533579        break;
    534580    }
    535 
     581    // signal data
    536582    int numcells = numsat * numsig;
    537583    /** Drop anything which exceeds our cell limit. Increase limit definition
    … …  
    684730              {
    685731                int k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1];
    686                 if (extsat[numsat] < 14) {
    687                   k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1] = 100 + extsat[numsat]
    688                       - 7;
     732                if (extsat[numsat] < 14) { // channel number is available as extended info for MSM5/7
     733                  k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1] = 100 + extsat[numsat] - 7;
    689734                }
    690                 if (k)
    691                   cd.wl = (
    692                       cd.wl == 0.0 ?
    693                           GLO_WAVELENGTH_L1(k - 100) :
    694                           GLO_WAVELENGTH_L2(k - 100));
    695                 else
     735                if (k) {
     736                  if      (cd.wl == 0.0) {
     737                    cd.wl = GLO_WAVELENGTH_L1(k - 100);
     738                  }
     739                  else if (cd.wl == 1.0) {
     740                    cd.wl = GLO_WAVELENGTH_L2(k - 100);
     741                  }
     742                }
     743                else if (!k && cd.wl <= 1) {
    696744                  cd.code = 0;
     745                }
    697746              }
    698747              break;
    … …  
    700749              cd = gal[RTCM3_MSM_NUMSIG - j - 1];
    701750              break;
     751            case 'I':
     752              cd = irn[RTCM3_MSM_NUMSIG - j - 1];
     753              break;             
    702754          }
    703755          if (cd.code) {
    … …  
    748800                if (cp[count] > -1.0 / (1 << 8)) {
    749801                  frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
    750                       + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
    751                           / cd.wl;
     802                                 + (rrmod[numsat] +  rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
    752803                  frqObs->_phaseValid = true;
    753804                  frqObs->_lockTime = lti2sec(type,ll[count]);
    … …  
    768819                if (cp[count] > -1.0 / (1 << 8)) {
    769820                  frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
    770                       + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
    771                           / cd.wl;
     821                                 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
    772822                  frqObs->_phaseValid = true;
    773823                  frqObs->_lockTime = lti2sec(type,ll[count]);
    … …  
    793843                if (cp[count] > -1.0 / (1 << 8)) {
    794844                  frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
    795                       + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
    796                           / cd.wl;
     845                                 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
    797846                  frqObs->_phaseValid = true;
    798847                  frqObs->_lockTime = lti2sec(type,ll[count]);
    … …  
    813862                if (cp[count] > -1.0 / (1 << 8)) {
    814863                  frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
    815                       + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0
    816                           / cd.wl;
     864                                 + (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
    817865                  frqObs->_phaseValid = true;
    818866                  frqObs->_lockTime = lti2sec(type,ll[count]);
    … …  
    834882        }
    835883      }
    836       if (CurrentObs._obs.size() > 0)
     884      if (CurrentObs._obs.size() > 0) {
    837885        _CurrentObsList.push_back(CurrentObs);
     886      }
    838887    }
    839888  }
    … …  
    10021051    i <<= 4;
    10031052    eph._TOC.set(i * 1000);
    1004     GETFLOATSIGN(eph._clock_driftrate, 8,
    1005         1.0 / (double )(1 << 30) / (double )(1 << 25))
    1006     GETFLOATSIGN(eph._clock_drift, 16,
    1007         1.0 / (double )(1 << 30) / (double )(1 << 13))
    1008     GETFLOATSIGN(eph._clock_bias, 22,
    1009         1.0 / (double )(1 << 30) / (double )(1 << 1))
     1053    GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
     1054    GETFLOATSIGN(eph._clock_drift,    16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
     1055    GETFLOATSIGN(eph._clock_bias,     22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
     1056
    10101057    GETBITS(eph._IODC, 10)
    10111058    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
    … …  
    10761123    GETBITS(i, 1)
    10771124    tk += i * 30;
    1078     eph._tki = tk < 3 * 60 * 60 ? tk - 3 * 60 * 60 + 86400 : tk - 3 * 60 * 60;
     1125       eph._tki = tk - 3*60*60;
     1126    if(eph._tki < 0.0) {
     1127      eph._tki += 86400.0;
     1128    }
    10791129    GETBITS(eph._health, 1) /* MSB of Bn*/
    10801130    GETBITS(eph._P2, 1)  /* P2 */
    … …  
    11531203    eph._TOC.set(i * 1000);
    11541204
    1155     GETFLOATSIGN(eph._clock_driftrate, 8,
    1156         1.0 / (double )(1 << 30) / (double )(1 << 25))
    1157     GETFLOATSIGN(eph._clock_drift, 16,
    1158         1.0 / (double )(1 << 30) / (double )(1 << 13))
    1159     GETFLOATSIGN(eph._clock_bias, 22,
    1160         1.0 / (double )(1 << 30) / (double )(1 << 1))
     1205    GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
     1206    GETFLOATSIGN(eph._clock_drift,    16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
     1207    GETFLOATSIGN(eph._clock_bias,     22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
     1208
    11611209    GETBITS(eph._IODE, 8)
    11621210    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
    … …  
    11991247    GETBITS(eph._fitInterval, 1)
    12001248    eph._TOT = 0.9999e9;
    1201     eph._L2PFlag = 0; /* does not exist for QZSS */
     1249
     1250    emit newGPSEph(eph);
     1251    decoded = true;
     1252  }
     1253  return decoded;
     1254}
     1255
     1256//
     1257////////////////////////////////////////////////////////////////////////////
     1258bool RTCM3Decoder::DecodeIRNSSEphemeris(unsigned char* data, int size) {
     1259  bool decoded = false;
     1260
     1261  if (size == 67) {
     1262    t_ephGPS eph;
     1263    int i, week, L5Flag, SFlag;
     1264    uint64_t numbits = 0, bitfield = 0;
     1265
     1266    data += 3; /* header */
     1267    size -= 6; /* header + crc */
     1268    SKIPBITS(12)
     1269
     1270    eph._receptDateTime = currentDateAndTimeGPS();
     1271
     1272    GETBITS(i, 6)
     1273    eph._prn.set('I', i);
     1274    GETBITS(week, 10)
     1275    GETFLOATSIGN(eph._clock_bias,     22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
     1276    GETFLOATSIGN(eph._clock_drift,    16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
     1277    GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
     1278    GETBITS(i, 4)
     1279    eph._ura = accuracyFromIndex(i, eph.type());
     1280    GETBITS(i, 16)
     1281    i <<= 4;
     1282    eph._TOC.set(i * 1000);
     1283    GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 <<  1))
     1284    GETFLOATSIGN(eph._Delta_n, 22, R2R_PI/(double)(1<<30)/(double)(1 << 11))
     1285    // IODCE
     1286    GETBITS(eph._IODE, 8)
     1287    eph._IODC = eph._IODE;
     1288    SKIPBITS(10)
     1289    GETBITS(L5Flag, 1)
     1290    GETBITS(SFlag, 1)
     1291    if      (L5Flag == 0 && SFlag == 0) {
     1292      eph._health = 0.0;
     1293    }
     1294    else if (L5Flag == 0 && SFlag == 1) {
     1295      eph._health = 1.0;
     1296    }
     1297    else if (L5Flag == 1 && SFlag == 0) {
     1298      eph._health = 2.0;
     1299    }
     1300    else if (L5Flag == 1 && SFlag == 1) {
     1301      eph._health = 3.0;
     1302    }
     1303    GETFLOATSIGN(eph._Cuc,  15, 1.0 / (double )(1 << 28))
     1304    GETFLOATSIGN(eph._Cus,  15, 1.0 / (double )(1 << 28))
     1305    GETFLOATSIGN(eph._Cic,  15, 1.0 / (double )(1 << 28))
     1306    GETFLOATSIGN(eph._Cis,  15, 1.0 / (double )(1 << 28))
     1307    GETFLOATSIGN(eph._Crc,  15, 1.0 / (double )(1 <<  4))
     1308    GETFLOATSIGN(eph._Crs,  15, 1.0 / (double )(1 <<  4))
     1309    GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
     1310    GETFLOATSIGN(eph._M0,   32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
     1311    GETBITS(i, 16)
     1312    i <<= 4;
     1313    eph._TOEsec = i;
     1314    bncTime t;
     1315    t.set(i * 1000);
     1316    eph._TOEweek = t.gpsw();
     1317    int numOfRollOvers = int(floor(t.gpsw()/1024.0));
     1318    week += (numOfRollOvers * 1024);
     1319    /* week from HOW, differs from TOC, TOE week, we use adapted value instead */
     1320    if (eph._TOEweek > week + 1 || eph._TOEweek < week - 1) /* invalid week */
     1321      return false;
     1322    GETFLOAT(eph._e,            32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
     1323    GETFLOAT(eph._sqrt_A,       32, 1.0 / (double )(1 << 19))
     1324    GETFLOATSIGN(eph._OMEGA0,   32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
     1325    GETFLOATSIGN(eph._omega,    32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
     1326    GETFLOATSIGN(eph._OMEGADOT, 22, R2R_PI/(double)(1<<30)/(double)(1<<11))
     1327    GETFLOATSIGN(eph._i0,       32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
     1328    SKIPBITS(2)
     1329    SKIPBITS(2)
     1330    eph._TOT = 0.9999e9;
    12021331
    12031332    emit newGPSEph(eph);
    … …  
    12731402    eph._prn.set('E', i, eph._inav ? 1 : 0);
    12741403
    1275     GETBITS(eph._TOEweek, 12)
     1404    GETBITS(eph._TOEweek, 12) //FIXME: roll-over after week 4095!!
    12761405    GETBITS(eph._IODnav, 10)
    12771406    GETBITS(i, 8)
    … …  
    12801409    GETBITSFACTOR(i, 14, 60)
    12811410    eph._TOC.set(1024 + eph._TOEweek, i);
    1282     GETFLOATSIGN(eph._clock_driftrate, 6,
    1283         1.0 / (double )(1 << 30) / (double )(1 << 29))
    1284     GETFLOATSIGN(eph._clock_drift, 21,
    1285         1.0 / (double )(1 << 30) / (double )(1 << 16))
    1286     GETFLOATSIGN(eph._clock_bias, 31,
    1287         1.0 / (double )(1 << 30) / (double )(1 << 4))
     1411    GETFLOATSIGN(eph._clock_driftrate, 6, 1.0 / (double )(1 << 30) / (double )(1 << 29))
     1412    GETFLOATSIGN(eph._clock_drift,    21, 1.0 / (double )(1 << 30) / (double )(1 << 16))
     1413    GETFLOATSIGN(eph._clock_bias,     31, 1.0 / (double )(1 << 30) / (double )(1 << 4))
    12881414    GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
    12891415    GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
    … …  
    13101436      eph._e5aDataInValid = false;
    13111437
    1312       GETFLOATSIGN(eph._BGD_1_5B, 10,
    1313           1.0 / (double )(1 << 30) / (double )(1 << 2))
     1438GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0 / (double )(1 << 30) / (double )(1 << 2))
    13141439      GETBITS(eph._E5bHS, 2)
    13151440      GETBITS(eph._e5bDataInValid, 1)
    … …  
    13641489    i <<= 3;
    13651490    eph._TOC.setBDS(i * 1000);
    1366     GETFLOATSIGN(eph._clock_driftrate, 11,
    1367         1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6))
    1368     GETFLOATSIGN(eph._clock_drift, 22,
    1369         1.0 / (double )(1 << 30) / (double )(1 << 20))
    1370     GETFLOATSIGN(eph._clock_bias, 24,
    1371         1.0 / (double )(1 << 30) / (double )(1 << 3))
     1491    GETFLOATSIGN(eph._clock_driftrate, 11,  1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6))
     1492    GETFLOATSIGN(eph._clock_drift,     22,  1.0 / (double )(1 << 30) / (double )(1 << 20))
     1493    GETFLOATSIGN(eph._clock_bias,      24,  1.0 / (double )(1 << 30) / (double )(1 << 3))
    13721494    GETBITS(eph._AODC, 5)
    13731495    GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6))
    … …  
    15111633        }
    15121634      }
    1513       else if (id >= 1070 && id <= 1229) /* MSM */ {
     1635      else if ((id >= 1070 && id <= 1229) ||
     1636               (id >=   21 && id <=   27)) /* MSM */ {
    15141637        if (DecodeRTCM3MSM(_Message, _BlockSize))
    15151638          decoded = true;
    … …  
    15551678              decoded = true;
    15561679            break;
     1680          case 29:
     1681            if (DecodeIRNSSEphemeris(_Message, _BlockSize))
     1682              decoded = true;
     1683            break;           
    15571684          case 1045:
    15581685          case 1046:
Note: See TracChangeset for help on using the changeset viewer.