From 59daf2271786c89c86d41d6a03646c211f299700 Mon Sep 17 00:00:00 2001 From: Our Air Quality Date: Sun, 22 Sep 2024 02:42:56 +1000 Subject: [PATCH 1/2] Index signal specific data using the signal code Rather than the signal frequency index which is still used in the observation structure to keep that compact. When there are many signals the set of signals can change and when the priorities are recomputed the frequency index can change so the frequency index is not a reliable key for this state. This is not an issue with just one signal, or perhaps two, but with more than NFREQ signals there were subtle issues as the priories changed and the state was mixed up. There are a good few structures with just one array of data for each signal and these have been expanded from being NFREQ+NEXOBS to MAXCODE so that the signal code can be used as the index. This is a little wasteful of memory but simple. --- src/convrnx.c | 152 ++++++++++++++++++++++++------------------- src/rcv/novatel.c | 58 +++++++++-------- src/rcv/septentrio.c | 39 ++++++----- src/rcv/ublox.c | 70 +++++++++++--------- src/rcv/unicore.c | 12 ++-- src/rcvraw.c | 9 +-- src/rinex.c | 20 +++--- src/rtcm.c | 11 ++-- src/rtcm2.c | 12 ++-- src/rtcm3.c | 78 ++++++++++++---------- src/rtcm3e.c | 13 ++-- src/rtklib.h | 16 ++--- 12 files changed, 264 insertions(+), 226 deletions(-) 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..ee8fec6c6 100644 --- a/src/rtcm2.c +++ b/src/rtcm2.c @@ -236,10 +236,10 @@ static int decode_type18(rtcm_t *rtcm) } 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; + rtcm->obs.data[index].code[freq]=lcode; + rtcm->loss[sat-1][lcode]=loss; } } rtcm->obsflag=!sync; @@ -288,8 +288,8 @@ static int decode_type19(rtcm_t *rtcm) } 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); + int pcode=!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C); + rtcm->obs.data[index].code[freq]=pcode; } } 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) */ From 69f53dbada48fa99a5ad183561edcd7597c26206 Mon Sep 17 00:00:00 2001 From: Our Air Quality Date: Wed, 25 Sep 2024 01:42:26 +1000 Subject: [PATCH 2/2] rtcm3: implement lock time slip checking Support determining the reported lock time from the lock time indicators. Calculate the time since the last observation (per signal). Use these to also check for a possible loss of lock. A slip might now be reported on an outage. Special case for receivers (Septentrio) that send a lock time indicator of zero on a half cycle ambiguity. Consider these as an outage deferring the decision to flag a slip until the signal returns. For a short period of half cycle ambiguity that recovers, a slip report might now be avoided. The functions for converting the lock time indicator to a time also support returning the time increment to the next indicator value. This was not found to be needed. Fix the observation flushing after a sync, which had been guarded by the max obs overflow check so that once the max obs limit was reached it was stuck. --- src/rtcm.c | 4 +- src/rtcm3.c | 295 ++++++++++++++++++++++++++++++++++++++++----------- src/rtklib.h | 4 +- 3 files changed, 238 insertions(+), 65 deletions(-) diff --git a/src/rtcm.c b/src/rtcm.c index c8f5eee33..9b6d142e5 100644 --- a/src/rtcm.c +++ b/src/rtcm.c @@ -92,10 +92,12 @@ 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; + rtcm->stime = time0; for (i=0;icp[i][code]=0.0; - rtcm->lock[i][code]=rtcm->loss[i][code]=0; + rtcm->lti[i][code]=rtcm->loss[i][code]=0; + rtcm->tobs[i][code] = time0; rtcm->lltime[i][code]=time0; } rtcm->nbyte=rtcm->nbit=rtcm->len=0; diff --git a/src/rtcm3.c b/src/rtcm3.c index 473422369..b28419702 100644 --- a/src/rtcm3.c +++ b/src/rtcm3.c @@ -207,13 +207,186 @@ static double adjcp(rtcm_t *rtcm, int sat, int code, double cp) rtcm->cp[sat-1][code]=cp; return cp; } -/* loss-of-lock indicator ----------------------------------------------------*/ -static int lossoflock(rtcm_t *rtcm, int sat, int code, int lock) -{ - int lli=(!lock&&!rtcm->lock[sat-1][code])||locklock[sat-1][code]; - rtcm->lock[sat-1][code]=(uint16_t)lock; - return lli; + +// Convert a RTCM 7 bit lock time indicator into the time in seconds. +// Optionally return the range scale per index increment which can also give +// the maximum time for the range as min + k. +static double locktime(int lti, double *ko) +{ + if (lti == 127) { + // There is no upper range for index 127, so return k of 1 year which + // should be adequate to allow the caller to work with without special + // cases. + if (ko) *ko = 365 * 24 * 60 * 60; + return 937; + } + + int k = 1, base = 0; + if (lti < 24) { k = 1; base = 0; } + else if (lti < 48) { k = 2; base = 24; } + else if (lti < 72) { k = 4; base = 120; } + else if (lti < 96) { k = 8; base = 408; } + else if (lti < 120) { k = 16; base = 1176; } + else if (lti < 127) { k = 32; base = 3096; } + else + trace(2, "Warning: 7 bit lock time out of bounds: i=%d\n", lti); + + if (ko) { + // Index 126 maps to a minimum time of 936 seconds, and index 127 maps to + // a time >= 937 seconds, a step of just 1 second. Might have been cleaner + // to have a step of 32 and then index 127 would have mapped to >= 968 + // seconds, but it is specified as >= 937 seconds in Table 3.4-2, and + // confirmed that Septentrio RTCM3 streams have a one second step here. + if (lti == 126) *ko = 1; + // At the other boundaries the ranges have the same increment k as the + // previous range. + else *ko = k; + } + + return k * lti - base; +} +// Convert a RTCM MSM 4 bit lock time indicator into the minimum range time +// with 1ms resolution. The maximum time in a range is twice the minimum lock +// time, as it doubles with each range, except for the last range. Optionally +// return the range scale per index increment which can also give the maximum +// time for the range as min + k. +static double msm_locktime(int lti, double *ko) +{ + if (lti > 15) { + trace(2, "Unexpected lock time indicator %d\n", lti); + lti = 15; + } + + if (lti == 0) { + if (ko) *ko = 32 * 0.001; + return 0; + } + + double min = pow(2, 4 + lti) * 0.001; + + // When the indicator is 15 the upper time is unlimited, but use a time of 1 + // year here which should be adequate to allow the caller to work with + // without special cases. + if (ko) { + if (lti < 15) *ko = pow(2, 5 + lti) * 0.001 - min; + else *ko = 365 * 24 * 60 * 60; + } + + return min; +} +// Convert a RCTM MSM 10 bit lock time indicator into a time with 1ms +// resolution. Optionally return the range scale per index increment which can +// also give the maximum time for the range as min + k. +static double msm_locktime_ex(int lti, double *ko) +{ + int c = 1; + int base = 0; + if (lti < 64) { c = 0; base = 0; } + else if (lti < 96) { c = 1; base = 64; } + else if (lti < 128) { c = 2; base = 96; } + else if (lti < 160) { c = 3; base = 128; } + else if (lti < 192) { c = 4; base = 160; } + else if (lti < 224) { c = 5; base = 192; } + else if (lti < 256) { c = 6; base = 224; } + else if (lti < 288) { c = 7; base = 256; } + else if (lti < 320) { c = 8; base = 288; } + else if (lti < 352) { c = 9; base = 320; } + else if (lti < 384) { c = 10; base = 352; } + else if (lti < 416) { c = 11; base = 384; } + else if (lti < 448) { c = 12; base = 416; } + else if (lti < 480) { c = 13; base = 448; } + else if (lti < 512) { c = 14; base = 480; } + else if (lti < 544) { c = 15; base = 512; } + else if (lti < 576) { c = 16; base = 544; } + else if (lti < 608) { c = 17; base = 576; } + else if (lti < 640) { c = 18; base = 608; } + else if (lti < 672) { c = 19; base = 640; } + else if (lti < 704) { c = 20; base = 672; } + else if (lti == 704) { c = 21; base = 704; } + else + trace(2, "Warning: High res lock time out of bounds: %d\n", lti); + + double pmax = lti < 64 ? 0 : pow(2, c + 5) * 0.001; + double k = pow(2, c) * 0.001; + double min = k * (lti - base) + pmax; + + // The scale within a range has a regular value, and the boundaries also + // apply the same scale so the maximum of a range is the min + k, even for + // index 703 to index 704 (unlike the 7 bit lock scale). Index values above + // 704 are reserved, but if not then they might continue ranges of 32 and + // doubling the scale, or just keep the same scale to 1023. + // + // When the indicator is 704 the upper time is unlimited, but use a time of + // 1 year here which should be adequate to allow the caller to work with + // without special cases. + if (ko) { + if (lti < 704) *ko = k; + else *ko = 365 * 24 * 60 * 60; + } + + return min; +} +/* Loss-of-lock indicator ---------------------------------------------------- + * locktype: 7 for non-MSM 7 bit; 4 for MSM 4 bit; or 10 for MSM 10 bit. + */ +static int lossoflock(rtcm_t *rtcm, int sat, int code, int idx, int lti, int type, int halfc, double L) { + gtime_t tobs = rtcm->tobs[sat - 1][code]; + if (tobs.time == 0) { + // Initialize the lock time to the first epoch, to give the outage + // time if this is the first instance of this signal. + if (rtcm->stime.time == 0) rtcm->stime = rtcm->time; + // Add on an extra 30 seconds to estimate the last lock time, an + // estimate of prior epoch. So that a loss is triggered if the + // lock time starts after this prior estimated epoch. + tobs = timeadd(rtcm->stime, -30.0); + rtcm->tobs[sat - 1][code] = tobs; + } + double dt = timediff(rtcm->time, tobs); + + // Previous epoch lock time indicator + int plti = rtcm->lti[sat - 1][code]; + + double nmin; + if (type == 10) { + nmin = msm_locktime_ex(lti,NULL); + } else if (type == 4) { + nmin = msm_locktime(lti,NULL); + } else if (type == 7) { + nmin = locktime(lti,NULL); + } else { + // Internal error + return 0; + } + + if (lti == 0 && (halfc || L == 0)) { + // Septentrio reports a lock time indicator of zero during a half + // cycle ambiguity, but the lock might not lose continuity, so + // consider it a lock outage. Seems similar to the Septentrio SBF + // meas3 lock encoding which can not encode a lock time when there + // is a half cycle ambiguity. + // + // The carrier phase is also reported as invalid, zero here, so omitted, + // deferring the continuity report until resolved. + // + // For the non-MSM paths, the halfc flag is not available, it + // just sends lti of zero and the carrier phase as invalid. + return 0; + } + + // Firstly check the raw lock time indicator. + // + // Detect a possible loss, where there was time since the last reported + // lock time for a reset of the lock timer and for it to again reach this + // same range. + int loc = 0; + if (lti < plti || dt - nmin > 1e-4) { + loc = 1; + } + rtcm->lti[sat - 1][code] = lti; + rtcm->tobs[sat - 1][code] = rtcm->time; + return loc ? LLI_SLIP : 0; } + /* S/N ratio -----------------------------------------------------------------*/ static uint16_t snratio(double snr) { @@ -271,6 +444,8 @@ static int decode_head1001(rtcm_t *rtcm, int *sync) char *msg,tstr[40]; int i=24,staid,nsat,type; + if (rtcm->obsflag) rtcm->obs.n = rtcm->obsflag = 0; + type=getbitu(rtcm->buff,i,12); i+=12; if (i+52<=rtcm->len*8) { @@ -309,16 +484,16 @@ static int decode_type1001(rtcm_t *rtcm) static int decode_type1002(rtcm_t *rtcm) { double pr1,cnr1,tt; - int i=24+64,j,index,nsat,sync,prn,code,sat,ppr1,lock1,amb,sys; + int i=24+64,j,index,nsat,sync,prn,code,sat,ppr1,lti1,amb,sys; if ((nsat=decode_head1001(rtcm,&sync))<0) return -1; - for (j=0;jobs.nlen*8;j++) { + for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i, 6); i+= 6; code =getbitu(rtcm->buff,i, 1); i+= 1; pr1 =getbitu(rtcm->buff,i,24); i+=24; ppr1 =getbits(rtcm->buff,i,20); i+=20; - lock1=getbitu(rtcm->buff,i, 7); i+= 7; + lti1=getbitu(rtcm->buff,i, 7); i+= 7; amb =getbitu(rtcm->buff,i, 8); i+= 8; cnr1 =getbitu(rtcm->buff,i, 8); i+= 8; if (prn<40) { @@ -332,9 +507,7 @@ static int decode_type1002(rtcm_t *rtcm) continue; } tt=timediff(rtcm->obs.data[0].time,rtcm->time); - if (rtcm->obsflag||fabs(tt)>1E-9) { - rtcm->obs.n=rtcm->obsflag=0; - } + if (fabs(tt)>1E-9) rtcm->obs.n=rtcm->obsflag=0; if ((index=obsindex(&rtcm->obs,rtcm->time,sat))<0) continue; pr1=pr1*0.02+amb*PRUNIT_GPS; rtcm->obs.data[index].P[0]=pr1; @@ -345,7 +518,7 @@ static int decode_type1002(rtcm_t *rtcm) 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,l1code,lock1); + rtcm->obs.data[index].LLI[0]=lossoflock(rtcm, sat, l1code, 0, lti1, 7, 0, rtcm->obs.data[index].L[0]); rtcm->obs.data[index].SNR[0]=snratio(cnr1*0.25); rtcm->obs.data[index].code[0]=l1code; } @@ -364,22 +537,22 @@ static int decode_type1004(rtcm_t *rtcm) { 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; + int lti1,lti2,amb,sys; if ((nsat=decode_head1001(rtcm,&sync))<0) return -1; - for (j=0;jobs.nlen*8;j++) { + for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i, 6); i+= 6; code1=getbitu(rtcm->buff,i, 1); i+= 1; pr1 =getbitu(rtcm->buff,i,24); i+=24; ppr1 =getbits(rtcm->buff,i,20); i+=20; - lock1=getbitu(rtcm->buff,i, 7); i+= 7; + lti1=getbitu(rtcm->buff,i, 7); i+= 7; amb =getbitu(rtcm->buff,i, 8); i+= 8; cnr1 =getbitu(rtcm->buff,i, 8); i+= 8; code2=getbitu(rtcm->buff,i, 2); i+= 2; pr21 =getbits(rtcm->buff,i,14); i+=14; ppr2 =getbits(rtcm->buff,i,20); i+=20; - lock2=getbitu(rtcm->buff,i, 7); i+= 7; + lti2=getbitu(rtcm->buff,i, 7); i+= 7; cnr2 =getbitu(rtcm->buff,i, 8); i+= 8; if (prn<40) { sys=SYS_GPS; @@ -392,9 +565,7 @@ static int decode_type1004(rtcm_t *rtcm) continue; } tt=timediff(rtcm->obs.data[0].time,rtcm->time); - if (rtcm->obsflag||fabs(tt)>1E-9) { - rtcm->obs.n=rtcm->obsflag=0; - } + if (fabs(tt)>1E-9) rtcm->obs.n=rtcm->obsflag=0; if ((index=obsindex(&rtcm->obs,rtcm->time,sat))<0) continue; pr1=pr1*0.02+amb*PRUNIT_GPS; rtcm->obs.data[index].P[0]=pr1; @@ -405,7 +576,7 @@ static int decode_type1004(rtcm_t *rtcm) 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,l1code,lock1); + rtcm->obs.data[index].LLI[0]=lossoflock(rtcm, sat, l1code, 0, lti1, 7, 0, rtcm->obs.data[index].L[0]); rtcm->obs.data[index].SNR[0]=snratio(cnr1*0.25); rtcm->obs.data[index].code[0]=l1code; @@ -418,7 +589,7 @@ static int decode_type1004(rtcm_t *rtcm) 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,l2code,lock2); + rtcm->obs.data[index].LLI[1]=lossoflock(rtcm, sat, l2code, 1, lti2, 7, 0, rtcm->obs.data[index].L[1]); rtcm->obs.data[index].SNR[1]=snratio(cnr2*0.25); rtcm->obs.data[index].code[1]=l2code; } @@ -584,6 +755,8 @@ static int decode_head1009(rtcm_t *rtcm, int *sync) char *msg,tstr[40]; int i=24,staid,nsat,type; + if (rtcm->obsflag) rtcm->obs.n = rtcm->obsflag = 0; + type=getbitu(rtcm->buff,i,12); i+=12; if (i+49<=rtcm->len*8) { @@ -622,17 +795,17 @@ static int decode_type1009(rtcm_t *rtcm) static int decode_type1010(rtcm_t *rtcm) { double pr1,cnr1,tt; - int i=24+61,j,index,nsat,sync,prn,sat,code,fcn,ppr1,lock1,amb,sys=SYS_GLO; + int i=24+61,j,index,nsat,sync,prn,sat,code,fcn,ppr1,lti1,amb,sys=SYS_GLO; if ((nsat=decode_head1009(rtcm,&sync))<0) return -1; - for (j=0;jobs.nlen*8;j++) { + for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i, 6); i+= 6; code =getbitu(rtcm->buff,i, 1); i+= 1; fcn =getbitu(rtcm->buff,i, 5); i+= 5; /* fcn+7 */ pr1 =getbitu(rtcm->buff,i,25); i+=25; ppr1 =getbits(rtcm->buff,i,20); i+=20; - lock1=getbitu(rtcm->buff,i, 7); i+= 7; + lti1=getbitu(rtcm->buff,i, 7); i+= 7; amb =getbitu(rtcm->buff,i, 7); i+= 7; cnr1 =getbitu(rtcm->buff,i, 8); i+= 8; if (!(sat=satno(sys,prn))) { @@ -643,9 +816,7 @@ static int decode_type1010(rtcm_t *rtcm) rtcm->nav.glo_fcn[prn-1]=fcn-7+8; /* fcn+8 */ } tt=timediff(rtcm->obs.data[0].time,rtcm->time); - if (rtcm->obsflag||fabs(tt)>1E-9) { - rtcm->obs.n=rtcm->obsflag=0; - } + if (fabs(tt)>1E-9) rtcm->obs.n=rtcm->obsflag=0; if ((index=obsindex(&rtcm->obs,rtcm->time,sat))<0) continue; pr1=pr1*0.02+amb*PRUNIT_GLO; rtcm->obs.data[index].P[0]=pr1; @@ -656,7 +827,7 @@ static int decode_type1010(rtcm_t *rtcm) 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,l1code,lock1); + rtcm->obs.data[index].LLI[0]=lossoflock(rtcm, sat, l1code, 0, lti1, 7, 0, rtcm->obs.data[index].L[0]); rtcm->obs.data[index].SNR[0]=snratio(cnr1*0.25); rtcm->obs.data[index].code[0]=l1code; } @@ -675,23 +846,23 @@ static int decode_type1012(rtcm_t *rtcm) { double pr1,cnr1,cnr2,tt,cp1,cp2,freq1,freq2; int i=24+61,j,index,nsat,sync,prn,sat,fcn,code1,code2,pr21,ppr1,ppr2; - int lock1,lock2,amb,sys=SYS_GLO; + int lti1,lti2,amb,sys=SYS_GLO; if ((nsat=decode_head1009(rtcm,&sync))<0) return -1; - for (j=0;jobs.nlen*8;j++) { + for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i, 6); i+= 6; code1=getbitu(rtcm->buff,i, 1); i+= 1; fcn =getbitu(rtcm->buff,i, 5); i+= 5; /* fcn+7 */ pr1 =getbitu(rtcm->buff,i,25); i+=25; ppr1 =getbits(rtcm->buff,i,20); i+=20; - lock1=getbitu(rtcm->buff,i, 7); i+= 7; + lti1=getbitu(rtcm->buff,i, 7); i+= 7; amb =getbitu(rtcm->buff,i, 7); i+= 7; cnr1 =getbitu(rtcm->buff,i, 8); i+= 8; code2=getbitu(rtcm->buff,i, 2); i+= 2; pr21 =getbits(rtcm->buff,i,14); i+=14; ppr2 =getbits(rtcm->buff,i,20); i+=20; - lock2=getbitu(rtcm->buff,i, 7); i+= 7; + lti2=getbitu(rtcm->buff,i, 7); i+= 7; cnr2 =getbitu(rtcm->buff,i, 8); i+= 8; if (!(sat=satno(sys,prn))) { trace(2,"rtcm3 1012 satellite number error: sys=%d prn=%d\n",sys,prn); @@ -701,9 +872,7 @@ static int decode_type1012(rtcm_t *rtcm) rtcm->nav.glo_fcn[prn-1]=fcn-7+8; /* fcn+8 */ } tt=timediff(rtcm->obs.data[0].time,rtcm->time); - if (rtcm->obsflag||fabs(tt)>1E-9) { - rtcm->obs.n=rtcm->obsflag=0; - } + if (fabs(tt)>1E-9) rtcm->obs.n=rtcm->obsflag=0; if ((index=obsindex(&rtcm->obs,rtcm->time,sat))<0) continue; pr1=pr1*0.02+amb*PRUNIT_GLO; rtcm->obs.data[index].P[0]=pr1; @@ -714,7 +883,7 @@ static int decode_type1012(rtcm_t *rtcm) 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,l1code,lock1); + rtcm->obs.data[index].LLI[0]=lossoflock(rtcm, sat, l1code, 0, lti1, 7, 0, rtcm->obs.data[index].L[0]); rtcm->obs.data[index].SNR[0]=snratio(cnr1*0.25); rtcm->obs.data[index].code[0]=l1code; @@ -727,7 +896,7 @@ static int decode_type1012(rtcm_t *rtcm) 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,l2code,lock2); + rtcm->obs.data[index].LLI[1]=lossoflock(rtcm, sat, l2code, 1, lti2, 7, 0, rtcm->obs.data[index].L[1]); rtcm->obs.data[index].SNR[1]=snratio(cnr2*0.25); rtcm->obs.data[index].code[1]=l2code; } @@ -1431,7 +1600,7 @@ static int decode_type1042(rtcm_t *rtcm) eph.ttr=rtcm->time; eph.A=sqrtA*sqrtA; if (!strstr(rtcm->opt,"-EPHALL")) { - if (timediff(eph.toe,rtcm->nav.eph[sat-1].toe)==0.0&& + if (fabs(timediff(eph.toe,rtcm->nav.eph[sat-1].toe)) < 1e-9 && eph.iode==rtcm->nav.eph[sat-1].iode&& eph.iodc==rtcm->nav.eph[sat-1].iodc) return 0; /* unchanged */ } @@ -1996,7 +2165,7 @@ static void sigindex(int sys, const uint8_t *code, int n, const char *opt, /* save obs data in MSM message ----------------------------------------------*/ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, const double *pr, const double *cp, const double *rr, - const double *rrf, const double *cnr, const int *lock, + const double *rrf, const double *cnr, const int *lti, int locktype, const int *ex, const int *half) { const char *sig[32]; @@ -2054,9 +2223,7 @@ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, if ((sat=satno(sys,prn))) { tt=timediff(rtcm->obs.data[0].time,rtcm->time); - if (rtcm->obsflag||fabs(tt)>1E-9) { - rtcm->obs.n=rtcm->obsflag=0; - } + if (fabs(tt)>1E-9) rtcm->obs.n=rtcm->obsflag=0; index=obsindex(&rtcm->obs,rtcm->time,sat); } else { @@ -2094,10 +2261,10 @@ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, } /* doppler (hz) */ if (rr&&rrf&&rrf[j]>-1E12) { - rtcm->obs.data[index].D[idx[k]]= - (float)(-(rr[i]+rrf[j])*freq/CLIGHT); + rtcm->obs.data[index].D[idx[k]]=-(rr[i]+rrf[j])*freq/CLIGHT; } - int LLI = lossoflock(rtcm,sat,code[k],lock[j])+(half[j]?LLI_HALFC:0); + int LLI = lossoflock(rtcm, sat, code[k], idx[k], lti[j], locktype, half[j], rtcm->obs.data[index].L[idx[k]]); + 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]; @@ -2115,6 +2282,8 @@ static int decode_msm_head(rtcm_t *rtcm, int sys, int *sync, int *iod, char *msg,tstr[40]; int i=24,j,dow,mask,staid,type,ncell=0; + if (rtcm->obsflag) rtcm->obs.n = rtcm->obsflag = 0; + type=getbitu(rtcm->buff,i,12); i+=12; *h=h0; @@ -2199,7 +2368,7 @@ static int decode_msm4(rtcm_t *rtcm, int sys) { msm_h_t h={0}; double r[64],pr[64],cp[64],cnr[64]; - int i,j,type,sync,iod,ncell,rng,rng_m,prv,cpv,lock[64],half[64]; + int i,j,type,sync,iod,ncell,rng,rng_m,prv,cpv,lti[64],half[64]; type=getbitu(rtcm->buff,24,12); @@ -2233,8 +2402,8 @@ static int decode_msm4(rtcm_t *rtcm, int sys) cpv=getbits(rtcm->buff,i,22); i+=22; if (cpv!=-2097152) cp[j]=cpv*P2_29*RANGE_MS; } - for (j=0;jbuff,i,4); i+=4; + for (j=0;jbuff,i,4); i+=4; } for (j=0;jbuff,i,1); i+=1; @@ -2243,7 +2412,7 @@ static int decode_msm4(rtcm_t *rtcm, int sys) cnr[j]=getbitu(rtcm->buff,i,6)*1.0; i+=6; } /* save obs data in msm message */ - save_msm_obs(rtcm,sys,&h,r,pr,cp,NULL,NULL,cnr,lock,NULL,half); + save_msm_obs(rtcm,sys,&h,r,pr,cp,NULL,NULL,cnr,lti,4,NULL,half); rtcm->obsflag=!sync; return sync?0:1; @@ -2253,7 +2422,7 @@ static int decode_msm5(rtcm_t *rtcm, int sys) { msm_h_t h={0}; double r[64],rr[64],pr[64],cp[64],rrf[64],cnr[64]; - int i,j,type,sync,iod,ncell,rng,rng_m,rate,prv,cpv,rrv,lock[64]; + int i,j,type,sync,iod,ncell,rng,rng_m,rate,prv,cpv,rrv,lti[64]; int ex[64],half[64]; type=getbitu(rtcm->buff,24,12); @@ -2297,8 +2466,8 @@ static int decode_msm5(rtcm_t *rtcm, int sys) cpv=getbits(rtcm->buff,i,22); i+=22; if (cpv!=-2097152) cp[j]=cpv*P2_29*RANGE_MS; } - for (j=0;jbuff,i,4); i+=4; + for (j=0;jbuff,i,4); i+=4; } for (j=0;jbuff,i,1); i+=1; @@ -2311,7 +2480,7 @@ static int decode_msm5(rtcm_t *rtcm, int sys) if (rrv!=-16384) rrf[j]=rrv*0.0001; } /* save obs data in msm message */ - save_msm_obs(rtcm,sys,&h,r,pr,cp,rr,rrf,cnr,lock,ex,half); + save_msm_obs(rtcm,sys,&h,r,pr,cp,rr,rrf,cnr,lti,4,ex,half); rtcm->obsflag=!sync; return sync?0:1; @@ -2321,7 +2490,7 @@ static int decode_msm6(rtcm_t *rtcm, int sys) { msm_h_t h={0}; double r[64],pr[64],cp[64],cnr[64]; - int i,j,type,sync,iod,ncell,rng,rng_m,prv,cpv,lock[64],half[64]; + int i,j,type,sync,iod,ncell,rng,rng_m,prv,cpv,lti[64],half[64]; type=getbitu(rtcm->buff,24,12); @@ -2355,8 +2524,8 @@ static int decode_msm6(rtcm_t *rtcm, int sys) cpv=getbits(rtcm->buff,i,24); i+=24; if (cpv!=-8388608) cp[j]=cpv*P2_31*RANGE_MS; } - for (j=0;jbuff,i,10); i+=10; + for (j=0;jbuff,i,10); i+=10; } for (j=0;jbuff,i,1); i+=1; @@ -2365,7 +2534,7 @@ static int decode_msm6(rtcm_t *rtcm, int sys) cnr[j]=getbitu(rtcm->buff,i,10)*0.0625; i+=10; } /* save obs data in msm message */ - save_msm_obs(rtcm,sys,&h,r,pr,cp,NULL,NULL,cnr,lock,NULL,half); + save_msm_obs(rtcm,sys,&h,r,pr,cp,NULL,NULL,cnr,lti,10,NULL,half); rtcm->obsflag=!sync; return sync?0:1; @@ -2375,7 +2544,7 @@ static int decode_msm7(rtcm_t *rtcm, int sys) { msm_h_t h={0}; double r[64],rr[64],pr[64],cp[64],rrf[64],cnr[64]; - int i,j,type,sync,iod,ncell,rng,rng_m,rate,prv,cpv,rrv,lock[64]; + int i,j,type,sync,iod,ncell,rng,rng_m,rate,prv,cpv,rrv,lti[64]; int ex[64],half[64]; type=getbitu(rtcm->buff,24,12); @@ -2422,10 +2591,10 @@ static int decode_msm7(rtcm_t *rtcm, int sys) cpv=getbits(rtcm->buff,i,24); i+=24; if (cpv!=-8388608) cp[j]=cpv*P2_31*RANGE_MS; } - for (j=0;jbuff,i,10); i+=10; + for (j=0;jbuff,i,10); i+=10; } - for (j=0;jbuff,i,1); i+=1; } for (j=0;jobsflag=!sync; return sync?0:1; diff --git a/src/rtklib.h b/src/rtklib.h index ed6e3bee3..e6bff6618 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -955,8 +955,10 @@ typedef struct { /* RTCM control struct type */ int ephsat; /* input ephemeris satellite number */ int ephset; /* input ephemeris set (0-1) */ double cp[MAXSAT][MAXCODE]; /* carrier-phase measurement */ - uint16_t lock[MAXSAT][MAXCODE]; /* lock time */ + uint16_t lti[MAXSAT][MAXCODE]; /* lock time indicator */ uint16_t loss[MAXSAT][MAXCODE]; /* loss of lock count */ + gtime_t tobs[MAXSAT][MAXCODE]; /* Observation data time */ + gtime_t stime; /* Time of first epoch, for slip detection */ gtime_t lltime[MAXSAT][MAXCODE]; /* last lock time */ int nbyte; /* number of bytes in message buffer */ int nbit; /* number of bits in word buffer */