/* update navigation data ----------------------------------------------------*/ static void updatenav(nav_t *nav) { int i,j; for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) { nav->lam[i][j]=satwavelen(i+1,j,nav); } }
/* decode binex mesaage 0x7f-05: trimble netr8 obs data ----------------------*/ static unsigned char *decode_bnx_7f_05_obs(raw_t *raw, unsigned char *buff, int sat, int nobs, obsd_t *data) { const unsigned char codes_gps[32]={ CODE_L1C ,CODE_L1C ,CODE_L1P ,CODE_L1W ,CODE_L1Y ,CODE_L1M , /* 0- 5 */ CODE_L1X ,CODE_L1N ,CODE_NONE,CODE_NONE,CODE_L2W ,CODE_L2C , /* 6-11 */ CODE_L2D ,CODE_L2S ,CODE_L2L ,CODE_L2X ,CODE_L2P ,CODE_L2W , /* 12-17 */ CODE_L2Y ,CODE_L2M ,CODE_L2N ,CODE_NONE,CODE_NONE,CODE_L5X , /* 18-23 */ CODE_L5I ,CODE_L5Q ,CODE_L5X /* 24-26 */ }; const unsigned char codes_glo[32]={ CODE_L1C ,CODE_L1C ,CODE_L1P ,CODE_NONE,CODE_NONE,CODE_NONE, /* 0- 5 */ CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE,CODE_L2C ,CODE_L2C , /* 6-11 */ CODE_L2P ,CODE_L3X ,CODE_L3I ,CODE_L3Q ,CODE_L3X /* 12-16 */ }; const unsigned char codes_gal[32]={ CODE_L1C ,CODE_L1A ,CODE_L1B ,CODE_L1C ,CODE_L1X ,CODE_L1Z , /* 0- 5 */ CODE_L5X ,CODE_L5I ,CODE_L5Q ,CODE_L5X ,CODE_L7X ,CODE_L7I , /* 6-11 */ CODE_L7Q ,CODE_L7X ,CODE_L8X ,CODE_L8I ,CODE_L8Q ,CODE_L8X , /* 12-17 */ CODE_L6X ,CODE_L6A ,CODE_L6B ,CODE_L6C ,CODE_L6X ,CODE_L6Z , /* 18-23 */ }; const unsigned char codes_sbs[32]={ CODE_L1C ,CODE_L1C ,CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE, /* 0- 5 */ CODE_L5X ,CODE_L5I ,CODE_L5Q ,CODE_L5X /* 6- 9 */ }; const unsigned char codes_cmp[32]={ CODE_L2X ,CODE_L2I ,CODE_L2Q ,CODE_L2X ,CODE_L7X ,CODE_L7I , /* 0- 5 */ CODE_L7Q ,CODE_L7X ,CODE_L6X ,CODE_L6I ,CODE_L6Q ,CODE_L6X , /* 6-11 */ CODE_L1X ,CODE_L1S ,CODE_L1L ,CODE_L1X /* 12-15 */ }; const unsigned char codes_qzs[32]={ CODE_L1C ,CODE_L1C ,CODE_L1S ,CODE_L1L ,CODE_L1X ,CODE_NONE, /* 0- 5 */ CODE_NONE,CODE_L2X ,CODE_L2S ,CODE_L2L ,CODE_L2X ,CODE_NONE, /* 6-11 */ CODE_NONE,CODE_L5X ,CODE_L5I ,CODE_L5Q ,CODE_L5X ,CODE_NONE, /* 12-17 */ CODE_NONE,CODE_L6X ,CODE_L6S ,CODE_L6L ,CODE_L6X ,CODE_NONE, /* 18-23 */ CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE, /* 24-29 */ CODE_L1Z /* 30-30 */ }; const unsigned char *codes=NULL; double range[8],phase[8],cnr[8],dopp[8]={0},acc,wl; unsigned char *p=buff; unsigned char flag,flag0,flag1,flag2,flag3; int i,j,k,sys,fcn=-10,code[8],slip[8],pri[8],freq[8],slipcnt[8]={0},mask[8]={0}; trace(5,"decode_bnx_7f_05_obs: sat=%2d nobs=%2d\n",sat,nobs); sys=satsys(sat,NULL); switch (sys) { case SYS_GPS: codes=codes_gps; break; case SYS_GLO: codes=codes_glo; break; case SYS_GAL: codes=codes_gal; break; case SYS_QZS: codes=codes_qzs; break; case SYS_SBS: codes=codes_sbs; break; case SYS_CMP: codes=codes_cmp; break; } for (i=0;i<nobs;i++) { flag =getbitu(p,0,1); slip[i]=getbitu(p,2,1); code[i]=getbitu(p,3,5); p++; flag0=flag1=flag2=flag3=0; if (flag) flag0=U1(p++); if (flag0&0x80) flag1=U1(p++); if (flag1&0x80) flag2=U1(p++); if (flag2&0x80) flag3=U1(p++); if (flag1&0x80) fcn=getbits(&flag2,2,4); acc=(flag0&0x20)?0.0001:0.00002; /* phase accuracy */ cnr[i]=U1(p++)*0.4; if (i==0) { cnr[i]+=getbits(p,0,2)*0.1; range[i]=getbitu(p,2,32)*0.064+getbitu(p,34,6)*0.001; p+=5; } else if (flag0&0x40) { cnr[i]+=getbits(p,0,2)*0.1; range[i]=range[0]+getbits(p,4,20)*0.001; p+=3; } else { range[i]=range[0]+getbits(p,0,16)*0.001; p+=2; } if (flag0&0x40) { phase[i]=range[i]+getbits(p,0,24)*acc; p+=3; } else { cnr[i]+=getbits(p,0,2)*0.1; phase[i]=range[i]+getbits(p,2,22)*acc; p+=3; } if (flag0&0x04) { dopp[i]=getbits(p,0,24)/256.0; p+=3; } if (flag0&0x18) { slipcnt[i]=U2(p); p+=2; } else if (flag0&0x08) { slipcnt[i]=U1(p); p+=1; } trace(5,"(%d) CODE=%2d S=%d F=%02X %02X %02X %02X\n",i+1, code[i],slip,flag0,flag1,flag2,flag3); trace(5,"(%d) P=%13.3f L=%13.3f D=%7.1f SNR=%4.1f SCNT=%2d\n", i+1,range[i],phase[i],dopp[i],cnr[i],slipcnt[i]); } if (!codes) { data->sat=0; return p; } data->time=raw->time; data->sat=sat; /* get code priority */ for (i=0;i<nobs;i++) { code2obs(codes[code[i]&0x3F],freq+i); pri[i]=getcodepri(sys,codes[code[i]&0x3F],raw->opt); } for (i=0;i<NFREQ;i++) { for (j=0,k=-1;j<nobs;j++) { if (freq[j]==i+1&&(k<0||pri[j]>pri[k])) k=j; } if (k<0) { data->P[i]=data->L[i]=0.0; data->D[i]=0.0f; data->SNR[i]=data->LLI[i]=0; data->code[i]=CODE_NONE; } else { wl=satwavelen(sat,i,&raw->nav); if (sys==SYS_GLO&&fcn>=-7&&freq[k]<=2) { wl=CLIGHT/(freq[k]==1?FREQ1_GLO+DFRQ1_GLO*fcn: FREQ2_GLO+DFRQ2_GLO*fcn); } data->P[i]=range[k]; data->L[i]=wl<=0.0?0.0:phase[k]/wl; data->D[i]=dopp[k]; data->SNR[i]=(unsigned char)(cnr[k]/0.25+0.5); data->code[i]=codes[code[k]&0x3F]; data->LLI[i]=slip[k]?1:0; mask[k]=1; } } for (;i<NFREQ+NEXOBS;i++) { for (k=0;k<nobs;k++) { if (!mask[k]) break; } if (k>=nobs) { data->P[i]=data->L[i]=0.0; data->D[i]=0.0f; data->SNR[i]=data->LLI[i]=0; data->code[i]=CODE_NONE; } else { wl=satwavelen(sat,freq[k]-1,&raw->nav); if (sys==SYS_GLO&&fcn>=-7&&freq[k]<=2) { wl=CLIGHT/(freq[k]==1?FREQ1_GLO+DFRQ1_GLO*fcn: FREQ2_GLO+DFRQ2_GLO*fcn); } data->P[i]=range[k]; data->L[i]=wl<=0.0?0.0:phase[k]/wl; data->D[i]=dopp[k]; data->SNR[i]=(unsigned char)(cnr[k]/0.25+0.5); data->code[i]=codes[code[k]&0x3F]; data->LLI[i]=slip[k]?1:0; mask[k]=1; } } return p; }
// update Multipath ------------------------------------------------------------ void __fastcall TPlot::UpdateMp(void) { AnsiString s; obsd_t *data; double lam1,lam2,I,C,B; int i,j,k,f1,f2,sat,sys,per,per_=-1,n; trace(3,"UpdateMp\n"); for (i=0;i<NFREQ+NEXOBS;i++) { delete [] Mp[i]; Mp[i]=NULL; } if (Obs.n<=0) return; for (i=0;i<NFREQ+NEXOBS;i++) { Mp[i]=new double[Obs.n]; } ReadWaitStart(); ShowLegend(NULL); for (i=0;i<Obs.n;i++) { data=Obs.data+i; sys=satsys(data->sat,NULL); for (j=0;j<NFREQ+NEXOBS;j++) { Mp[j][i]=0.0; code2obs(data->code[j],&f1); if (sys==SYS_CMP) { if (f1==5) f1=2; /* B2 */ else if (f1==4) f1=3; /* B3 */ } if (sys==SYS_GAL) f2=f1==1?3:1; /* E1/E5a */ else if (sys==SYS_SBS) f2=f1==1?3:1; /* L1/L5 */ else if (sys==SYS_CMP) f2=f1==1?2:1; /* B1/B2 */ else f2=f1==1?2:1; /* L1/L2 */ lam1=satwavelen(data->sat,f1-1,&Nav); lam2=satwavelen(data->sat,f2-1,&Nav); if (lam1==0.0||lam2==0.0) continue; if (data->P[j]!=0.0&&data->L[j]!=0.0&&data->L[f2-1]) { C=SQR(lam1)/(SQR(lam1)-SQR(lam2)); I=lam1*data->L[j]-lam2*data->L[f2-1]; Mp[j][i]=data->P[j]-lam1*data->L[j]+2.0*C*I; } } } for (sat=1;sat<=MAXSAT;sat++) for (i=0;i<NFREQ+NEXOBS;i++) { sys=satsys(sat,NULL); for (j=k=n=0,B=0.0;j<Obs.n;j++) { if (Obs.data[j].sat!=sat) continue; code2obs(Obs.data[j].code[i],&f1); if (sys==SYS_CMP) { if (f1==5) f1=2; /* B2 */ else if (f1==4) f1=3; /* B3 */ } if (sys==SYS_GAL) f2=f1==1?3:1; else if (sys==SYS_CMP) f2=f1==1?2:1; else f2=f1==1?2:1; if ((Obs.data[j].LLI[i]&1)||(Obs.data[j].LLI[f2-1]&1)|| fabs(Mp[i][j]-B)>THRES_SLIP) { for (;k<j;k++) if (Obs.data[k].sat==sat) Mp[i][k]-=B; B=Mp[i][j]; n=1; k=j; } else { if (n==0) k=j; B+=(Mp[i][j]-B)/++n; } } if (n>0) { for (;k<j;k++) if (Obs.data[k].sat==sat) Mp[i][k]-=B; } per=sat*100/MAXSAT; if (per!=per_) { ShowMsg(s.sprintf("updating multipath... (%d%%)",(per_=per))); Application->ProcessMessages(); } } ReadWaitEnd(); }