diff --git a/src/convrnx.c b/src/convrnx.c index 503b45159..dd8b45111 100644 --- a/src/convrnx.c +++ b/src/convrnx.c @@ -82,8 +82,8 @@ typedef struct { /* stream file type */ raw_t raw; /* input receiver raw data */ rnxctr_t rnx; /* input RINEX control data */ stas_t *stas; /* station list */ - uint8_t slips [MAXSAT][NFREQ+NEXOBS]; /* cycle slip flag cache */ - halfc_t *halfc[MAXSAT][NFREQ+NEXOBS]; /* half-cycle ambiguity list */ + uint8_t slips [MAXSAT][MAXCODE]; /* cycle slip flag cache */ + halfc_t *halfc[MAXSAT][MAXCODE]; /* half-cycle ambiguity list */ FILE *fp; /* output file pointer */ } strfile_t; @@ -147,7 +147,6 @@ static strfile_t *gen_strfile(int format, const char *opt) { strfile_t *str; gtime_t time0={0}; - int i,j; trace(3,"init_strfile:\n"); @@ -200,9 +199,10 @@ static strfile_t *gen_strfile(int format, const char *opt) } str->stas=NULL; - for (i=0;islips[i][j]=0; - str->halfc[i][j]=NULL; + for (int i=0;islips[i][code]=0; + str->halfc[i][code]=NULL; } str->fp=NULL; return str; @@ -212,7 +212,6 @@ static void free_strfile(strfile_t *str) { stas_t *sp,*sp_next; halfc_t *hp,*hp_next; - int i,j; trace(3,"free_strfile:\n"); @@ -229,8 +228,9 @@ static void free_strfile(strfile_t *str) sp_next=sp->next; free(sp); } - for (i=0;ihalfc[i][j];hp;hp=hp_next) { + for (int i=0;ihalfc[i][code];hp;hp=hp_next) { hp_next=hp->next; free(hp); } @@ -625,15 +625,15 @@ static void dump_stas(const strfile_t *str) #endif } /* add half-cycle ambiguity list ---------------------------------------------*/ -static int add_halfc(strfile_t *str, int sat, int idx, gtime_t time) +static int add_halfc(strfile_t *str, int sat, int code, gtime_t time) { halfc_t *p; if (!(p=(halfc_t *)calloc(sizeof(halfc_t),1))) return 0; p->ts=p->te=time; p->stat=0; - p->next=str->halfc[sat-1][idx]; - str->halfc[sat-1][idx]=p; + p->next=str->halfc[sat-1][code]; + str->halfc[sat-1][code]=p; return 1; } /* update half-cycle ambiguity -----------------------------------------------*/ @@ -644,56 +644,59 @@ static void update_halfc(strfile_t *str, obsd_t *obs) for (i=0;iL[i]==0.0) continue; + int code = obs->code[i]; + if (code == CODE_NONE) continue; + /* if no list, start list */ - if (!str->halfc[sat-1][i]) { - if (!add_halfc(str,sat,i,obs->time)) continue; + if (!str->halfc[sat-1][code]) { + if (!add_halfc(str,sat,code,obs->time)) continue; } /* reset list if true cycle slip */ if ((obs->LLI[i]&LLI_SLIP)&&!(obs->LLI[i]&(LLI_HALFA|LLI_HALFS))) { - str->halfc[sat-1][i]->stat=0; + str->halfc[sat-1][code]->stat=0; } if (obs->LLI[i]&LLI_HALFC) { /* halfcyc unresolved */ /* if new list, set unresolved start epoch */ - if (str->halfc[sat-1][i]->stat==0) { - str->halfc[sat-1][i]->ts=obs->time; + if (str->halfc[sat-1][code]->stat==0) { + str->halfc[sat-1][code]->ts=obs->time; } /* update unresolved end epoch and set status to active */ - str->halfc[sat-1][i]->te=obs->time; - str->halfc[sat-1][i]->stat=1; + str->halfc[sat-1][code]->te=obs->time; + str->halfc[sat-1][code]->stat=1; } /* else if resolved, update status */ - else if (str->halfc[sat-1][i]->stat==1) { + else if (str->halfc[sat-1][code]->stat==1) { if (obs->LLI[i]&LLI_HALFA) { - str->halfc[sat-1][i]->stat=2; /* resolved with add */ + str->halfc[sat-1][code]->stat=2; /* resolved with add */ } else if (obs->LLI[i]&LLI_HALFS) { - str->halfc[sat-1][i]->stat=3; /* resolved with subtract */ + str->halfc[sat-1][code]->stat=3; /* resolved with subtract */ } else { - str->halfc[sat-1][i]->stat=4; /* resolved with no adjust */ + str->halfc[sat-1][code]->stat=4; /* resolved with no adjust */ } /* create new list entry */ - if (!add_halfc(str,sat,i,obs->time)) continue; + if (!add_halfc(str,sat,code,obs->time)) continue; } } } /* dump half-cycle ambiguity list --------------------------------------------*/ static void dump_halfc(const strfile_t *str) { -#if 0 /* for debug */ - halfc_t *p; - char s0[8],s1[40],s2[40],*stats[]={"ADD","SUB","NON"}; - int i,j; - +#ifdef RTK_DISABLED /* for debug */ trace(2,"# HALF-CYCLE AMBIGUITY CORRECTIONS\n"); trace(2,"# %20s %22s %4s %3s %3s\n","START","END","SAT","FRQ","COR"); - for (i=0;ihalfc[i][j];p;p=p->next) { + for (int i=0;ihalfc[i][code];p;p=p->next) { if (p->stat<=1) continue; + char s0[8],s1[40],s2[40]; satno2id(i+1,s0); time2str(p->ts,s1,2); time2str(p->te,s2,2); - trace(2,"%s %s %4s %3d %3s\n",s1,s2,s0,j+1,stats[p->stat-2]); + const char *stats[]={"ADD","SUB","NON"}; + trace(2,"%s %s %4s %2s %3s\n",s1,s2,s0,obs,stats[p->stat-2]); } } #endif @@ -702,15 +705,16 @@ static void dump_halfc(const strfile_t *str) static void resolve_halfc(const strfile_t *str, obsd_t *data, int n) { halfc_t *p; - int i,j,sat; + int sat; - for (i=0;ihalfc[sat-1][j];p;p=p->next) { - if (p->stat<=1) continue; /* unresolved half cycle */ - if (timediff(data[i].time,p->ts)<-DTTOL|| - timediff(data[i].time,p->te)> DTTOL) continue; + for (int j=0;jhalfc[sat-1][code];p;p=p->next) { + if (p->stat<=1) continue; /* unresolved half cycle */ + if (timediff(data[i].time,p->ts)<-DTTOL|| + timediff(data[i].time,p->te)> DTTOL) continue; if (p->stat==2) { /* add half cycle */ data[i].L[j]+=0.5; @@ -721,6 +725,7 @@ static void resolve_halfc(const strfile_t *str, obsd_t *data, int n) data[i].LLI[j]&=~LLI_HALFC; } data[i].LLI[j]&=~(LLI_HALFA|LLI_HALFS); + } } } /* scan input files ----------------------------------------------------------*/ @@ -757,14 +762,14 @@ static int scan_file(char **files, int nf, rnxopt_t *opt, strfile_t *str, /* update obs-types */ for (j=0;jobs->data[i].code[j]; - if (c==CODE_NONE) continue; + int cd=str->obs->data[i].code[j]; + if (cd==CODE_NONE) continue; for (k=0;k=n[l]&&n[l]<32) { - codes[l][n[l]++]=c; + codes[l][n[l]++]=cd; } if (kobs->data[i].P[j]!=0.0) types[l][k]|=1; @@ -971,21 +976,27 @@ static void outrnxevent(FILE *fp, const rnxopt_t *opt, gtime_t time, int event, /* save cycle slips ----------------------------------------------------------*/ static void save_slips(strfile_t *str, obsd_t *data, int n) { - int i,j; - - for (i=0;islips[data[i].sat-1][j]=1; + for (int i=0;islips[sat-1][code]=1; } } /* restore cycle slips -------------------------------------------------------*/ static void rest_slips(strfile_t *str, obsd_t *data, int n) { - int i,j; - - for (i=0;islips[data[i].sat-1][j]) { - data[i].LLI[j]|=LLI_SLIP; - str->slips[data[i].sat-1][j]=0; + for (int i=0;islips[sat-1][code]) { + data[i].LLI[j]|=LLI_SLIP; + str->slips[sat-1][code]=0; } } } @@ -1025,14 +1036,19 @@ static void convobs(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n, /* save cycle slips */ save_slips(str,str->obs->data,str->obs->n); - if (!screent_ttol(time,opt->ts,opt->te,opt->tint,opt->ttol)) return; + if (!screent_ttol(time,opt->ts,opt->te,opt->tint,opt->ttol)) { + if (str->staid!=*staid) { /* Station ID changed */ + *staid=str->staid; + } + str->obs->flag = 0; + return; + } - /* restore cycle slips */ + /* Restore cycle slips */ rest_slips(str,str->obs->data,str->obs->n); - if (str->staid!=*staid) { /* station ID changed */ - - if (*staid>=0) { /* output RINEX event */ + if (str->staid!=*staid) { /* Station ID changed */ + if (*staid>=0) { /* Output RINEX event */ outrnxevent(ofp[0],opt,str->time,EVENT_NEWSITE,str->stas,str->staid); } *staid=str->staid; @@ -1259,7 +1275,7 @@ static int convrnx_s(int sess, int format, rnxopt_t *opt, const char *file, FILE *ofp[NOUTFILE]={NULL}; strfile_t *str; gtime_t tend[3]={{0}}; - int i,j,nf,type,n[NOUTFILE+2]={0},mask[MAXEXFILE]={0},staid=-1,abort=0; + int j,nf,type,n[NOUTFILE+2]={0},mask[MAXEXFILE]={0},staid=-1,abort=0; char path[1024],*paths[NOUTFILE],s[NOUTFILE][1024]; char *epath[MAXEXFILE]={0},*staname=*opt->staid?opt->staid:"0000"; @@ -1273,7 +1289,7 @@ static int convrnx_s(int sess, int format, rnxopt_t *opt, const char *file, return 0; } /* expand wild-cards in input file */ - for (i=0;ircvopt))) { - for (i=0;itime=timeadd(opt->ts,-1.0); } /* set GLONASS FCN in RINEX options */ - for (i=0;inav->glo_fcn[i]=opt->glofcn[i]; /* FCN+8 */ } /* scan input files */ if (!scan_file(epath,nf,opt,str,mask)) { - for (i=0;its.time?opt->ts:str->tstart, staname,"")<0) { @@ -1319,13 +1335,13 @@ static int convrnx_s(int sess, int format, rnxopt_t *opt, const char *file, } /* open output files */ if (!openfile(ofp,paths,path,opt,str->nav)) { - for (i=0;itime=str->tstart; - for (i=0;inav); /* remove empty output files */ - for (i=0;itstart,opt->tend,n); @@ -1365,7 +1381,7 @@ static int convrnx_s(int sess, int format, rnxopt_t *opt, const char *file, unsetopt_file(opt); free_strfile(str); - for (i=0;itobs[sat-1][idx].time!=0) { - tt=timediff(raw->time,raw->tobs[sat-1][idx]); - lli=(lockt<65535.968&&lockt-raw->lockt[sat-1][idx]+0.05<=tt)?LLI_SLIP:0; + 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][idx]=raw->time; - raw->lockt[sat-1][idx]=lockt; - raw->halfc[sat-1][idx]=halfc; + 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 */ @@ -498,18 +498,18 @@ static int decode_rangeb(raw_t *raw) raw->nav.glo_fcn[prn-1]=gfrq; /* fcn+8 */ } } - if (raw->tobs[sat-1][idx].time!=0) { - tt=timediff(raw->time,raw->tobs[sat-1][idx]); - lli=lockt-raw->lockt[sat-1][idx]+0.05<=tt?LLI_SLIP:0; + 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][idx]=raw->time; - raw->lockt[sat-1][idx]=lockt; - raw->halfc[sat-1][idx]=halfc; + 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 */ @@ -1093,18 +1093,19 @@ static int decode_rgeb(raw_t *raw) trace(2,"oem3 regb satellite number error: sys=%d prn=%d\n",sys,prn); continue; } - if (raw->tobs[sat-1][freq].time!=0) { - tt=timediff(raw->time,raw->tobs[sat-1][freq]); - lli=lockt-raw->lockt[sat-1][freq]+0.05halfc[sat-1][freq]; + int code=freq==0?CODE_L1C:CODE_L2P; + 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.05halfc[sat-1][code]; } else { lli=0; } if (!parity) lli|=2; - raw->tobs [sat-1][freq]=raw->time; - raw->lockt[sat-1][freq]=lockt; - raw->halfc[sat-1][freq]=parity; + raw->tobs [sat-1][code]=raw->time; + raw->lockt[sat-1][code]=lockt; + raw->halfc[sat-1][code]=parity; if (fabs(timediff(raw->obs.data[0].time,raw->time))>1E-9) { raw->obs.n=0; @@ -1116,7 +1117,7 @@ static int decode_rgeb(raw_t *raw) raw->obs.data[index].SNR[freq]= 0.0<=snr&&snr<255.0?(uint16_t)(snr/SNR_UNIT+0.5):0; raw->obs.data[index].LLI[freq]=(uint8_t)lli; - raw->obs.data[index].code[freq]=freq==0?CODE_L1C:CODE_L2P; + raw->obs.data[index].code[freq]=code; } } return 1; @@ -1159,18 +1160,19 @@ static int decode_rged(raw_t *raw) adr_rolls=floor((psr/(freq==0?WL1:WL2)-adr)/MAXVAL+0.5); adr=adr+MAXVAL*adr_rolls; - if (raw->tobs[sat-1][freq].time!=0) { - tt=timediff(raw->time,raw->tobs[sat-1][freq]); - lli=lockt-raw->lockt[sat-1][freq]+0.05halfc[sat-1][freq]; + int code = freq==0?CODE_L1C:CODE_L2P; + 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.05halfc[sat-1][code]; } else { lli=0; } if (!parity) lli|=2; - raw->tobs [sat-1][freq]=raw->time; - raw->lockt[sat-1][freq]=lockt; - raw->halfc[sat-1][freq]=parity; + raw->tobs [sat-1][code]=raw->time; + raw->lockt[sat-1][code]=lockt; + raw->halfc[sat-1][code]=parity; if (fabs(timediff(raw->obs.data[0].time,raw->time))>1E-9) { raw->obs.n=0; @@ -1181,7 +1183,7 @@ static int decode_rged(raw_t *raw) raw->obs.data[index].D [freq]=(float)dop; raw->obs.data[index].SNR[freq]=(uint16_t)(snr/SNR_UNIT+0.5); raw->obs.data[index].LLI[freq]=(uint8_t)lli; - raw->obs.data[index].code[freq]=freq==0?CODE_L1C:CODE_L2P; + raw->obs.data[index].code[freq]=code; } } return 1; diff --git a/src/rcv/septentrio.c b/src/rcv/septentrio.c index b27c46ab2..b107baee1 100644 --- a/src/rcv/septentrio.c +++ b/src/rcv/septentrio.c @@ -76,8 +76,7 @@ typedef struct { uint8_t signalIdx[MEAS3_SYS_MAX][MEAS3_SAT_MAX][MEAS3_SIG_MAX]; // Reference signal indeces uint32_t slaveSignalMask[MEAS3_SYS_MAX][MEAS3_SAT_MAX]; // Mask of available slave signals int16_t prRate[MEAS3_SYS_MAX][MEAS3_SAT_MAX]; // Pseudo-range change rate in 64 mm steps - uint32_t lockt[MEAS3_SYS_MAX][MEAS3_SAT_MAX][NFREQ + NEXOBS]; // Lock time of the PLL, in ms - uint8_t halfc[MEAS3_SYS_MAX][MEAS3_SAT_MAX][NFREQ + NEXOBS]; // Half cycle ambiguity + uint32_t lockt[MEAS3_SYS_MAX][MEAS3_SAT_MAX][MAXCODE]; // Lock time of the PLL, in ms uint8_t constellationHeader[MEAS3_SYS_MAX][32]; // Copy of constellation header } Meas3_RefEpoch_t; @@ -782,9 +781,9 @@ static int decode_measepoch(raw_t *raw) if (P1!=0.0 && freq1>0.0 && lock!=65535 && (I1(p+14) != -128 || U2(p+12) != 0)) { L1 = I1(p+14)*65.536 + U2(p+12)*0.001; raw->obuf.data[n].L[idx] = P1*freq1/CLIGHT + L1; - LLI = (locklockt[sat-1][idx] ? 1 : 0) + ((info & (1<<2)) ? 2 : 0); + LLI = (locklockt[sat-1][code] ? 1 : 0) + ((info & (1<<2)) ? 2 : 0); raw->obuf.data[n].LLI[idx] = (uint8_t)LLI; - raw->lockt[sat-1][idx] = lock; + raw->lockt[sat-1][code] = lock; } if (U1(p+15) != 255) { S1 = U1(p+15)*0.25 + ((sig==1 || sig==2) ? 0.0 : 10.0); @@ -810,9 +809,9 @@ static int decode_measepoch(raw_t *raw) P2 = 0.0; freq2 = code2freq(sys, code, fcn); if (lock != 255) { - LLI = (locklockt[sat-1][idx] ? 1 : 0) + ((info&(1<<2)) ? 2 : 0); + LLI = (locklockt[sat-1][code] ? 1 : 0) + ((info&(1<<2)) ? 2 : 0); raw->obuf.data[n].LLI[idx] = (uint8_t)LLI; - raw->lockt[sat-1][idx] = lock; + raw->lockt[sat-1][code] = lock; } if (U1(p+2) != 255) { S2 = U1(p+2)*0.25 + ((sig==1 || sig==2) ? 0.0 : 10.0); @@ -1105,7 +1104,7 @@ static int decode_meas3ranges(raw_t *raw) { raw->obuf.data[n].L[masterFreqIndex] = pr / (CLIGHT/freqMaster) - 131.072 + (double)cmc * .001; 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->lockt[satNo-1][codeMaster] = lockTime; raw->obuf.data[n].freq = glofnc+7; sbf->meas3_freqAssignment[navsys][svid][0] = masterFreqIndex; }; @@ -1151,7 +1150,7 @@ static int decode_meas3ranges(raw_t *raw) { 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->lockt[satNo-1][codeMaster] = lockTime; raw->obuf.data[n].freq = glofnc+7; sbf->meas3_freqAssignment[navsys][svid][0] = masterFreqIndex; }; @@ -1187,7 +1186,7 @@ static int decode_meas3ranges(raw_t *raw) { master_reference->L[masterFreqIndex] - 32.768 + (double)cmc * .001; raw->obuf.data[n].LLI[masterFreqIndex] = master_reference->LLI[masterFreqIndex]; - raw->lockt[satNo-1][masterFreqIndex] = sbf->meas3_refEpoch.lockt[navsys][svid][masterFreqIndex]; + raw->lockt[satNo-1][codeMaster] = sbf->meas3_refEpoch.lockt[navsys][svid][codeMaster]; raw->obuf.data[n].freq = glofnc+7; sbf->meas3_freqAssignment[navsys][svid][0] = masterFreqIndex; } @@ -1213,7 +1212,7 @@ static int decode_meas3ranges(raw_t *raw) { 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] * SNR_UNIT - 1.0 + CN0) / SNR_UNIT + 0.5); raw->obuf.data[n].LLI[masterFreqIndex] = masterReference->LLI[masterFreqIndex]; - raw->lockt[satNo-1][masterFreqIndex] = sbf->meas3_refEpoch.lockt[navsys][svid][masterFreqIndex]; + raw->lockt[satNo-1][codeMaster] = sbf->meas3_refEpoch.lockt[navsys][svid][codeMaster]; raw->obuf.data[n].code[masterFreqIndex] = codeMaster; raw->obuf.data[n].freq = glofnc+8; sbf->meas3_freqAssignment[navsys][svid][0] = masterFreqIndex; @@ -1238,12 +1237,12 @@ static int decode_meas3ranges(raw_t *raw) { sbf->meas3_refEpoch.obsData[navsys][svid].L[masterFreqIndex] = raw->obuf.data[n].L[masterFreqIndex]; sbf->meas3_refEpoch.obsData[navsys][svid].SNR[masterFreqIndex] = raw->obuf.data[n].SNR[masterFreqIndex]; sbf->meas3_refEpoch.obsData[navsys][svid].LLI[masterFreqIndex] = raw->obuf.data[n].LLI[masterFreqIndex]; - sbf->meas3_refEpoch.lockt[navsys][svid][masterFreqIndex] = raw->lockt[satNo-1][masterFreqIndex]; + sbf->meas3_refEpoch.lockt[navsys][svid][codeMaster] = raw->lockt[satNo-1][codeMaster]; } // update PLL lock time - if (satNo > 0 && raw->lockt[satNo-1][masterFreqIndex] > sbf->meas3_refEpoch.lockt[navsys][svid][masterFreqIndex]) - sbf->meas3_refEpoch.lockt[navsys][svid][masterFreqIndex] = raw->lockt[satNo-1][masterFreqIndex]; + if (satNo > 0 && raw->lockt[satNo-1][codeMaster] > sbf->meas3_refEpoch.lockt[navsys][svid][codeMaster]) + sbf->meas3_refEpoch.lockt[navsys][svid][codeMaster] = raw->lockt[satNo-1][codeMaster]; /* decode slave data */ int slaveCnt = 0; @@ -1288,7 +1287,7 @@ static int decode_meas3ranges(raw_t *raw) { 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); - raw->lockt[satNo-1][slaveFreqIndex] = lockTime; + raw->lockt[satNo-1][codeSlave] = lockTime; sbf->meas3_freqAssignment[navsys][svid][slaveCnt+1] = slaveFreqIndex; } @@ -1318,8 +1317,8 @@ static int decode_meas3ranges(raw_t *raw) { 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); - raw->lockt[satNo-1][slaveFreqIndex] = lockTime; + raw->obuf.data[n].LLI[slaveFreqIndex] = (lockTime < raw->lockt[satNo-1][codeSlave] ? LLI_SLIP : 0) | (lti4 == 0 ? LLI_HALFC : 0); + raw->lockt[satNo-1][codeSlave] = lockTime; sbf->meas3_freqAssignment[navsys][svid][slaveCnt+1] = slaveFreqIndex; } @@ -1351,7 +1350,7 @@ static int decode_meas3ranges(raw_t *raw) { raw->obuf.data[n].code[slaveFreqIndex] = codeSlave; raw->obuf.data[n].LLI[slaveFreqIndex] = slaveReference->LLI[slaveRefFreqIdx]; - raw->lockt[satNo-1][slaveFreqIndex] = sbf->meas3_refEpoch.lockt[navsys][svid][slaveCnt+1]; + raw->lockt[satNo-1][codeSlave] = sbf->meas3_refEpoch.lockt[navsys][svid][codeSlave]; sbf->meas3_freqAssignment[navsys][svid][slaveCnt+1] = slaveFreqIndex; } @@ -1369,11 +1368,11 @@ static int decode_meas3ranges(raw_t *raw) { sbf->meas3_refEpoch.obsData[navsys][svid].L[slaveFreqIndex] = raw->obuf.data[n].L[slaveFreqIndex]; sbf->meas3_refEpoch.obsData[navsys][svid].SNR[slaveFreqIndex] = raw->obuf.data[n].SNR[slaveFreqIndex]; sbf->meas3_refEpoch.obsData[navsys][svid].LLI[slaveFreqIndex] = raw->obuf.data[n].LLI[slaveFreqIndex]; - sbf->meas3_refEpoch.lockt[navsys][svid][slaveFreqIndex] = raw->lockt[satNo-1][slaveFreqIndex]; + sbf->meas3_refEpoch.lockt[navsys][svid][codeSlave] = raw->lockt[satNo-1][codeSlave]; } - if (raw->lockt[satNo-1][slaveFreqIndex] > sbf->meas3_refEpoch.lockt[navsys][svid][slaveFreqIndex]) - sbf->meas3_refEpoch.lockt[navsys][svid][slaveFreqIndex] = raw->lockt[satNo-1][slaveFreqIndex]; + if (raw->lockt[satNo-1][codeSlave] > sbf->meas3_refEpoch.lockt[navsys][svid][codeSlave]) + sbf->meas3_refEpoch.lockt[navsys][svid][codeSlave] = raw->lockt[satNo-1][codeSlave]; slaveCnt++; /* delete this bit of the mask */ diff --git a/src/rcv/ublox.c b/src/rcv/ublox.c index dcff51014..72319040e 100644 --- a/src/rcv/ublox.c +++ b/src/rcv/ublox.c @@ -123,7 +123,7 @@ typedef enum { false, true } bool; #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 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;} static double I8(uint8_t *p) {return I4(p+4)*4294967296.0+U4(p);} @@ -334,9 +334,9 @@ static int decode_rxmraw(raw_t *raw) } raw->obs.data[n].sat=sat; - if (raw->obs.data[n].LLI[0]&1) raw->lockt[sat-1][0]=0.0; - else if (tt<1.0||10.0lockt[sat-1][0]=0.0; - else raw->lockt[sat-1][0]+=tt; + if (raw->obs.data[n].LLI[0]&LLI_SLIP) raw->lockt[sat-1][CODE_L1C]=0.0; + else if (tt<1.0||10.0lockt[sat-1][CODE_L1C]=0.0; + else raw->lockt[sat-1][CODE_L1C]+=tt; for (j=1;jobs.data[n].L[j]=raw->obs.data[n].P[j]=0.0; @@ -358,7 +358,7 @@ static int decode_rxmrawx(raw_t *raw) gtime_t time; char *q,tstr[40]; double tow,P,L,D,tn,tadj=0.0,toff=0.0; - int i,j,k,idx,sys,prn,sat,code,slip,halfv,halfc,LLI,n=0; + int i,j,k,idx,sys,prn,sat,slip,halfv,halfc,LLI,n=0; int week,nmeas,ver,gnss,svid,sigid,frqid,lockt,cn0,cpstd=0,prstd=0,tstat; int multicode=0, rcvstds=0; @@ -452,14 +452,22 @@ static int decode_rxmrawx(raw_t *raw) if (sys==SYS_GLO&&!raw->nav.glo_fcn[prn-1]) { raw->nav.glo_fcn[prn-1]=frqid-7+8; } + // The signal code, not the combined multi-code, is the main key for + // signal state, to keep signal state separate. But this decoder + // defaults to combining signals and so state is also saved using this + // combined code as a key which should work so long as there are not + // multiple live signals per combined code. + int mcode, code; if (ver>=1) { + mcode=ubx_sig(sys,sigid); if (multicode) - code=ubx_sig(sys,sigid); + code=mcode; else code=ubx_sig_combined(sys,sigid); } else { - code=(sys==SYS_CMP)?CODE_L2I:((sys==SYS_GAL)?CODE_L1X:CODE_L1C); + mcode=(sys==SYS_CMP)?CODE_L2I:((sys==SYS_GAL)?CODE_L1X:CODE_L1C); + code=mcode; } /* signal index in obs data */ if ((idx=sig_idx(sys,code))<0) { @@ -480,20 +488,20 @@ static int decode_rxmrawx(raw_t *raw) else halfv=(tstat&4)?1:0; /* half cycle valid */ halfc=(tstat&8)?1:0; /* half cycle subtracted from phase */ - slip=lockt==0||lockt*1E-3lockt[sat-1][idx]|| - halfc!=raw->halfc[sat-1][idx]; - if (cpstd>=cpstd_slip) slip=LLI_SLIP; - if (slip) raw->lockflag[sat-1][idx]=slip; - raw->lockt[sat-1][idx]=lockt*1E-3; + slip=lockt==0||lockt*1E-3lockt[sat-1][mcode]|| + halfc!=raw->halfc[sat-1][mcode]; + if (cpstd>=cpstd_slip) slip=1; + if (slip) raw->lockflag[sat-1][mcode]=raw->lockflag[sat-1][code]=slip; + raw->lockt[sat-1][mcode]=raw->lockt[sat-1][code]=lockt*1E-3; /* LLI: bit0=slip,bit1=half-cycle-unresolved */ LLI=!halfv&&L!=0.0?LLI_HALFC:0; /* half cycle adjusted */ LLI|=halfc?LLI_HALFA:0; /* set cycle slip if half cycle subtract bit changed state */ - LLI|=halfc!=raw->halfc[sat-1][idx]?LLI_SLIP:0; - raw->halfc[sat-1][idx]=halfc; + LLI|=halfc!=raw->halfc[sat-1][mcode]?LLI_SLIP:0; + raw->halfc[sat-1][mcode]=raw->halfc[sat-1][code]=halfc; /* set cycle slip flag if first valid phase since slip */ - if (L!=0.0) LLI|=raw->lockflag[sat-1][idx]>0.0?LLI_SLIP:0; + if (L!=0.0) LLI|=raw->lockflag[sat-1][mcode]>0?LLI_SLIP:0; for (j=0;jobs.data[j].sat==sat) break; @@ -519,7 +527,8 @@ static int decode_rxmrawx(raw_t *raw) raw->obs.data[j].SNR[idx]=(uint16_t)(cn0*1.0/SNR_UNIT+0.5); raw->obs.data[j].LLI[idx]=(uint8_t)LLI; raw->obs.data[j].code[idx]=(uint8_t)code; - if (L!=0.0) raw->lockflag[sat-1][idx]=0; /* clear slip carry-forward flag if valid phase*/ + /* clear slip carry-forward flag if valid phase*/ + if (L!=0.0) raw->lockflag[sat-1][mcode]=raw->lockflag[sat-1][code]=0; } raw->time=time; raw->obs.n=n; @@ -647,10 +656,13 @@ static int decode_trkmeas(raw_t *raw) snr =U2(p+20)/256.0; adr =I8(p+32)*P2_32+(flag&0x40?0.5:0.0); dop =I4(p+40)*P2_10*10.0; - + + int code=sys==SYS_CMP?CODE_L2I:CODE_L1C; + /* set slip flag */ - if (lock2==0||lock2lockt[sat-1][0]) raw->lockt[sat-1][1]=1.0; - raw->lockt[sat-1][0]=lock2; + int slip = 0; + if (lock2==0||lock2lockt[sat-1][code]) slip=1; + raw->lockt[sat-1][code]=lock2; #if 0 /* for debug */ trace(2,"[%2d] qi=%d sys=%d prn=%3d frq=%2d flag=%02X ?=%02X %02X " @@ -671,16 +683,15 @@ static int decode_trkmeas(raw_t *raw) raw->obs.data[n].L[0]=-adr; raw->obs.data[n].D[0]=(float)dop; raw->obs.data[n].SNR[0]=(uint16_t)(snr/SNR_UNIT+0.5); - raw->obs.data[n].code[0]=sys==SYS_CMP?CODE_L2I:CODE_L1C; + raw->obs.data[n].code[0]=code; raw->obs.data[n].Lstd[0]=8-qi; - raw->obs.data[n].LLI[0]=raw->lockt[sat-1][1]>0.0?1:0; + raw->obs.data[n].LLI[0]=slip?LLI_SLIP:0; if (sys==SYS_SBS) { /* half-cycle valid */ - raw->obs.data[n].LLI[0]|=lock2>142?0:2; + raw->obs.data[n].LLI[0]|=lock2>142?0:LLI_HALFC; } else { - raw->obs.data[n].LLI[0]|=flag&0x80?0:2; + raw->obs.data[n].LLI[0]|=flag&0x80?0:LLI_HALFC; } - raw->lockt[sat-1][1]=0.0; /* adjust code measurements for GLONASS sats */ if (sys==SYS_GLO&&frq>=-7&&frq<=7) { if (fw==2) raw->obs.data[n].P[0]+=(double)P_adj_fw2[frq+7]; @@ -778,8 +789,9 @@ static int decode_trkd5(raw_t *raw) adr=qi<6?0.0:I8(p+8)*P2_32+(flag&0x01?0.5:0.0); dop=I4(p+16)*P2_10/4.0; snr=U2(p+32)/256.0; - - if (snr<=10.0) raw->lockt[sat-1][1]=1.0; + + int slip = 0; + if (snr<=10.0) slip=1; #if 0 /* for debug */ trace(2,"[%2d] qi=%d sys=%d prn=%3d frq=%2d flag=%02X ts=%1.3f " @@ -799,8 +811,7 @@ static int decode_trkd5(raw_t *raw) raw->obs.data[n].D[0]=(float)dop; raw->obs.data[n].SNR[0]=(uint16_t)(snr/SNR_UNIT+0.5); raw->obs.data[n].code[0]=sys==SYS_CMP?CODE_L2I:CODE_L1C; - raw->obs.data[n].LLI[0]=raw->lockt[sat-1][1]>0.0?1:0; - raw->lockt[sat-1][1]=0.0; + raw->obs.data[n].LLI[0]=slip?LLI_SLIP:0; for (j=1;jobs.data[n].L[j]=raw->obs.data[n].P[j]=0.0; @@ -1339,8 +1350,7 @@ extern int input_ubx(raw_t *raw, uint8_t data) /* synchronize frame */ if (raw->nbyte==0) { - if (!sync_ubx(raw->buff,data)) return 0; - raw->nbyte=2; + if (sync_ubx(raw->buff,data)) raw->nbyte=2; return 0; } raw->buff[raw->nbyte++]=data; diff --git a/src/rcv/unicore.c b/src/rcv/unicore.c index 06ec05788..63d3c7340 100644 --- a/src/rcv/unicore.c +++ b/src/rcv/unicore.c @@ -781,18 +781,18 @@ static int decode_obsvmb(raw_t* raw) raw->nav.glo_fcn[prn - 1] = gfrq; /* fcn+8 */ } } - if (raw->tobs[sat - 1][idx].time != 0) { - tt = timediff(raw->time, raw->tobs[sat - 1][idx]); - lli = lockt - raw->lockt[sat - 1][idx] + 0.05 <= tt ? LLI_SLIP : 0; + 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][idx] = raw->time; - raw->lockt[sat - 1][idx] = lockt; - raw->halfc[sat - 1][idx] = 0; + 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 */ diff --git a/src/rcvraw.c b/src/rcvraw.c index d4d15b44f..dc2a62d54 100644 --- a/src/rcvraw.c +++ b/src/rcvraw.c @@ -1316,10 +1316,11 @@ extern int init_raw(raw_t *raw, int format) raw->msgtype[0]='\0'; for (i=0;isubfrm[i][j]=0; - for (j=0;jtobs [i][j]=time0; - raw->lockt[i][j]=0.0; - raw->halfc[i][j]=0; + for (int code=0; codetobs [i][code]=time0; + raw->lockt[i][code]=0.0; + raw->halfc[i][code]=0; + raw->lockflag[i][code]=0; } raw->icpp[i]=raw->off[i]=raw->prCA[i]=raw->dpCA[i]=0.0; } diff --git a/src/rinex.c b/src/rinex.c index f43004813..c33f4d544 100644 --- a/src/rinex.c +++ b/src/rinex.c @@ -915,20 +915,20 @@ static int decode_obsdata(FILE *fp, char *buff, double ver, int mask, return 1; } /* save cycle slips ----------------------------------------------------------*/ -static void saveslips(uint8_t slips[][NFREQ+NEXOBS], obsd_t *data) +static void saveslips(uint8_t slips[][MAXCODE], obsd_t *data) { - int i; - for (i=0;iLLI[i]&1) slips[data->sat-1][i]|=LLI_SLIP; + for (int i=0;icode[i]; + if (data->LLI[i]&1) slips[data->sat-1][code]=1; } } /* restore cycle slips -------------------------------------------------------*/ -static void restslips(uint8_t slips[][NFREQ+NEXOBS], obsd_t *data) +static void restslips(uint8_t slips[][MAXCODE], obsd_t *data) { - int i; - for (i=0;isat-1][i]&1) data->LLI[i]|=LLI_SLIP; - slips[data->sat-1][i]=0; + for (int i=0;icode[i]; + if (slips[data->sat-1][code]&1) data->LLI[i]|=LLI_SLIP; + slips[data->sat-1][code]=0; } } /* add observation data ------------------------------------------------------*/ @@ -1115,7 +1115,7 @@ static int readrnxobs(FILE *fp, gtime_t ts, gtime_t te, double tint, { gtime_t eventime={0},time0={0},time1={0}; obsd_t *data; - uint8_t slips[MAXSAT][NFREQ+NEXOBS]={{0}}; + uint8_t slips[MAXSAT][MAXCODE]={{0}}; int i,n,n1=0,flag=0,stat=0; double dtime1=0; diff --git a/src/rtcm.c b/src/rtcm.c index c0adf530d..c8f5eee33 100644 --- a/src/rtcm.c +++ b/src/rtcm.c @@ -70,7 +70,7 @@ extern int init_rtcm(rtcm_t *rtcm) eph_t eph0 ={0,-1,-1}; geph_t geph0={0,-1}; ssr_t ssr0={{{0}}}; - int i,j; + int i; trace(3,"init_rtcm:\n"); @@ -92,10 +92,11 @@ extern int init_rtcm(rtcm_t *rtcm) rtcm->msg[0]=rtcm->msgtype[0]=rtcm->opt[0]='\0'; for (i=0;i<6;i++) rtcm->msmtype[i][0]='\0'; rtcm->obsflag=rtcm->ephsat=0; - for (i=0;icp[i][j]=0.0; - rtcm->lock[i][j]=rtcm->loss[i][j]=0; - rtcm->lltime[i][j]=time0; + for (i=0;icp[i][code]=0.0; + rtcm->lock[i][code]=rtcm->loss[i][code]=0; + rtcm->lltime[i][code]=time0; } rtcm->nbyte=rtcm->nbit=rtcm->len=0; rtcm->word=0; diff --git a/src/rtcm2.c b/src/rtcm2.c index cac2465cf..fb5bb0413 100644 --- a/src/rtcm2.c +++ b/src/rtcm2.c @@ -195,10 +195,11 @@ static int decode_type17(rtcm_t *rtcm) /* decode type 18: rtk uncorrected carrier-phase -----------------------------*/ static int decode_type18(rtcm_t *rtcm) { - gtime_t time; - double usec,cp,tt; + double usec,cp; int i=48,index,freq,sync=1,code,sys,prn,sat,loss; + if (rtcm->obsflag) rtcm->obs.n = rtcm->obsflag = 0; + trace(4,"decode_type18: len=%d\n",rtcm->len); if (i+24<=rtcm->len*8) { @@ -215,7 +216,9 @@ static int decode_type18(rtcm_t *rtcm) } freq>>=1; - while (i+48<=rtcm->len*8&&rtcm->obs.ntime, usec * 1E-6); + + while (i+48<=rtcm->len*8) { sync=getbitu(rtcm->buff,i, 1); i+= 1; code=getbitu(rtcm->buff,i, 1); i+= 1; sys =getbitu(rtcm->buff,i, 1); i+= 1; @@ -227,19 +230,18 @@ static int decode_type18(rtcm_t *rtcm) trace(2,"rtcm2 18 satellite number error: sys=%d prn=%d\n",sys,prn); continue; } - time=timeadd(rtcm->time,usec*1E-6); - if (sys) time=utc2gpst(time); /* convert glonass time to gpst */ + gtime_t time; + if (sys) time=utc2gpst(rtime); /* convert glonass time to gpst */ + else time=rtime; - tt=timediff(rtcm->obs.data[0].time,time); - if (rtcm->obsflag||fabs(tt)>1E-9) { - rtcm->obs.n=rtcm->obsflag=0; - } + double tt=timediff(time,rtcm->obs.data[0].time); + if (fabs(tt)>1E-9) rtcm->obs.n=rtcm->obsflag=0; if ((index=obsindex(&rtcm->obs,time,sat))>=0) { rtcm->obs.data[index].L[freq]=-cp/256.0; - rtcm->obs.data[index].LLI[freq]=rtcm->loss[sat-1][freq]!=loss; - rtcm->obs.data[index].code[freq]= - !freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C); - rtcm->loss[sat-1][freq]=loss; + int lcode=!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C); + rtcm->obs.data[index].LLI[freq]=rtcm->loss[sat-1][lcode]!=loss?LLI_SLIP:0; + rtcm->obs.data[index].code[freq]=lcode; + rtcm->loss[sat-1][lcode]=loss; } } rtcm->obsflag=!sync; @@ -248,12 +250,13 @@ static int decode_type18(rtcm_t *rtcm) /* decode type 19: rtk uncorrected pseudorange -------------------------------*/ static int decode_type19(rtcm_t *rtcm) { - gtime_t time; - double usec,pr,tt; - int i=48,index,freq,sync=1,code,sys,prn,sat; + double usec,pr; + int i=48,freq,sync=1,code,sys,prn,sat; trace(4,"decode_type19: len=%d\n",rtcm->len); + if (rtcm->obsflag) rtcm->obs.n = rtcm->obsflag = 0; + if (i+24<=rtcm->len*8) { freq=getbitu(rtcm->buff,i, 2); i+= 2+2; usec=getbitu(rtcm->buff,i,20); i+=20; @@ -268,7 +271,7 @@ static int decode_type19(rtcm_t *rtcm) } freq>>=1; - while (i+48<=rtcm->len*8&&rtcm->obs.nlen*8) { sync=getbitu(rtcm->buff,i, 1); i+= 1; code=getbitu(rtcm->buff,i, 1); i+= 1; sys =getbitu(rtcm->buff,i, 1); i+= 1; @@ -279,17 +282,25 @@ static int decode_type19(rtcm_t *rtcm) trace(2,"rtcm2 19 satellite number error: sys=%d prn=%d\n",sys,prn); continue; } - time=timeadd(rtcm->time,usec*1E-6); + gtime_t time=timeadd(rtcm->time,usec*1E-6); if (sys) time=utc2gpst(time); /* convert glonass time to gpst */ - tt=timediff(rtcm->obs.data[0].time,time); - if (rtcm->obsflag||fabs(tt)>1E-9) { - rtcm->obs.n=rtcm->obsflag=0; - } - if ((index=obsindex(&rtcm->obs,time,sat))>=0) { - rtcm->obs.data[index].P[freq]=pr*0.02; - rtcm->obs.data[index].code[freq]= - !freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C); + double tt=timediff(time, rtcm->obs.data[0].time); + if (fabs(tt)>1E-9) rtcm->obs.n=rtcm->obsflag=0; + int index=obsindex(&rtcm->obs,time,sat); + if (index>=0) { + double P = pr * 0.02; + rtcm->obs.data[index].P[freq] = P; + int lcode = rtcm->obs.data[index].code[freq]; + int pcode = !freq ? (code ? CODE_L1P : CODE_L1C) : (code ? CODE_L2P : CODE_L2C); + rtcm->obs.data[index].code[freq] = pcode; + if (lcode > 0) { + if (pcode != lcode) trace(2, "rtcm2 19 code mismatch Lcode=%d Pcode=%d\n", lcode, pcode); + double L = rtcm->obs.data[index].L[freq]; + double lam = freq ? CLIGHT / FREQL2 : CLIGHT / FREQL1; + double n = floor((P / lam - L) / pow(2, 23) + 0.5); + rtcm->obs.data[index].L[freq] = n * pow(2, 23) + L; + } } } rtcm->obsflag=!sync; diff --git a/src/rtcm3.c b/src/rtcm3.c index f9a7f75f8..473422369 100644 --- a/src/rtcm3.c +++ b/src/rtcm3.c @@ -199,19 +199,19 @@ static void adjday_glot(rtcm_t *rtcm, double tod) rtcm->time=utc2gpst(timeadd(time,-10800.0)); } /* adjust carrier-phase rollover ---------------------------------------------*/ -static double adjcp(rtcm_t *rtcm, int sat, int idx, double cp) +static double adjcp(rtcm_t *rtcm, int sat, int code, double cp) { - if (rtcm->cp[sat-1][idx]==0.0) ; - else if (cpcp[sat-1][idx]-750.0) cp+=1500.0; - else if (cp>rtcm->cp[sat-1][idx]+750.0) cp-=1500.0; - rtcm->cp[sat-1][idx]=cp; + if (rtcm->cp[sat-1][code]==0.0) ; + else if (cpcp[sat-1][code]-750.0) cp+=1500.0; + else if (cp>rtcm->cp[sat-1][code]+750.0) cp-=1500.0; + rtcm->cp[sat-1][code]=cp; return cp; } /* loss-of-lock indicator ----------------------------------------------------*/ -static int lossoflock(rtcm_t *rtcm, int sat, int idx, int lock) +static int lossoflock(rtcm_t *rtcm, int sat, int code, int lock) { - int lli=(!lock&&!rtcm->lock[sat-1][idx])||locklock[sat-1][idx]; - rtcm->lock[sat-1][idx]=(uint16_t)lock; + int lli=(!lock&&!rtcm->lock[sat-1][code])||locklock[sat-1][code]; + rtcm->lock[sat-1][code]=(uint16_t)lock; return lli; } /* S/N ratio -----------------------------------------------------------------*/ @@ -308,7 +308,7 @@ static int decode_type1001(rtcm_t *rtcm) /* decode type 1002: extended L1-only GPS RTK observables --------------------*/ static int decode_type1002(rtcm_t *rtcm) { - double pr1,cnr1,tt,cp1,freq=FREQL1; + double pr1,cnr1,tt; int i=24+64,j,index,nsat,sync,prn,code,sat,ppr1,lock1,amb,sys; if ((nsat=decode_head1001(rtcm,&sync))<0) return -1; @@ -339,13 +339,15 @@ static int decode_type1002(rtcm_t *rtcm) pr1=pr1*0.02+amb*PRUNIT_GPS; rtcm->obs.data[index].P[0]=pr1; + int l1code = code ? CODE_L1P : CODE_L1C; if (ppr1!=(int)0xFFF80000) { - cp1=adjcp(rtcm,sat,0,ppr1*0.0005*freq/CLIGHT); + double freq=FREQL1; + double cp1=adjcp(rtcm,sat,l1code,ppr1*0.0005*freq/CLIGHT); rtcm->obs.data[index].L[0]=pr1*freq/CLIGHT+cp1; } - rtcm->obs.data[index].LLI[0]=lossoflock(rtcm,sat,0,lock1); + rtcm->obs.data[index].LLI[0]=lossoflock(rtcm,sat,l1code,lock1); rtcm->obs.data[index].SNR[0]=snratio(cnr1*0.25); - rtcm->obs.data[index].code[0]=code?CODE_L1P:CODE_L1C; + rtcm->obs.data[index].code[0]=l1code; } return sync?0:1; } @@ -360,8 +362,7 @@ static int decode_type1003(rtcm_t *rtcm) /* decode type 1004: extended L1&L2 GPS RTK observables ----------------------*/ static int decode_type1004(rtcm_t *rtcm) { - const int L2codes[]={CODE_L2X,CODE_L2P,CODE_L2D,CODE_L2W}; - double pr1,cnr1,cnr2,tt,cp1,cp2,freq[2]={FREQL1,FREQL2}; + double pr1,cnr1,cnr2,tt; int i=24+64,j,index,nsat,sync,prn,sat,code1,code2,pr21,ppr1,ppr2; int lock1,lock2,amb,sys; @@ -398,24 +399,28 @@ static int decode_type1004(rtcm_t *rtcm) pr1=pr1*0.02+amb*PRUNIT_GPS; rtcm->obs.data[index].P[0]=pr1; + const double freq[2]={FREQL1,FREQL2}; + int l1code = code1?CODE_L1P:CODE_L1C; if (ppr1!=(int)0xFFF80000) { - cp1=adjcp(rtcm,sat,0,ppr1*0.0005*freq[0]/CLIGHT); + double cp1=adjcp(rtcm,sat,l1code,ppr1*0.0005*freq[0]/CLIGHT); rtcm->obs.data[index].L[0]=pr1*freq[0]/CLIGHT+cp1; } - rtcm->obs.data[index].LLI[0]=lossoflock(rtcm,sat,0,lock1); + rtcm->obs.data[index].LLI[0]=lossoflock(rtcm,sat,l1code,lock1); rtcm->obs.data[index].SNR[0]=snratio(cnr1*0.25); - rtcm->obs.data[index].code[0]=code1?CODE_L1P:CODE_L1C; + rtcm->obs.data[index].code[0]=l1code; if (pr21!=(int)0xFFFFE000) { rtcm->obs.data[index].P[1]=pr1+pr21*0.02; } + const int L2codes[]={CODE_L2X,CODE_L2P,CODE_L2D,CODE_L2W}; + int l2code=L2codes[code2]; if (ppr2!=(int)0xFFF80000) { - cp2=adjcp(rtcm,sat,1,ppr2*0.0005*freq[1]/CLIGHT); + double cp2=adjcp(rtcm,sat,l2code,ppr2*0.0005*freq[1]/CLIGHT); rtcm->obs.data[index].L[1]=pr1*freq[1]/CLIGHT+cp2; } - rtcm->obs.data[index].LLI[1]=lossoflock(rtcm,sat,1,lock2); + rtcm->obs.data[index].LLI[1]=lossoflock(rtcm,sat,l2code,lock2); rtcm->obs.data[index].SNR[1]=snratio(cnr2*0.25); - rtcm->obs.data[index].code[1]=L2codes[code2]; + rtcm->obs.data[index].code[1]=l2code; } rtcm->obsflag=!sync; return sync?0:1; @@ -616,7 +621,7 @@ static int decode_type1009(rtcm_t *rtcm) /* decode type 1010: extended L1-only glonass rtk observables ----------------*/ static int decode_type1010(rtcm_t *rtcm) { - double pr1,cnr1,tt,cp1,freq1; + double pr1,cnr1,tt; int i=24+61,j,index,nsat,sync,prn,sat,code,fcn,ppr1,lock1,amb,sys=SYS_GLO; if ((nsat=decode_head1009(rtcm,&sync))<0) return -1; @@ -645,14 +650,15 @@ static int decode_type1010(rtcm_t *rtcm) pr1=pr1*0.02+amb*PRUNIT_GLO; rtcm->obs.data[index].P[0]=pr1; + int l1code=code?CODE_L1P:CODE_L1C; if (ppr1!=(int)0xFFF80000) { - freq1=code2freq(SYS_GLO,CODE_L1C,fcn-7); - cp1=adjcp(rtcm,sat,0,ppr1*0.0005*freq1/CLIGHT); + double freq1=code2freq(SYS_GLO,l1code,fcn-7); + double cp1=adjcp(rtcm,sat,l1code,ppr1*0.0005*freq1/CLIGHT); rtcm->obs.data[index].L[0]=pr1*freq1/CLIGHT+cp1; } - rtcm->obs.data[index].LLI[0]=lossoflock(rtcm,sat,0,lock1); + rtcm->obs.data[index].LLI[0]=lossoflock(rtcm,sat,l1code,lock1); rtcm->obs.data[index].SNR[0]=snratio(cnr1*0.25); - rtcm->obs.data[index].code[0]=code?CODE_L1P:CODE_L1C; + rtcm->obs.data[index].code[0]=l1code; } return sync?0:1; } @@ -702,26 +708,28 @@ static int decode_type1012(rtcm_t *rtcm) pr1=pr1*0.02+amb*PRUNIT_GLO; rtcm->obs.data[index].P[0]=pr1; + int l1code=code1?CODE_L1P:CODE_L1C; if (ppr1!=(int)0xFFF80000) { - freq1=code2freq(SYS_GLO,CODE_L1C,fcn-7); - cp1=adjcp(rtcm,sat,0,ppr1*0.0005*freq1/CLIGHT); + freq1=code2freq(SYS_GLO,l1code,fcn-7); + cp1=adjcp(rtcm,sat,l1code,ppr1*0.0005*freq1/CLIGHT); rtcm->obs.data[index].L[0]=pr1*freq1/CLIGHT+cp1; } - rtcm->obs.data[index].LLI[0]=lossoflock(rtcm,sat,0,lock1); + rtcm->obs.data[index].LLI[0]=lossoflock(rtcm,sat,l1code,lock1); rtcm->obs.data[index].SNR[0]=snratio(cnr1*0.25); - rtcm->obs.data[index].code[0]=code1?CODE_L1P:CODE_L1C; + rtcm->obs.data[index].code[0]=l1code; if (pr21!=(int)0xFFFFE000) { rtcm->obs.data[index].P[1]=pr1+pr21*0.02; } + int l2code=code2?CODE_L2P:CODE_L2C; if (ppr2!=(int)0xFFF80000) { - freq2=code2freq(SYS_GLO,CODE_L2C,fcn-7); - cp2=adjcp(rtcm,sat,1,ppr2*0.0005*freq2/CLIGHT); + freq2=code2freq(SYS_GLO,l2code,fcn-7); + cp2=adjcp(rtcm,sat,l2code,ppr2*0.0005*freq2/CLIGHT); rtcm->obs.data[index].L[1]=pr1*freq2/CLIGHT+cp2; } - rtcm->obs.data[index].LLI[1]=lossoflock(rtcm,sat,1,lock2); + rtcm->obs.data[index].LLI[1]=lossoflock(rtcm,sat,l2code,lock2); rtcm->obs.data[index].SNR[1]=snratio(cnr2*0.25); - rtcm->obs.data[index].code[1]=code2?CODE_L2P:CODE_L2C; + rtcm->obs.data[index].code[1]=l2code; } rtcm->obsflag=!sync; return sync?0:1; @@ -2089,8 +2097,8 @@ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, rtcm->obs.data[index].D[idx[k]]= (float)(-(rr[i]+rrf[j])*freq/CLIGHT); } - rtcm->obs.data[index].LLI[idx[k]]= - lossoflock(rtcm,sat,idx[k],lock[j])+(half[j]?2:0); + int LLI = lossoflock(rtcm,sat,code[k],lock[j])+(half[j]?LLI_HALFC:0); + 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]; } diff --git a/src/rtcm3e.c b/src/rtcm3e.c index 3ed0eda81..b2cdf1d25 100644 --- a/src/rtcm3e.c +++ b/src/rtcm3e.c @@ -2013,12 +2013,13 @@ static void gen_msm_sig(rtcm_t *rtcm, int sys, int nsat, int nsig, int ncell, if (!(sat=to_satid(sys,data->sat))) continue; for (j=0;jcode[j]))) continue; + int code = data->code[j]; + if (!(sig=to_sigid(sys,code))) continue; k=sat_ind[sat-1]-1; if ((cell=cell_ind[sig_ind[sig-1]-1+k*nsig])>=64) continue; - freq=code2freq(sys,data->code[j],fcn-7); + freq=code2freq(sys,code,fcn-7); lambda=freq==0.0?0.0:CLIGHT/freq; psrng_s=data->P[j]==0.0?0.0:data->P[j]-rrng[k]; phrng_s=data->L[j]==0.0||lambda<=0.0?0.0: data->L[j]*lambda-rrng [k]; @@ -2026,13 +2027,13 @@ static void gen_msm_sig(rtcm_t *rtcm, int sys, int nsat, int nsig, int ncell, /* subtract phase - psudorange integer cycle offset */ LLI=data->LLI[j]; - if ((LLI&1)||fabs(phrng_s-rtcm->cp[data->sat-1][j])>1171.0) { - rtcm->cp[data->sat-1][j]=ROUND(phrng_s/lambda)*lambda; + if ((LLI&1)||fabs(phrng_s-rtcm->cp[data->sat-1][code])>1171.0) { + rtcm->cp[data->sat-1][code]=ROUND(phrng_s/lambda)*lambda; LLI|=1; } - phrng_s-=rtcm->cp[data->sat-1][j]; + phrng_s-=rtcm->cp[data->sat-1][code]; - lt=locktime_d(data->time,rtcm->lltime[data->sat-1]+j,LLI); + lt=locktime_d(data->time,rtcm->lltime[data->sat-1]+code,LLI); if (psrng&&psrng_s!=0.0) psrng[cell-1]=psrng_s; if (phrng&&phrng_s!=0.0) phrng[cell-1]=phrng_s; diff --git a/src/rtklib.h b/src/rtklib.h index 1a073dab2..ed6e3bee3 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -954,10 +954,10 @@ typedef struct { /* RTCM control struct type */ int obsflag; /* obs data complete flag (1:ok,0:not complete) */ int ephsat; /* input ephemeris satellite number */ int ephset; /* input ephemeris set (0-1) */ - double cp[MAXSAT][NFREQ+NEXOBS]; /* carrier-phase measurement */ - uint16_t lock[MAXSAT][NFREQ+NEXOBS]; /* lock time */ - uint16_t loss[MAXSAT][NFREQ+NEXOBS]; /* loss of lock count */ - gtime_t lltime[MAXSAT][NFREQ+NEXOBS]; /* last lock time */ + double cp[MAXSAT][MAXCODE]; /* carrier-phase measurement */ + uint16_t lock[MAXSAT][MAXCODE]; /* lock time */ + uint16_t loss[MAXSAT][MAXCODE]; /* loss of lock count */ + gtime_t lltime[MAXSAT][MAXCODE]; /* last lock time */ int nbyte; /* number of bytes in message buffer */ int nbit; /* number of bits in word buffer */ int len; /* message length (bytes) */ @@ -1206,7 +1206,7 @@ typedef struct { /* RTK control/result type */ typedef struct { /* receiver raw data control type */ gtime_t time; /* message time */ - gtime_t tobs[MAXSAT][NFREQ+NEXOBS]; /* observation data time */ + gtime_t tobs[MAXSAT][MAXCODE]; /* observation data time */ obs_t obs; /* observation data */ obs_t obuf; /* observation data buffer */ nav_t nav; /* satellite ephemerides */ @@ -1216,11 +1216,11 @@ typedef struct { /* receiver raw data control type */ sbsmsg_t sbsmsg; /* SBAS message */ char msgtype[256]; /* last message type */ uint8_t subfrm[MAXSAT][380]; /* subframe buffer */ - double lockt[MAXSAT][NFREQ+NEXOBS]; /* lock time (s) */ - unsigned char lockflag[MAXSAT][NFREQ+NEXOBS]; /* used for carrying forward cycle slip */ + double lockt[MAXSAT][MAXCODE]; /* lock time (s) */ + unsigned char lockflag[MAXSAT][MAXCODE]; /* used for carrying forward cycle slip */ double icpp[MAXSAT],off[MAXSAT],icpc; /* carrier params for ss2 */ double prCA[MAXSAT],dpCA[MAXSAT]; /* L1/CA pseudorange/doppler for javad */ - uint8_t halfc[MAXSAT][NFREQ+NEXOBS]; /* half-cycle resolved */ + uint8_t halfc[MAXSAT][MAXCODE]; /* half-cycle resolved */ char freqn[MAXOBS]; /* frequency number for javad */ int nbyte; /* number of bytes in message buffer */ int len; /* message length (bytes) */