/* 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();
}
