From 49aa5be03456d7a53a534485aea6ec2014aa90c2 Mon Sep 17 00:00:00 2001 From: Our Air Quality Date: Sun, 29 Sep 2024 15:28:04 +1000 Subject: [PATCH] sigindex: common function to resolve the frequence index The frequency index returned by code2idx() is not unique and conflicts need to be resolved. There were different implementations across the code, RINEX, RTCM, and the raw receiver decoders. Add a common exported function to implement this resolution. sigindex(..., opt) allocates an frequency index given a signal code, returning the allocated signal index on success of -1 on failure. Conflicts are resolved using the signal priorities that can be modified by the options in the opt argument, moving an existing allocation if necessary. sigindex(..., NULL) uses the same search algorithm as sigindex() but does not allocate a new entry or check the consistency of code priorities. Without the signal priorities, with the opt argument NULL, the allocation ordering could not be determined, so this case is used for this non-allocating variant. Apply this resolution per satellite as the set of signals can vary between satellites. Update the RINEX, RTCM3, Unicore and Novatel format decoders to use this new common function. --- src/rcv/novatel.c | 385 ++++++++++++++++++++-------------------------- src/rcv/unicore.c | 136 ++++++---------- src/rinex.c | 235 ++++++++++------------------ src/rtcm3.c | 67 ++------ src/rtkcmn.c | 97 ++++++++++++ src/rtklib.h | 1 + 6 files changed, 403 insertions(+), 518 deletions(-) diff --git a/src/rcv/novatel.c b/src/rcv/novatel.c index dd615ed22..dbcf595e3 100644 --- a/src/rcv/novatel.c +++ b/src/rcv/novatel.c @@ -120,18 +120,18 @@ #define SQR(x) ((x)*(x)) /* get fields (little-endian) ------------------------------------------------*/ -#define U1(p) (*((uint8_t *)(p))) -#define I1(p) (*((int8_t *)(p))) -static uint16_t U2(uint8_t *p) {uint16_t u; memcpy(&u,p,2); return u;} -static uint32_t U4(uint8_t *p) {uint32_t u; memcpy(&u,p,4); return u;} -static int32_t I4(uint8_t *p) {int32_t i; memcpy(&i,p,4); return i;} -static float R4(uint8_t *p) {float r; memcpy(&r,p,4); return r;} -static double R8(uint8_t *p) {double r; memcpy(&r,p,8); return r;} +#define U1(p) (*((const uint8_t *)(p))) +#define I1(p) (*((const int8_t *)(p))) +static uint16_t U2(const uint8_t *p) {uint16_t u; memcpy(&u,p,2); return u;} +static uint32_t U4(const uint8_t *p) {uint32_t u; memcpy(&u,p,4); return u;} +static int32_t I4(const uint8_t *p) {int32_t i; memcpy(&i,p,4); return i;} +static float R4(const uint8_t *p) {float r; memcpy(&r,p,4); return r;} +static double R8(const uint8_t *p) {double r; memcpy(&r,p,8); return r;} /* extend sign ---------------------------------------------------------------*/ static int32_t exsign(uint32_t v, int bits) { - return (int32_t)(v&(1<<(bits-1))?v|(~0u<buff+OEM4HLEN; - char *q; - double psr,adr,adr_rolls,lockt,tt,dop,snr,freq,glo_bias=0.0; - int i,index,nobs,prn,sat,sys,code,idx,track,plock,clock,parity,halfc,lli; - - if ((q=strstr(raw->opt,"-GLOBIAS="))) sscanf(q,"-GLOBIAS=%lf",&glo_bias); - - nobs=U4(p); - if (raw->lenlen,nobs); - return -1; - } - if (raw->outtype) { - sprintf(raw->msgtype+strlen(raw->msgtype)," nobs=%d",nobs); - } - for (i=0,p+=4;i=MINPRNQZS_S&&prn<=MAXPRNQZS_S&&code==CODE_L1C) { - sys=SYS_QZS; - prn+=10; - code=CODE_L1Z; /* QZS L1S */ - } - if (!(sat=satno(sys,prn))) { - trace(3,"oem4 rangecmpb satellite number error: sys=%d,prn=%d\n",sys,prn); - continue; - } - if (sys==SYS_GLO&&!parity) continue; /* invalid if GLO parity unknown */ - - if ((idx=checkpri(raw->opt,sys,code,idx))<0) continue; - - dop=exsign(U4(p+4)&0xFFFFFFF,28)/256.0; - psr=(U4(p+7)>>4)/128.0+U1(p+11)*2097152.0; - - if ((freq=sat2freq(sat,(uint8_t)code,&raw->nav))!=0.0) { - adr=I4(p+12)/256.0; - adr_rolls=(psr*freq/CLIGHT+adr)/MAXVAL; - adr=-adr+MAXVAL*floor(adr_rolls+(adr_rolls<=0?-0.5:0.5)); - if (sys==SYS_GLO) adr+=glo_bias*freq/CLIGHT; - } - else { - adr=1e-9; - } - lockt=(U4(p+18)&0x1FFFFF)/32.0; /* lock time */ - - if (raw->tobs[sat-1][code].time!=0) { - tt=timediff(raw->time,raw->tobs[sat-1][code]); - lli=(lockt<65535.968&&lockt-raw->lockt[sat-1][code]+0.05<=tt)?LLI_SLIP:0; - } - else { - lli=0; - } - if (!parity) lli|=LLI_HALFC; - if (halfc ) lli|=LLI_HALFA; - raw->tobs [sat-1][code]=raw->time; - raw->lockt[sat-1][code]=lockt; - raw->halfc[sat-1][code]=halfc; - - snr=((U2(p+20)&0x3FF)>>5)+20.0; - if (!clock) psr=0.0; /* code unlock */ - if (!plock) adr=dop=0.0; /* phase unlock */ - - if (fabs(timediff(raw->obs.data[0].time,raw->time))>1E-9) { - raw->obs.n=0; - } - if ((index=obsindex(&raw->obs,raw->time,sat))>=0) { - raw->obs.data[index].L [idx]=adr; - raw->obs.data[index].P [idx]=psr; - raw->obs.data[index].D [idx]=(float)dop; - raw->obs.data[index].SNR[idx]=(uint16_t)(snr/SNR_UNIT+0.5); - raw->obs.data[index].LLI[idx]=(uint8_t)lli; - raw->obs.data[index].code[idx]=(uint8_t)code; - } - } - return 1; +/* Decode RANGECMPB ----------------------------------------------------------*/ +static int decode_rangecmpb(raw_t *raw) { + double glo_bias = 0.0; + const char *q = strstr(raw->opt, "-GLOBIAS="); + if (q) sscanf(q, "-GLOBIAS=%lf", &glo_bias); + + int pi = OEM4HLEN; + int nobs = U4(raw->buff + pi); + if (raw->len < OEM4HLEN + 4 + nobs * 24) { + trace(2, "oem4 rangecmpb length error: len=%d nobs=%d\n", raw->len, nobs); + return -1; + } + if (raw->outtype) { + sprintf(raw->msgtype, " nobs=%d", nobs); + } + + if (fabs(timediff(raw->obs.data[0].time, raw->time)) > 1E-9) raw->obs.n = 0; + + pi = OEM4HLEN + 4; + for (int i = 0; i < nobs; i++, pi += 24) { + int sys, code, track, plock, clock, parity, halfc; + int idx = + decode_track_stat(U4(raw->buff + pi), &sys, &code, &track, &plock, &clock, &parity, &halfc); + if (idx < 0) continue; + int prn = U1(raw->buff + pi + 17); + if (sys == SYS_GLO) prn -= 37; + if (sys == SYS_SBS && prn >= MINPRNQZS_S && prn <= MAXPRNQZS_S && code == CODE_L1C) { + sys = SYS_QZS; + prn += 10; + code = CODE_L1Z; /* QZS L1S */ + } + int sat = satno(sys, prn); + if (!sat) { + trace(3, "oem4 rangecmpb satellite number error: sys=%d,prn=%d\n", sys, prn); + continue; + } + if (sys == SYS_GLO && !parity) continue; /* Invalid if GLO parity unknown */ + + double dop = exsign(U4(raw->buff + pi + 4) & 0xFFFFFFF, 28) / 256.0; + double psr = (U4(raw->buff + pi + 7) >> 4) / 128.0 + U1(raw->buff + pi + 11) * 2097152.0; + + double freq = sat2freq(sat, code, &raw->nav), adr; + if (freq != 0.0) { + adr = I4(raw->buff + pi + 12) / 256.0; + double adr_rolls = (psr * freq / CLIGHT + adr) / MAXVAL; + adr = -adr + MAXVAL * floor(adr_rolls + (adr_rolls <= 0 ? -0.5 : 0.5)); + if (sys == SYS_GLO) adr += glo_bias * freq / CLIGHT; + } else { + adr = 1e-9; + } + double lockt = (U4(raw->buff + pi + 18) & 0x1FFFFF) / 32.0; /* Lock time */ + + int lli; + if (raw->tobs[sat - 1][code].time != 0) { + double tt = timediff(raw->time, raw->tobs[sat - 1][code]); + lli = (lockt < 65535.968 && lockt - raw->lockt[sat - 1][code] + 0.05 <= tt) ? LLI_SLIP : 0; + } else { + lli = 0; + } + if (!parity) lli |= LLI_HALFC; + if (halfc) lli |= LLI_HALFA; + raw->tobs[sat - 1][code] = raw->time; + raw->lockt[sat - 1][code] = lockt; + raw->halfc[sat - 1][code] = halfc; + + double snr = ((U2(raw->buff + pi + 20) & 0x3FF) >> 5) + 20.0; + if (!clock) continue; /* Code unlock */ + if (!plock) adr = dop = 0.0; /* Phase unlock */ + + int index = obsindex(&raw->obs, raw->time, sat); + if (index >= 0) { + int idx = sigindex(raw->obs.data + index, sys, code, raw->opt); + if (idx < 0) continue; + raw->obs.data[index].L[idx] = adr; + raw->obs.data[index].P[idx] = psr; + raw->obs.data[index].D[idx] = (float)dop; + raw->obs.data[index].SNR[idx] = (uint16_t)(snr / SNR_UNIT + 0.5); + raw->obs.data[index].LLI[idx] = (uint8_t)lli; + } + } + return 1; } -/* decode RANGEB -------------------------------------------------------------*/ -static int decode_rangeb(raw_t *raw) -{ - uint8_t *p=raw->buff+OEM4HLEN; - char *q; - double psr,adr,dop,snr,lockt,tt,freq,glo_bias=0.0; - int i,index,nobs,prn,sat,sys,code,idx,track,plock,clock,parity,halfc,lli; - int gfrq; - - if ((q=strstr(raw->opt,"-GLOBIAS="))) sscanf(q,"-GLOBIAS=%lf",&glo_bias); - - nobs=U4(p); - if (raw->lenlen,nobs); - return -1; - } - if (raw->outtype) { - sprintf(raw->msgtype+strlen(raw->msgtype)," nobs=%d",nobs); - } - for (i=0,p+=4;i=MINPRNQZS_S&&prn<=MAXPRNQZS_S&&code==CODE_L1C) { - sys=SYS_QZS; - prn+=10; - code=CODE_L1Z; /* QZS L1S */ - } - if (!(sat=satno(sys,prn))) { - trace(3,"oem4 rangeb satellite number error: sys=%d,prn=%d\n",sys,prn); - continue; - } - if (sys==SYS_GLO&&!parity) continue; - - if ((idx=checkpri(raw->opt,sys,code,idx))<0) continue; - - gfrq =U2(p+ 2); /* GLONASS FCN+8 */ - psr =R8(p+ 4); - adr =R8(p+16); - dop =R4(p+28); - snr =R4(p+32); - lockt=R4(p+36); - - if (sys==SYS_GLO) { - freq=sat2freq(sat,(uint8_t)code,&raw->nav); - adr-=glo_bias*freq/CLIGHT; - if (!raw->nav.glo_fcn[prn-1]) { - raw->nav.glo_fcn[prn-1]=gfrq; /* fcn+8 */ - } - } - if (raw->tobs[sat-1][code].time!=0) { - tt=timediff(raw->time,raw->tobs[sat-1][code]); - lli=lockt-raw->lockt[sat-1][code]+0.05<=tt?LLI_SLIP:0; - } - else { - lli=0; - } - if (!parity) lli|=LLI_HALFC; - if (halfc ) lli|=LLI_HALFA; - raw->tobs [sat-1][code]=raw->time; - raw->lockt[sat-1][code]=lockt; - raw->halfc[sat-1][code]=halfc; - - if (!clock) psr=0.0; /* code unlock */ - if (!plock) adr=dop=0.0; /* phase unlock */ - - if (fabs(timediff(raw->obs.data[0].time,raw->time))>1E-9) { - raw->obs.n=0; - } - if ((index=obsindex(&raw->obs,raw->time,sat))>=0) { - raw->obs.data[index].L [idx]=-adr; - raw->obs.data[index].P [idx]=psr; - raw->obs.data[index].D [idx]=(float)dop; - raw->obs.data[index].SNR[idx]=(uint16_t)(snr/SNR_UNIT+0.5); - raw->obs.data[index].LLI[idx]=(uint8_t)lli; - raw->obs.data[index].code[idx]=(uint8_t)code; - } - } - return 1; +/* Decode RANGEB -------------------------------------------------------------*/ +static int decode_rangeb(raw_t *raw) { + double glo_bias = 0.0; + const char *q = strstr(raw->opt, "-GLOBIAS="); + if (q) sscanf(q, "-GLOBIAS=%lf", &glo_bias); + + int pi = OEM4HLEN; + int nobs = U4(raw->buff + pi); + if (raw->len < OEM4HLEN + 4 + nobs * 44) { + trace(2, "oem4 rangeb length error: len=%d nobs=%d\n", raw->len, nobs); + return -1; + } + if (raw->outtype) { + sprintf(raw->msgtype, " nobs=%d", nobs); + } + + if (fabs(timediff(raw->obs.data[0].time, raw->time)) > 1E-9) raw->obs.n = 0; + + pi = OEM4HLEN + 4; + for (int i = 0; i < nobs; i++, pi += 44) { + int sys, code, track, plock, clock, parity, halfc; + int idx = decode_track_stat(U4(raw->buff + pi + 40), &sys, &code, &track, &plock, &clock, + &parity, &halfc); + if (idx < 0) continue; + int prn = U2(raw->buff + pi); + if (sys == SYS_GLO) prn -= 37; + if (sys == SYS_SBS && prn >= MINPRNQZS_S && prn <= MAXPRNQZS_S && code == CODE_L1C) { + sys = SYS_QZS; + prn += 10; + code = CODE_L1Z; /* QZS L1S */ + } + int sat = satno(sys, prn); + if (!sat) { + trace(3, "oem4 rangeb satellite number error: sys=%d,prn=%d\n", sys, prn); + continue; + } + if (sys == SYS_GLO && !parity) continue; + + int gfrq = U2(raw->buff + pi + 2); /* GLONASS FCN+8 */ + double psr = R8(raw->buff + pi + 4); + double adr = R8(raw->buff + pi + 16); + double dop = R4(raw->buff + pi + 28); + double snr = R4(raw->buff + pi + 32); + double lockt = R4(raw->buff + pi + 36); + + if (sys == SYS_GLO) { + double freq = sat2freq(sat, code, &raw->nav); + adr -= glo_bias * freq / CLIGHT; + if (!raw->nav.glo_fcn[prn - 1]) { + raw->nav.glo_fcn[prn - 1] = gfrq; /* fcn+8 */ + } + } + int lli; + if (raw->tobs[sat - 1][code].time != 0) { + double tt = timediff(raw->time, raw->tobs[sat - 1][code]); + lli = lockt - raw->lockt[sat - 1][code] + 0.05 <= tt ? LLI_SLIP : 0; + } else { + lli = 0; + } + if (!parity) lli |= LLI_HALFC; + if (halfc) lli |= LLI_HALFA; + raw->tobs[sat - 1][code] = raw->time; + raw->lockt[sat - 1][code] = lockt; + raw->halfc[sat - 1][code] = halfc; + + if (!clock) continue; /* Code unlock */ + if (!plock) adr = dop = 0.0; /* Phase unlock */ + + int index = obsindex(&raw->obs, raw->time, sat); + if (index >= 0) { + int idx = sigindex(raw->obs.data + index, sys, code, raw->opt); + if (idx < 0) continue; + raw->obs.data[index].L[idx] = -adr; + raw->obs.data[index].P[idx] = psr; + raw->obs.data[index].D[idx] = (float)dop; + raw->obs.data[index].SNR[idx] = (uint16_t)(snr / SNR_UNIT + 0.5); + raw->obs.data[index].LLI[idx] = (uint8_t)lli; + } + } + return 1; } /* decode RAWEPHEMB ----------------------------------------------------------*/ static int decode_rawephemb(raw_t *raw) @@ -1364,15 +1326,6 @@ static int sync_oem3(uint8_t *buff, uint8_t data) * option strings separated by spaces. * * -EPHALL : input all ephemerides -* -GL1L : select 1L for GPS L1 (default 1C) -* -GL2S : select 2S for GPS L2 (default 2W) -* -GL2P : select 2P for GPS L2 (default 2W) -* -RL2C : select 2C for GLO G2 (default 2P) -* -EL6B : select 6B for GAL E6 (default 6C) -* -JL1L : select 1L for QZS L1 (default 1C) -* -JL1Z : select 1Z for QZS L1 (default 1C) -* -CL1P : select 1P for BDS B1 (default 2I) -* -CL7D : select 7D for BDS B2 (default 7I) * -GALINAV: select I/NAV for Galileo ephemeris (default: all) * -GALFNAV: select F/NAV for Galileo ephemeris (default: all) * -GLOBIAS=bias: GLONASS code-phase bias (m) diff --git a/src/rcv/unicore.c b/src/rcv/unicore.c index 5e6c202be..70ee31113 100644 --- a/src/rcv/unicore.c +++ b/src/rcv/unicore.c @@ -36,13 +36,12 @@ /* get fields (little-endian) ------------------------------------------------*/ -#define U1(p) (*((uint8_t *)(p))) -#define I1(p) (*((int8_t *)(p))) -static uint16_t U2(uint8_t* p) { uint16_t u; memcpy(&u, p, 2); return u; } -static uint32_t U4(uint8_t* p) { uint32_t u; memcpy(&u, p, 4); return u; } -static int32_t I4(uint8_t* p) { int32_t i; memcpy(&i, p, 4); return i; } -static float R4(uint8_t* p) { float r; memcpy(&r, p, 4); return r; } -static double R8(uint8_t* p) { double r; memcpy(&r, p, 8); return r; } +#define U1(p) (*((const uint8_t *)(p))) +#define I1(p) (*((const int8_t *)(p))) +static uint16_t U2(const uint8_t* p) { uint16_t u; memcpy(&u, p, 2); return u; } +static uint32_t U4(const uint8_t* p) { uint32_t u; memcpy(&u, p, 4); return u; } +static float R4(const uint8_t* p) { float r; memcpy(&r, p, 4); return r; } +static double R8(const uint8_t* p) { double r; memcpy(&r, p, 8); return r; } /* sync header ---------------------------------------------------------------*/ @@ -96,42 +95,6 @@ static gtime_t adjweek(gtime_t time, double tow) return gpst2time(week, tow); } -/* check code priority and return freq-index ---------------------------------*/ -static int checkpri(const char* opt, int sys, int code, int idx) -{ - int nex = NEXOBS; - - if (sys == SYS_GPS) { - if (strstr(opt, "-GL1L") && idx == 0) return (code == CODE_L1L) ? 0 : -1; - if (strstr(opt, "-GL2S") && idx == 1) return (code == CODE_L2X) ? 1 : -1; - if (strstr(opt, "-GL2P") && idx == 1) return (code == CODE_L2P) ? 1 : -1; - if (code == CODE_L1L) return (nex < 1) ? -1 : NFREQ; - if (code == CODE_L2S) return (nex < 2) ? -1 : NFREQ + 1; - if (code == CODE_L2P) return (nex < 3) ? -1 : NFREQ + 2; - } - else if (sys == SYS_GLO) { - if (strstr(opt, "-RL2C") && idx == 1) return (code == CODE_L2C) ? 1 : -1; - if (code == CODE_L2C) return (nex < 1) ? -1 : NFREQ; - } - else if (sys == SYS_GAL) { - if (strstr(opt, "-EL6B") && idx == 3) return (code == CODE_L6B) ? 3 : -1; - if (code == CODE_L6B) return (nex < 2) ? -1 : NFREQ; - } - else if (sys == SYS_QZS) { - if (strstr(opt, "-JL1L") && idx == 0) return (code == CODE_L1L) ? 0 : -1; - if (strstr(opt, "-JL1Z") && idx == 0) return (code == CODE_L1Z) ? 0 : -1; - if (code == CODE_L1L) return (nex < 1) ? -1 : NFREQ; - if (code == CODE_L1Z) return (nex < 2) ? -1 : NFREQ + 1; - } - else if (sys == SYS_CMP) { - if (strstr(opt, "-CL1P") && idx == 0) return (code == CODE_L1P) ? 0 : -1; - if (strstr(opt, "-CL7D") && idx == 0) return (code == CODE_L7D) ? 0 : -1; - if (code == CODE_L1P) return (nex < 1) ? -1 : NFREQ; - if (code == CODE_L7D) return (nex < 2) ? -1 : NFREQ + 1; - } - return idx < NFREQ ? idx : -1; -} - /* signal type to obs code ---------------------------------------------------*/ static int sig2code(int sys, int sigtype, int l2c) { @@ -214,20 +177,16 @@ static int sig2code(int sys, int sigtype, int l2c) return 0; } -static int decode_track_stat(uint32_t stat, int* sys, int* code, int* plock, int* clock) +static int decode_track_stat(uint32_t stat, int *sys, int *code, int *plock, int *clock) { - int satsys, sigtype, idx = -1; - int l2c; - *code = CODE_NONE; *plock = (stat >> 10) & 1; *clock = (stat >> 12) & 1; - satsys = (stat >> 16) & 7; - sigtype = (stat >> 21) & 0x1F; - l2c = (stat >> 26) & 0x01; + int sysno = (stat >> 16) & 7; + int sigtype = (stat >> 21) & 0x1F; + int l2c = (stat >> 26) & 0x01; - - switch (satsys) { + switch (sysno) { case 0: *sys = SYS_GPS; break; case 1: *sys = SYS_GLO; break; case 2: *sys = SYS_SBS; break; @@ -239,6 +198,7 @@ static int decode_track_stat(uint32_t stat, int* sys, int* code, int* plock, int trace(2, "unicore unknown system: sys=%d\n", satsys); return -1; } + int idx = -1; if (!(*code = sig2code(*sys, sigtype, l2c)) || (idx = code2idx(*sys, *code)) < 0) { trace(2, "unicore signal type error: sys=%d sigtype=%d\n", *sys, sigtype); return -1; @@ -721,18 +681,14 @@ static int decode_irnssephb(raw_t* raw) { // decode OBSVMB static int decode_obsvmb(raw_t* raw) { - uint8_t* p = raw->buff + HLEN; - char* q; - double psr, adr, dop, snr, lockt, tt, freq, glo_bias = 0.0; - int i, index, nobs, prn, sat, sys, code, idx, track, plock, clock, lli; - int gfrq; - - if ((q = strstr(raw->opt, "-GLOBIAS="))) sscanf(q, "-GLOBIAS=%lf", &glo_bias); + double glo_bias = 0.0; + const char *q = strstr(raw->opt, "-GLOBIAS="); + if (q) sscanf(q, "-GLOBIAS=%lf", &glo_bias); int rcvstds = 0; if (strstr(raw->opt, "-RCVSTDS")) rcvstds = 1; - nobs = U4(p); + int nobs = U4(raw->buff + HLEN); if (nobs == 0)return 1; if (raw->len < HLEN + 4 + nobs * 40) { trace(2, "unicore obsvmb length error: len=%d nobs=%d\n", raw->len, nobs); @@ -742,73 +698,71 @@ static int decode_obsvmb(raw_t* raw) sprintf(raw->msgtype + strlen(raw->msgtype), " nobs=%d", nobs); } - p += 4; // Number of observation messages + if (fabs(timediff(raw->obs.data[0].time, raw->time)) > 1E-9) + raw->obs.n = 0; - for (i = 0; i < nobs; i++, p += 40) { - uint32_t trk_stat = U4(p + 36); - - if ((idx = decode_track_stat(trk_stat, &sys, &code, &plock, &clock)) < 0) { - continue; - } - prn = U2(p + 2); + int pi = HLEN + 4; // Number of observation messages + for (int i = 0; i < nobs; i++, pi += 40) { + uint32_t trk_stat = U4(raw->buff + pi + 36); + int sys, code, plock, clock; + int idx = decode_track_stat(trk_stat, &sys, &code, &plock, &clock); + if (idx < 0) continue; + int prn = U2(raw->buff + pi + 2); if (sys == SYS_GLO) prn -= 37; if (sys == SYS_SBS && prn >= MINPRNQZS_S && prn <= MAXPRNQZS_S && code == CODE_L1C) { sys = SYS_QZS; prn += 10; - code = CODE_L1Z; /* QZS L1S */ + code = CODE_L1Z; // QZS L1S } - if (!(sat = satno(sys, prn))) { + int sat = satno(sys, prn); + if (!sat) { trace(3, "unicore obsvm satellite number error: sys=%d,prn=%d\n", sys, prn); continue; } //if (sys == SYS_GLO && !parity) continue; - if ((idx = checkpri(raw->opt, sys, code, idx)) < 0) continue; - - gfrq = U2(p) + 1; /* GLONASS FCN+8 */ - psr = R8(p + 4); - adr = R8(p + 12); - dop = R4(p + 24); - snr = U2(p + 28) / 100.0; - lockt = R4(p + 32); + int gfrq = U2(raw->buff + pi) + 1; /* GLONASS FCN+8 */ + double psr = R8(raw->buff + pi + 4); + double adr = R8(raw->buff + pi + 12); + double dop = R4(raw->buff + pi + 24); + double snr = U2(raw->buff + pi + 28) / 100.0; + double lockt = R4(raw->buff + pi + 32); if (sys == SYS_GLO) { - freq = sat2freq(sat, (uint8_t)code, &raw->nav); + double freq = sat2freq(sat, code, &raw->nav); adr -= glo_bias * freq / CLIGHT; if (!raw->nav.glo_fcn[prn - 1]) { raw->nav.glo_fcn[prn - 1] = gfrq; /* fcn+8 */ } } + int lli; if (raw->tobs[sat - 1][code].time != 0) { - tt = timediff(raw->time, raw->tobs[sat - 1][code]); + double tt = timediff(raw->time, raw->tobs[sat - 1][code]); lli = lockt - raw->lockt[sat - 1][code] + 0.05 <= tt ? LLI_SLIP : 0; } else { lli = 0; } - // if (!parity) lli |= LLI_HALFC; - // if (halfc) lli |= LLI_HALFA; raw->tobs[sat - 1][code] = raw->time; raw->lockt[sat - 1][code] = lockt; raw->halfc[sat - 1][code] = 0; - if (!clock) psr = 0.0; /* code unlock */ - if (!plock) adr = dop = 0.0; /* phase unlock */ + if (!clock) continue; + if (!plock) adr = dop = 0.0; // phase valid - if (fabs(timediff(raw->obs.data[0].time, raw->time)) > 1E-9) { - raw->obs.n = 0; - } - if ((index = obsindex(&raw->obs, raw->time, sat)) >= 0) { + int index = obsindex(&raw->obs, raw->time, sat); + if (index >= 0) { + int idx = sigindex(raw->obs.data + index, sys, code, raw->opt); + if (idx < 0) continue; raw->obs.data[index].L[idx] = -adr; raw->obs.data[index].P[idx] = psr; raw->obs.data[index].D[idx] = (float)dop; raw->obs.data[index].SNR[idx] = (uint16_t)(snr / SNR_UNIT + 0.5); raw->obs.data[index].LLI[idx] = (uint8_t)lli; - raw->obs.data[index].code[idx] = (uint8_t)code; if (rcvstds) { - double pstd = U2(p + 20) * 0.01; // Meters + double pstd = U2(raw->buff + pi + 20) * 0.01; // Meters // To RTKlib encoding pstd = log2(pstd / 0.01) - 5; pstd = pstd > 0 ? pstd : 0; @@ -816,7 +770,7 @@ static int decode_obsvmb(raw_t* raw) pstd = pstd <= 254 ? pstd : 254; raw->obs.data[index].Pstd[idx] = pstd + 0.5; - double lstd = U2(p + 22) * 0.0001; // Cycles + double lstd = U2(raw->buff + pi + 22) * 0.0001; // Cycles // To RTKlib encoding lstd = lstd / 0.004; lstd = lstd > 0 ? lstd : 0; diff --git a/src/rinex.c b/src/rinex.c index 525aaba43..2b72df4f3 100644 --- a/src/rinex.c +++ b/src/rinex.c @@ -147,14 +147,11 @@ static const double ura_nominal[]={ /* URA nominal values */ 2048.0,4096.0,8192.0 }; /* type definition -----------------------------------------------------------*/ -typedef struct { /* signal index type */ - int n; /* number of index */ - int idx[MAXOBSTYPE]; /* signal freq-index */ - int pos[MAXOBSTYPE]; /* signal index in obs data (-1:no) */ - uint8_t pri [MAXOBSTYPE]; /* signal priority (15-0) */ - uint8_t type[MAXOBSTYPE]; /* type (0:C,1:L,2:D,3:S) */ - uint8_t code[MAXOBSTYPE]; /* obs-code (CODE_L??) */ - double shift[MAXOBSTYPE]; /* phase shift (cycle) */ +typedef struct { /* Signal index type */ + int n; /* Number of observation types */ + int code[MAXOBSTYPE]; /* Obs-code (CODE_L??) */ + uint8_t type[MAXOBSTYPE]; /* Type (0:C,1:L,2:D,3:S) */ + double shift[MAXOBSTYPE]; /* Phase shift (cycle) */ } sigind_t; /* set string without tail space ---------------------------------------------*/ @@ -774,7 +771,7 @@ static int decode_obsepoch(FILE *fp, char *buff, double ver, gtime_t *time, return n; } /* decode observation data ---------------------------------------------------*/ -static int decode_obsdata(FILE *fp, char *buff, double ver, int mask, +static int decode_obsdata(FILE *fp, const char *opt, char *buff, double ver, int mask, sigind_t *index, obsd_t *obs) { sigind_t *ind; @@ -782,7 +779,7 @@ static int decode_obsdata(FILE *fp, char *buff, double ver, int mask, uint8_t lli[MAXOBSTYPE]={0}; uint8_t std[MAXOBSTYPE]={0}; char satid[8]=""; - int i,j,n,m,q,stat=1,p[MAXOBSTYPE],k[16],l[16],r[16]; + int i,j,stat=1; trace(4,"decode_obsdata: ver=%.2f\n",ver); @@ -798,7 +795,8 @@ static int decode_obsdata(FILE *fp, char *buff, double ver, int mask, stat=0; } /* read observation data fields */ - switch (satsys(obs->sat,NULL)) { + int sys = satsys(obs->sat, NULL); + switch (sys) { case SYS_GLO: ind=index+1; break; case SYS_GAL: ind=index+2; break; case SYS_QZS: ind=index+3; break; @@ -807,14 +805,20 @@ static int decode_obsdata(FILE *fp, char *buff, double ver, int mask, case SYS_IRN: ind=index+6; break; default: ind=index ; break; } + int codesig[MAXCODE] = {-1}, nsigs = 0; for (i=0,j=ver<=2.99?0:3;in;i++,j+=16) { if (ver<=2.99&&j>=80) { /* ver.2 */ if (!fgets(buff,MAXRNXLEN,fp)) break; j=0; } - if (stat) { + int code = ind->code[i]; + if (stat && code != CODE_NONE) { val[i]=str2num(buff,j,14)+ind->shift[i]; + if (ind->type[i] == 0 && val[i] != 0) { + codesig[code] = nsigs; + nsigs++; + } lli[i]=(uint8_t)str2num(buff,j+14,1)&3; /* measurement std from receiver */ std[i]=(uint8_t)str2num(buff,j+15,1); @@ -826,88 +830,29 @@ static int decode_obsdata(FILE *fp, char *buff, double ver, int mask, obs->P[i]=obs->L[i]=0.0; obs->D[i]=0.0f; obs->SNR[i]=obs->LLI[i]=obs->Lstd[i]=obs->Pstd[i]=obs->code[i]=0; } - /* assign position in observation data */ - for (i=n=m=q=0;in;i++) { - - p[i]=(ver<=2.11)?ind->idx[i]:ind->pos[i]; - - if (ind->type[i]==0&&p[i]==0) k[n++]=i; /* C1? index */ - if (ind->type[i]==0&&p[i]==1) l[m++]=i; /* C2? index */ - if (ind->type[i]==0&&p[i]==2) r[q++]=i; /* C3? index */ - } - /* if multiple codes (C1/P1,C2/P2), select higher priority */ - if (ver<=2.11) { - if (n>=2) { - if (val[k[0]]==0.0&&val[k[1]]==0.0) { - p[k[0]]=-1; p[k[1]]=-1; - } - else if (val[k[0]]!=0.0&&val[k[1]]==0.0) { - p[k[0]]=0; p[k[1]]=-1; - } - else if (val[k[0]]==0.0&&val[k[1]]!=0.0) { - p[k[0]]=-1; p[k[1]]=0; - } - else if (ind->pri[k[1]]>ind->pri[k[0]]) { - p[k[1]]=0; p[k[0]]=NEXOBS<1?-1:NFREQ; - } - else { - p[k[0]]=0; p[k[1]]=NEXOBS<1?-1:NFREQ; - } - } - if (m>=2) { - if (val[l[0]]==0.0&&val[l[1]]==0.0) { - p[l[0]]=-1; p[l[1]]=-1; - } - else if (val[l[0]]!=0.0&&val[l[1]]==0.0) { - p[l[0]]=1; p[l[1]]=-1; - } - else if (val[l[0]]==0.0&&val[l[1]]!=0.0) { - p[l[0]]=-1; p[l[1]]=1; - } - else if (ind->pri[l[1]]>ind->pri[l[0]]) { - p[l[1]]=1; p[l[0]]=NEXOBS<2?-1:NFREQ+1; - } - else { - p[l[0]]=1; p[l[1]]=NEXOBS<2?-1:NFREQ+1; - } - } - if (q>=2) { - if (val[r[0]]==0.0&&val[r[1]]==0.0) { - p[r[0]]=-1; p[r[1]]=-1; - } - else if (val[r[0]]!=0.0&&val[r[1]]==0.0) { - p[r[0]]=2; p[r[1]]=-1; - } - else if (val[r[0]]==0.0&&val[r[1]]!=0.0) { - p[r[0]]=-1; p[r[1]]=2; - } - else if (ind->pri[r[1]]>ind->pri[r[0]]) { - p[r[1]]=2; p[r[0]]=NEXOBS<3?-1:NFREQ+2; - } - else { - p[r[0]]=2; p[r[1]]=NEXOBS<3?-1:NFREQ+2; - } - } - } - /* save observation data */ for (i=0;in;i++) { - if (p[i]<0||(val[i]==0.0&&lli[i]==0)) continue; + if (val[i] == 0.0 && lli[i] == 0) continue; + int code = ind->code[i]; + if (code == CODE_NONE) continue; + int sig = codesig[code]; + if (sig < 0) continue; + int freqidx = sigindex(obs, sys, code, opt); + if (freqidx < 0) continue; switch (ind->type[i]) { - case 0: obs->P[p[i]]=val[i]; - obs->code[p[i]]=ind->code[i]; - obs->Pstd[p[i]]=std[i]>0?std[i]:0; + case 0: obs->P[freqidx]=val[i]; + obs->Pstd[freqidx]=std[i]>0?std[i]:0; break; - case 1: obs->L[p[i]]=val[i]; - obs->LLI[p[i]]=lli[i]; - obs->Lstd[p[i]]=std[i]>0?std[i]:0; + case 1: obs->L[freqidx]=val[i]; + obs->LLI[freqidx]=lli[i]; + obs->Lstd[freqidx]=std[i]>0?std[i]:0; break; - case 2: obs->D[p[i]]=(float)val[i]; break; - case 3: obs->SNR[p[i]]=(uint16_t)(val[i]/SNR_UNIT+0.5); break; + case 2: obs->D[freqidx]=(float)val[i]; break; + case 3: obs->SNR[freqidx]=(uint16_t)(val[i]/SNR_UNIT+0.5); break; } - trace(4, "obs: i=%d f=%d P=%14.3f L=%14.3f LLI=%d code=%d\n",i,p[i],obs->P[p[i]], - obs->L[p[i]],obs->LLI[p[i]],obs->code[p[i]]); + trace(4, "obs: i=%d f=%d P=%14.3f L=%14.3f LLI=%d code=%d\n",i,freqidx,obs->P[freqidx], + obs->L[freqidx],obs->LLI[freqidx],obs->code[freqidx]); } trace(4,"decode_obsdata: time=%s sat=%2d\n",time_str(obs->time,0),obs->sat); return 1; @@ -968,20 +913,18 @@ static int set_sysmask(const char *opt) return mask; } /* set signal index ----------------------------------------------------------*/ -static void set_index(double ver, int sys, const char *opt, - char tobs[MAXOBSTYPE][4], sigind_t *ind) +static void set_sys_index(double ver, int sys, const char *opt, char tobs[MAXOBSTYPE][4], + sigind_t *ind) { const char *p; - char str[8],*optstr=""; - double shift; - int i,j,k,n; + char *optstr=""; + int i,n; for (i=n=0;*tobs[i];i++,n++) { - ind->code[i]=obs2code(tobs[i]+1); - ind->type[i]=(p=strchr(obscodes,tobs[i][0]))?(int)(p-obscodes):0; - ind->idx[i]=code2idx(sys,ind->code[i]); - ind->pri[i]=getcodepri(sys,ind->code[i],opt); - ind->pos[i]=-1; + int code = obs2code(tobs[i] + 1); + ind->code[i] = code; + const char *p = strchr(obscodes, tobs[i][0]); + ind->type[i]=p?(int)(p-obscodes):0; } /* parse phase shift options */ switch (sys) { @@ -994,6 +937,8 @@ static void set_index(double ver, int sys, const char *opt, case SYS_IRN: optstr="-IL%2s=%lf"; break; } for (p=opt;p&&(p=strchr(p,'-'));p++) { + char str[8]; + double shift; if (sscanf(p,optstr,str,&shift)<2) continue; for (i=0;icode[i]),str)) continue; @@ -1002,52 +947,47 @@ static void set_index(double ver, int sys, const char *opt, tobs[i],shift); } } - /* assign index for highest priority code */ - for (i=0;iidx[j]==i&&ind->pri[j]&&(k<0||ind->pri[j]>ind->pri[k])) { - k=j; - } - } - if (k<0) continue; - - for (j=0;jcode[j]==ind->code[k]) ind->pos[j]=i; - } - } - /* assign index of extended observation data */ - for (i=0;icode[j]&&ind->pri[j]&&ind->pos[j]<0) break; - } - if (j>=n) break; - - for (k=0;kcode[k]==ind->code[j]) ind->pos[k]=NFREQ+i; - } - } - /* list rejected observation types */ - for (i=0;icode[i]||!ind->pri[i]||ind->pos[i]>=0) continue; - trace(4,"reject obs type: sys=%2d, obs=%s\n",sys,tobs[i]); - } ind->n=n; - -#if 0 /* for debug */ - for (i=0;icode[i],ind->pri[i],ind->idx[i],ind->pos[i], - ind->shift[i]); + +#ifdef RTK_DISABLED /* for debug */ + for (int i=0;icode[i]; + trace(2,"set_index: sys=%2d tobs=%s code=%2d shift=%5.2f\n", + sys,tobs[i],c,,ind->shift[i]); } #endif } +static void set_index(double ver, const char *opt, char tobs[][MAXOBSTYPE][4], + sigind_t index[RNX_NUMSYS]) +{ +#if RNX_NUMSYS >= 1 + set_sys_index(ver, SYS_GPS, opt, tobs[RNX_SYS_GPS], index); +#endif +#if RNX_NUMSYS >= 2 + set_sys_index(ver, SYS_GLO, opt, tobs[RNX_SYS_GLO], index + 1); +#endif +#if RNX_NUMSYS >= 3 + set_sys_index(ver, SYS_GAL, opt, tobs[RNX_SYS_GAL], index + 2); +#endif +#if RNX_NUMSYS >= 4 + set_sys_index(ver, SYS_QZS, opt, tobs[RNX_SYS_QZS], index + 3); +#endif +#if RNX_NUMSYS >= 5 + set_sys_index(ver, SYS_SBS, opt, tobs[RNX_SYS_SBS], index + 4); +#endif +#if RNX_NUMSYS >= 6 + set_sys_index(ver, SYS_CMP, opt, tobs[RNX_SYS_CMP], index + 5); +#endif +#if RNX_NUMSYS >= 7 + set_sys_index(ver, SYS_IRN, opt, tobs[RNX_SYS_IRN], index + 6); +#endif +} /* read RINEX observation data body ------------------------------------------*/ static int readrnxobsb(FILE *fp, const char *opt, double ver, int *tsys, char tobs[][MAXOBSTYPE][4], int *flag, obsd_t *data, sta_t *sta) { gtime_t time={0}; - sigind_t index[RNX_NUMSYS]={{0}}; char buff[MAXRNXLEN]; int i=0,n=0,nsat=0,sats[MAXOBS]={0},mask; @@ -1055,28 +995,10 @@ static int readrnxobsb(FILE *fp, const char *opt, double ver, int *tsys, mask=set_sysmask(opt); /* set signal index */ -#if RNX_NUMSYS>=1 - set_index(ver,SYS_GPS,opt,tobs[RNX_SYS_GPS],index ); -#endif -#if RNX_NUMSYS>=2 - set_index(ver,SYS_GLO,opt,tobs[RNX_SYS_GLO],index+1); -#endif -#if RNX_NUMSYS>=3 - set_index(ver,SYS_GAL,opt,tobs[RNX_SYS_GAL],index+2); -#endif -#if RNX_NUMSYS>=4 - set_index(ver,SYS_QZS,opt,tobs[RNX_SYS_QZS],index+3); -#endif -#if RNX_NUMSYS>=5 - set_index(ver,SYS_SBS,opt,tobs[RNX_SYS_SBS],index+4); -#endif -#if RNX_NUMSYS>=6 - set_index(ver,SYS_CMP,opt,tobs[RNX_SYS_CMP],index+5); -#endif -#if RNX_NUMSYS>=7 - set_index(ver,SYS_IRN,opt,tobs[RNX_SYS_IRN],index+6); -#endif - + // Could hoist set_index into readrnxobs and rnxctr_t if a performance issue. + sigind_t index[RNX_NUMSYS]={{0}}; + set_index(ver, opt, tobs, index); + /* read record */ while (fgets(buff,MAXRNXLEN,fp)) { @@ -1095,12 +1017,13 @@ static int readrnxobsb(FILE *fp, const char *opt, double ver, int *tsys, data[n].sat=(uint8_t)sats[i-1]; /* decode RINEX observation data */ - if (decode_obsdata(fp,buff,ver,mask,index,data+n)) n++; + if (decode_obsdata(fp,opt,buff,ver,mask,index,data+n)) n++; } else if (*flag==3||*flag==4) { /* new site or header info follows */ /* decode RINEX observation data file header */ decode_obsh(fp,buff,ver,tsys,tobs,NULL,sta); + set_index(ver, opt, tobs, index); } if (++i>nsat) return n; } diff --git a/src/rtcm3.c b/src/rtcm3.c index 0eaa457d3..0b9d08984 100644 --- a/src/rtcm3.c +++ b/src/rtcm3.c @@ -2124,55 +2124,15 @@ static int decode_ssr7(rtcm_t *rtcm, int sys, int subtype) } return 20; } -/* get signal index ----------------------------------------------------------*/ -static void sigindex(int sys, const uint8_t *code, int n, const char *opt, - int *idx) -{ - int i,nex,pri,pri_h[8]={0},index[8]={0},ex[32]={0}; - - /* test code priority */ - for (i=0;i=NFREQ) { /* save as extended signal if idx >= NFREQ */ - ex[i]=1; - continue; - } - /* code priority */ - pri=getcodepri(sys,code[i],opt); - - /* select highest priority signal */ - if (pri>pri_h[idx[i]]) { - if (index[idx[i]]) ex[index[idx[i]]-1]=1; - pri_h[idx[i]]=pri; - index[idx[i]]=i+1; - } - else ex[i]=1; - } - /* signal index in obs data */ - for (i=nex=0;ibuff,24,12); @@ -2186,6 +2146,8 @@ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, case SYS_IRN: msm_type=q=rtcm->msmtype[6]; break; } /* id to signal */ + const char *sig[32]; + int code[32]; for (i=0;insig;i++) { switch (sys) { case SYS_GPS: sig[i]=msm_sig_gps[h->sigs[i]-1]; break; @@ -2199,8 +2161,6 @@ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, } /* signal to rinex obs type */ code[i]=obs2code(sig[i]); - idx[i]=code2idx(sys,code[i]); - if (code[i]!=CODE_NONE) { if (q) q+=sprintf(q,"L%s%s",sig[i],insig-1?",":""); } @@ -2212,9 +2172,6 @@ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, } trace(3,"rtcm3 %d: signals=%s\n",type,msm_type); - /* get signal index */ - sigindex(sys,code,h->nsig,rtcm->opt,idx); - for (i=j=0;insat;i++) { prn=h->sats[i]; @@ -2245,29 +2202,29 @@ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, fcn=rtcm->nav.glo_fcn[prn-1]-8; } } + for (k=0;knsig;k++) { if (!h->cellmask[k+i*h->nsig]) continue; - - if (sat&&index>=0&&idx[k]>=0) { + int freqidx = sigindex(rtcm->obs.data + index, sys, code[k], rtcm->opt); + if (sat && index >= 0 && freqidx >= 0) { freq=fcn<-7?0.0:code2freq(sys,code[k],fcn); /* pseudorange (m) */ if (r[i]!=0.0&&pr[j]>-1E12) { - rtcm->obs.data[index].P[idx[k]]=r[i]+pr[j]; + rtcm->obs.data[index].P[freqidx]=r[i]+pr[j]; } /* carrier-phase (cycle) */ if (r[i]!=0.0&&cp[j]>-1E12) { - rtcm->obs.data[index].L[idx[k]]=(r[i]+cp[j])*freq/CLIGHT; + rtcm->obs.data[index].L[freqidx]=(r[i]+cp[j])*freq/CLIGHT; } /* doppler (hz) */ if (rr&&rrf&&rrf[j]>-1E12) { - rtcm->obs.data[index].D[idx[k]]=-(rr[i]+rrf[j])*freq/CLIGHT; + rtcm->obs.data[index].D[freqidx]=-(rr[i]+rrf[j])*freq/CLIGHT; } - int LLI = lossoflock(rtcm, sat, code[k], idx[k], lti[j], locktype, half[j], rtcm->obs.data[index].L[idx[k]]); + int LLI = lossoflock(rtcm, sat, code[k], freqidx, lti[j], locktype, half[j], rtcm->obs.data[index].L[freqidx]); if (r[i] != 0.0 && cp[j] > -1E12 && half[j]) LLI |= LLI_HALFC; - rtcm->obs.data[index].LLI[idx[k]]=LLI; - rtcm->obs.data[index].SNR [idx[k]]=(uint16_t)(cnr[j]/SNR_UNIT+0.5); - rtcm->obs.data[index].code[idx[k]]=code[k]; + rtcm->obs.data[index].LLI[freqidx]=LLI; + rtcm->obs.data[index].SNR [freqidx]=(uint16_t)(cnr[j]/SNR_UNIT+0.5); } j++; } diff --git a/src/rtkcmn.c b/src/rtkcmn.c index 5ff96f154..dcaf1c350 100644 --- a/src/rtkcmn.c +++ b/src/rtkcmn.c @@ -725,6 +725,103 @@ extern int code2idx(int sys, uint8_t code) } return -1; } + +// sigindex -------------------------------------------- +// +// Allocate a new signal frequency index for the signal with the given code and +// taking into account the code priorities. There can be multiple codes for a +// given frequency index and if there is a conflict then this is resolved +// using the code priorities. If the new code has a higher priority then the +// current signal is moved into the extra-obs region and the new allocation is +// initialized. If there is an existing frequency index allocation with the +// same code then the data is not modified which allows repeated calls with +// the same index, vs allocating multiple frequency indices for the same +// code. It is assumed that the data code at an unused index is set to +// CODE_NONE. On the first allocation at a data index the data is +// initialized. The system could be computed form the data->sat, but pass it +// in anyway as the caller usually has this pre-computed. +// +// If the opt argument is NULL then do not allocate a new index, rather just +// search for a matching allocation for the given code using the same search +// strategy but without checking consistency of the code priorities. +extern int sigindex(obsd_t *data, int sys, int code, const char *opt) { + int idx = code2idx(sys, code); + if (idx < 0) return -1; + + // Check if the index is free + int ccode = data->code[idx]; + if (ccode == code) { + // Same code at this index. + return idx; + } + if (ccode == CODE_NONE) { + // Empty index. + if (opt == NULL) return -1; // No allocation. + // Allocate to this index. + data->L[idx] = data->P[idx] = 0.0; + data->D[idx] = 0.0; + data->SNR[idx] = 0; + data->Lstd[idx] = data->Pstd[idx] = 0; + data->LLI[idx] = 0; + data->code[idx] = code; + return idx; + } + + // Search the extra obs region, for matching code, or a free index. The + // allocations in the region are in order, so can stop at the first free + // index. + int idx2; + for (idx2 = NFREQ; idx2 < NFREQ + NEXOBS; idx2++) { + if (data->code[idx2] == code) { + // Same code at this index. + return idx2; + } + if (data->code[idx2] == CODE_NONE) { + // Free index. + break; + } + } + + if (opt == NULL) return -1; // No allocation. + + // Resolve the conflict using the code priorities. + int cpri = getcodepri(sys, ccode, opt); + int pri = getcodepri(sys, code, opt); + if (pri <= cpri) { + // Allocate to the extra obs region. + if (idx2 < NFREQ + NEXOBS) { + data->L[idx2] = data->P[idx2] = 0.0; + data->D[idx2] = 0.0; + data->SNR[idx2] = 0; + data->Lstd[idx2] = data->Pstd[idx2] = 0; + data->LLI[idx2] = 0; + data->code[idx2] = code; + return idx2; + } + return -1; + } + // Move the conflicting observation to a free index in the extra obs region + // if room, otherwise drop it. + if (idx2 < NFREQ + NEXOBS) { + data->L[idx2] = data->L[idx]; + data->P[idx2] = data->P[idx]; + data->D[idx2] = data->D[idx]; + data->SNR[idx2] = data->SNR[idx]; + data->Lstd[idx2] = data->Lstd[idx]; + data->Pstd[idx2] = data->Pstd[idx]; + data->LLI[idx2] = data->LLI[idx]; + data->code[idx2] = ccode; + } + + data->L[idx] = data->P[idx] = 0.0; + data->D[idx] = 0.0; + data->SNR[idx] = 0; + data->Lstd[idx] = data->Pstd[idx] = 0.0; + data->LLI[idx] = 0; + data->code[idx] = code; + return idx; +} + /* system and obs code to frequency -------------------------------------------- * convert system and obs code to carrier frequency * args : int sys I satellite system (SYS_???) diff --git a/src/rtklib.h b/src/rtklib.h index e0482f5c3..6843d677c 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -1383,6 +1383,7 @@ EXPORT int testsnr(int base, int freq, double el, double snr, const snrmask_t *mask); EXPORT void setcodepri(int sys, int idx, const char *pri); EXPORT int getcodepri(int sys, uint8_t code, const char *opt); +EXPORT int sigindex(obsd_t *data, int sys, int code, const char *opt); /* matrix and vector functions -----------------------------------------------*/ EXPORT double *mat (int n, int m);