diff --git a/src/rcv/septentrio.c b/src/rcv/septentrio.c index 175ccb4ba..9c8e8a8c0 100644 --- a/src/rcv/septentrio.c +++ b/src/rcv/septentrio.c @@ -720,7 +720,7 @@ static int decode_measepoch(raw_t *raw) fcn = 0; if (sig == 31) sig = (info>>3)+32; else if (sig>=8 && sig<=11) fcn = (info>>3)-8; - raw->obuf.data[n].freq = fcn+8; + raw->obuf.data[n].freq = fcn+7; if (ant != ant_sel) { trace(3, "sbf measepoch ant error: svid=%d ant=%d\n", svid, ant); @@ -822,7 +822,6 @@ static int decode_measepochextra(raw_t *raw) uint8_t n_chn, sbLen, i, misc; uint8_t code, type, chn; uint16_t revision; - double codeVar, carrierVar, pstd; int sig, n, idx, sat, ret = 0; int ant_sel = 0; /* antenna selection (0:main) */ @@ -844,6 +843,9 @@ static int decode_measepochextra(raw_t *raw) n_chn = U1 (p+6); sbLen = U1(p+7); + int rcvstds = 0; + if (strstr(raw->opt, "-RCVSTDS")) rcvstds = 1; + for (i = 0; i < n_chn; i++) { uint8_t *p_chan = p+12+i*sbLen; @@ -867,20 +869,35 @@ static int decode_measepochextra(raw_t *raw) continue; } - codeVar = U2(p_chan+6)/10000.; /* in meters^2 */ - carrierVar = U2(p_chan+8)/1000.; /* in cycles^2 */ + /* Write rcvr stdevs to unused RINEX fields */ + if (rcvstds) { + uint16_t codeVarU = U2(p_chan + 6); + if (codeVarU != 65535) { + double codeVar = codeVarU / 10000.; /* meters^2 */ + // To RTKlib encoding + // TODO starting from 2 rather than 5 would better suit here + double pstd = log2(sqrt(codeVar) * 100) - 5; + pstd = pstd > 0 ? pstd : 0; + /* Further limited to 9 in RINEX output */ + pstd = pstd <= 254 ? pstd : 254; + raw->obuf.data[n].Pstd[idx] = pstd + 0.5; + } - if (codeVar != 65535) { - pstd = log2(sqrt(codeVar)*100)-5; - raw->obuf.data[n].Pstd[idx] = pstd>0 ? pstd : 0; - } - if (carrierVar != 65535) { - raw->obuf.data[n].Lstd[idx] = sqrt(carrierVar)/0.004; + uint16_t carrierVarU = U2(p_chan + 8); + if (carrierVarU != 65535) { + double carrierVar = carrierVarU / 1000000.; /* cycles^2 */ + // To RTKlib encoding + double lstd = sqrt(carrierVar) / 0.004; + lstd = lstd > 0 ? lstd : 0; + /* Further limited to 9 in RINEX output */ + lstd = lstd <= 254 ? lstd : 254; + raw->obuf.data[n].Lstd[idx] = lstd + 0.5; + } } if ((revision >= 3) && (sbLen >= 16)) { /* later revision contains high-resolution extension for CN0 values*/ misc = U1(p_chan+15); - raw->obuf.data[n].SNR[idx] += (uint16_t)(((misc & 0x7 ) * 0.0312)/SNR_UNIT+0.5); + raw->obuf.data[n].SNR[idx] = (uint16_t)((raw->obuf.data[n].SNR[idx] * SNR_UNIT + (misc & 0x7) * 0.03125) / SNR_UNIT + 0.5); } } @@ -972,8 +989,8 @@ static int decode_meas3ranges(raw_t *raw) { } idx_navsys += nB; - // read glonass fcn list - if (navsys == 1) { // glonass + // Read GLONASS fcn list + if (navsys == 1) { // GLONASS memcpy(gloFncs, p_navsys+idx_navsys, (nSats+1) / 2); idx_navsys += (nSats+1) / 2; } else if (navsys == 3) { // BDS @@ -1016,7 +1033,10 @@ static int decode_meas3ranges(raw_t *raw) { if ((satMask & (1ULL << svid)) == 0) continue; - int8_t glofnc = (int)((gloFncs[satCnt / 2] >> (4 * (satCnt % 2))) & 0xf) - 8; + int8_t glofnc = 0; + if (navsys == 1) { // GLONASS + glofnc = (int)((gloFncs[satCnt / 2] >> (4 * (satCnt % 2))) & 0xf) - 8; + } int satNo; int masterFreqIndex; uint8_t codeMaster; @@ -1057,7 +1077,7 @@ static int decode_meas3ranges(raw_t *raw) { raw->obuf.data[n].LLI[masterFreqIndex] = (lockTime < raw->lockt[satNo-1][masterFreqIndex] ? LLI_SLIP : 0) | (lti3 == 0 ? LLI_HALFC : 0); raw->lockt[satNo-1][masterFreqIndex] = lockTime; - raw->obuf.data[n].freq = glofnc+8; + raw->obuf.data[n].freq = glofnc+7; meas3_freqAssignment[navsys][svid][0] = masterFreqIndex; }; @@ -1096,14 +1116,14 @@ static int decode_meas3ranges(raw_t *raw) { uint8_t isGPSPCode = (navsys == 0) && (codeMaster == CODE_L1W || codeMaster == CODE_L2W); raw->obuf.data[n].P[masterFreqIndex] = ((double)prLsb + 4294967296.0 * (double)prMsb) * .001; - raw->obuf.data[n].SNR[masterFreqIndex] = (uint16_t)((isGPSPCode ? (float)CN0 : (float)CN0 + 10.0F)/SNR_UNIT+0.5); + raw->obuf.data[n].SNR[masterFreqIndex] = (uint16_t)((isGPSPCode ? CN0 : CN0 + 10.0) / SNR_UNIT + 0.5); raw->obuf.data[n].code[masterFreqIndex] = codeMaster; if (cmc != 0) raw->obuf.data[n].L[masterFreqIndex] = raw->obuf.data[n].P[masterFreqIndex] / (CLIGHT/freqMaster) - 2097.152 + (double)cmc * .001; raw->obuf.data[n].LLI[masterFreqIndex] = (lockTime < raw->lockt[satNo-1][masterFreqIndex] ? LLI_SLIP : 0) | (lti4 == 0 ? LLI_HALFC : 0); raw->lockt[satNo-1][masterFreqIndex] = lockTime; - raw->obuf.data[n].freq = glofnc+8; + raw->obuf.data[n].freq = glofnc+7; meas3_freqAssignment[navsys][svid][0] = masterFreqIndex; }; @@ -1131,7 +1151,7 @@ static int decode_meas3ranges(raw_t *raw) { raw->obuf.data[n].P[masterFreqIndex] = master_reference->P[masterFreqIndex] + ((int64_t)meas3_refEpoch.prRate[navsys][svid] * 64 * (int32_t)(TOW % refEpochInterval) / 1000) * .001 + (double)pr * .001 - 65.536; - raw->obuf.data[n].SNR[masterFreqIndex] = (uint16_t)(master_reference->SNR[masterFreqIndex] - 4/SNR_UNIT + (float)CN0/SNR_UNIT+0.5); + raw->obuf.data[n].SNR[masterFreqIndex] = (uint16_t)((master_reference->SNR[masterFreqIndex] * SNR_UNIT - 4.0 + CN0) / SNR_UNIT + 0.5); raw->obuf.data[n].code[masterFreqIndex] = codeMaster; if (cmc != 0) raw->obuf.data[n].L[masterFreqIndex] = (raw->obuf.data[n].P[masterFreqIndex] - master_reference->P[masterFreqIndex]) / (CLIGHT/freqMaster) + @@ -1139,7 +1159,7 @@ static int decode_meas3ranges(raw_t *raw) { raw->obuf.data[n].LLI[masterFreqIndex] = master_reference->LLI[masterFreqIndex]; raw->lockt[satNo-1][masterFreqIndex] = meas3_refEpoch.lockt[navsys][svid][masterFreqIndex]; - raw->obuf.data[n].freq = glofnc+8; + raw->obuf.data[n].freq = glofnc+7; meas3_freqAssignment[navsys][svid][0] = masterFreqIndex; } @@ -1162,8 +1182,7 @@ static int decode_meas3ranges(raw_t *raw) { raw->obuf.data[n].P[masterFreqIndex] = masterReference->P[masterFreqIndex] + ((int64_t)meas3_refEpoch.prRate[navsys][svid] * 64 * (int32_t)(TOW % refEpochInterval) / 1000) * .001 + (double)pr * .001 - 8.192; if (cmc != 0) raw->obuf.data[n].L[masterFreqIndex] = (raw->obuf.data[n].P[masterFreqIndex] - masterReference->P[masterFreqIndex]) / (CLIGHT/freqMaster) + masterReference->L[masterFreqIndex] - 8.192 + (double)cmc * .001; - raw->obuf.data[n].SNR[masterFreqIndex] = (uint16_t)(masterReference->SNR[masterFreqIndex] - 1/SNR_UNIT + ((float)CN0)/SNR_UNIT+0.5); - + raw->obuf.data[n].SNR[masterFreqIndex] = (uint16_t)((masterReference->SNR[masterFreqIndex] * SNR_UNIT - 1.0 + CN0) / SNR_UNIT + 0.5); raw->obuf.data[n].LLI[masterFreqIndex] = masterReference->LLI[masterFreqIndex]; raw->lockt[satNo-1][masterFreqIndex] = meas3_refEpoch.lockt[navsys][svid][masterFreqIndex]; raw->obuf.data[n].code[masterFreqIndex] = codeMaster; @@ -1234,10 +1253,9 @@ static int decode_meas3ranges(raw_t *raw) { - 32.768 + cmcRes * .001; if ((navsys == 0) && (codeSlave == CODE_L1W || codeSlave == CODE_L2W)) - raw->obuf.data[n].SNR[slaveFreqIndex] = (uint16_t)(raw->obuf.data[n].SNR[masterFreqIndex] - 3.0/SNR_UNIT - (float)CN0/SNR_UNIT+0.5); + raw->obuf.data[n].SNR[slaveFreqIndex] = (uint16_t)((raw->obuf.data[n].SNR[masterFreqIndex] * SNR_UNIT - 3.0 - CN0) / SNR_UNIT + 0.5); else - raw->obuf.data[n].SNR[slaveFreqIndex] = (uint16_t)((CN0 + 24.0F)/SNR_UNIT+0.5); - + raw->obuf.data[n].SNR[slaveFreqIndex] = (uint16_t)((CN0 + 24.0) / SNR_UNIT + 0.5); raw->obuf.data[n].code[slaveFreqIndex] = codeSlave; raw->obuf.data[n].LLI[slaveFreqIndex] = (lockTime < raw->lockt[satNo-1][slaveFreqIndex] ? LLI_SLIP : 0) | (lti3 == 0 ? LLI_HALFC : 0); @@ -1250,7 +1268,7 @@ static int decode_meas3ranges(raw_t *raw) { /* Slave Long */ uint32_t BF1 = U4(p+idx); uint16_t prLsbRel = U2(p+idx + 4); - uint8_t BF3 = U1(p+6); + uint8_t BF3 = U1(p+idx + 6); uint32_t cmc = (BF1 >> 2) & 0x3fffff; uint32_t lti4 = (BF1 >> 24) & 0xf; @@ -1266,9 +1284,9 @@ static int decode_meas3ranges(raw_t *raw) { raw->obuf.data[n].L[slaveFreqIndex] = raw->obuf.data[n].P[slaveFreqIndex] / (CLIGHT/freqSlave) - 2097.152 + cmc * 0.001; if ((navsys == 0) && (codeSlave == CODE_L1W || codeSlave == CODE_L2W)) - raw->obuf.data[n].SNR[slaveFreqIndex] = (uint16_t)(CN0/SNR_UNIT+0.5); + raw->obuf.data[n].SNR[slaveFreqIndex] = (uint16_t)(CN0 / SNR_UNIT + 0.5); else - raw->obuf.data[n].SNR[slaveFreqIndex] = (uint16_t)((CN0 + 10.0)/SNR_UNIT+0.5); //FIXME + raw->obuf.data[n].SNR[slaveFreqIndex] = (uint16_t)((CN0 + 10.0) / SNR_UNIT + 0.5); raw->obuf.data[n].code[slaveFreqIndex] = codeSlave; raw->obuf.data[n].LLI[slaveFreqIndex] = (lockTime < raw->lockt[satNo-1][slaveFreqIndex] ? LLI_SLIP : 0) | (lti4 == 0 ? LLI_HALFC : 0); @@ -1347,7 +1365,7 @@ int32_t meas3_DopplerPrRate(raw_t* raw, uint32_t *offset) { int32_t prRate; - uint32_t value = *((uint32_t*)(raw->buff + 16 + *offset)); + uint32_t value = U4(raw->buff + 16 + *offset); if ((value & 2) == 0) { @@ -1405,8 +1423,6 @@ int decode_meas3Doppler(raw_t* raw) continue; sys = satsys(raw->obuf.data[n].sat, &prn); - if (prn > MEAS3_SAT_MAX) - continue; for (navsys = 0; navsys < 7; navsys++) if (Meas3_NavSys[navsys] == sys) @@ -1415,17 +1431,23 @@ int decode_meas3Doppler(raw_t* raw) continue; int svid = prn - Meas3_SVIDBase[navsys]; + if (svid >= MEAS3_SAT_MAX) { + // Give up on the remainder, sync with the buffer offset would be lost. + trace(1, "SBF decode_meas3Doppler: svid=%d out of bounds %d\n", svid, MEAS3_SAT_MAX); + return ret; + } masterFreqIndex = meas3_freqAssignment[navsys][svid][0]; - double freqMaster = code2freq(sys, raw->obuf.data[n].code[masterFreqIndex], raw->obuf.data[n].freq); + double freqMaster = code2freq(sys, raw->obuf.data[n].code[masterFreqIndex], raw->obuf.data[n].freq - 7); raw->obuf.data[n].D[masterFreqIndex] = (float)(-(prRate + (int32_t)meas3_refEpoch.prRate[navsys][svid] * 64) * 0.001 / (CLIGHT/freqMaster)); for (int i = 0; iobuf.data[n].code[slaveFreqIndex], raw->obuf.data[n].freq); + prRate = meas3_DopplerPrRate(raw, &offset); + double freqSlave = code2freq(sys, raw->obuf.data[n].code[slaveFreqIndex], raw->obuf.data[n].freq - 7); raw->obuf.data[n].D[slaveFreqIndex] = (float)((raw->obuf.data[n].D[masterFreqIndex] * (CLIGHT/freqMaster) * 1000.0 - prRate) * 0.001 / (CLIGHT/freqSlave)); } } @@ -1451,12 +1473,10 @@ int decode_meas3CN(raw_t* raw) else if (strstr(raw->opt, "-AUX2")) ant_sel = 2; if ((flags & 0x7) != ant_sel) return 0; - for (n = 0; n < raw->obuf.n && offset+16 < (uint32_t)raw->len; n++) { + for (n = 0; n < raw->obuf.n && offset/2+16 < (uint32_t)raw->len; n++) { int navsys, sys, prn; int8_t masterFreqIndex, slaveFreqIndex; sys = satsys(raw->obuf.data[n].sat, &prn); - if (prn > MEAS3_SAT_MAX) - continue; for (navsys = 0; navsys < 7; navsys++) if (Meas3_NavSys[navsys] == sys) @@ -1465,10 +1485,16 @@ int decode_meas3CN(raw_t* raw) continue; int svid = prn - Meas3_SVIDBase[navsys]; + if (svid >= MEAS3_SAT_MAX) { + // Give up correcting the remainder. + trace(1, "SBF decode_meas3CN: svid=%d out of bounds %d\n", svid, MEAS3_SAT_MAX); + return ret; + } masterFreqIndex = meas3_freqAssignment[navsys][svid][0]; - raw->obuf.data[n].SNR[masterFreqIndex] += (float)(((U1(raw->buff + 16 + offset / 2) >> ((offset % 2) * 4)) & 0xf) * .0625F - 0.5F)/SNR_UNIT + 0.5; + uint8_t mc = (U1(raw->buff + 16 + offset / 2) >> ((offset % 2) * 4)) & 0xf; + raw->obuf.data[n].SNR[masterFreqIndex] += (mc * 0.0625 - 0.5) / SNR_UNIT + 0.5; offset++; for (int i = 0; inav.geph[prn-1] = eph; raw->ephsat = sat; - raw->nav.glo_fcn[prn-1] = eph.frq + 8; /* savbe frequency number */ + raw->nav.glo_fcn[prn-1] = eph.frq + 8; /* save frequency number */ return 2; } @@ -3247,13 +3273,13 @@ static int decode_sbsprnmask(raw_t *raw) for (n=0; nnav.sbssat.nsat && nnav.sbssat.sat[n].sat = sat; } @@ -3924,6 +3950,7 @@ static int sync_sbf(unsigned char *buff, unsigned char data) * -GALFNAV : select F/NAV for Galileo ephemeris (default: all) * -NO_MEAS2: ignore range measurements version 2 blocks * -NO_MEAS3: ignore range measurements version 3 blocks +* -RCVSTDS : save receiver stdevs to unused RINEX fields *-----------------------------------------------------------------------------*/ extern int input_sbf(raw_t *raw, unsigned char data) { diff --git a/src/rcv/ublox.c b/src/rcv/ublox.c index 73b8ddf26..451acf9c8 100644 --- a/src/rcv/ublox.c +++ b/src/rcv/ublox.c @@ -511,8 +511,6 @@ static int decode_rxmrawx(raw_t *raw) } n++; } - prstd=prstd<=9?prstd:9; /* limit to 9 to fit RINEX format */ - cpstd=cpstd<=9?cpstd:9; /* limit to 9 to fit RINEX format */ raw->obs.data[j].L[idx]=L; raw->obs.data[j].P[idx]=P; raw->obs.data[j].Lstd[idx]=rcvstds?cpstd:0; diff --git a/src/rinex.c b/src/rinex.c index b8e2d0b53..2891c124b 100644 --- a/src/rinex.c +++ b/src/rinex.c @@ -2245,7 +2245,7 @@ static void outrnxobsf(FILE *fp, double obs, int lli, int std) else { fprintf(fp,"%1.1d",lli&(LLI_SLIP|LLI_HALFC|LLI_BOCTRK)); } - if (std<=0) fprintf(fp," "); else fprintf(fp,"%1.1x",std); + if (std<=0) fprintf(fp," "); else fprintf(fp,"%1.1x", std > 9 ? 9 : std); } /* search observation data index -------------------------------------------*/ static int obsindex(int rnxver, int sys, const uint8_t *code, const char *tobs, diff --git a/src/rtklib.h b/src/rtklib.h index 7ccc67c0d..b30fd50ff 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -656,7 +656,7 @@ typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ typedef struct { /* GLONASS broadcast ephemeris type */ int sat; /* satellite number */ int iode; /* IODE (0-6 bit of tb field) */ - int frq; /* satellite frequency number */ + int frq; /* Satellite frequency number (-7 to 13) */ int svh,sva,age; /* satellite health, accuracy, age of operation */ gtime_t toe; /* epoch of ephemerides (gpst) */ gtime_t tof; /* message frame time (gpst) */