//--------------------------------------------------------------------------- void __fastcall TOptDialog::SetPos(int type, TEdit **edit, double *pos) { AnsiString s; double p[3],dms1[3],dms2[3],s1,s2; if (type==1) { /* lat/lon/height dms/m */ ecef2pos(pos,p); s1=p[0]<0?-1:1; s2=p[1]<0?-1:1; p[0]=fabs(p[0])*R2D+1E-12; p[1]=fabs(p[1])*R2D+1E-12; dms1[0]=floor(p[0]); p[0]=(p[0]-dms1[0])*60.0; dms1[1]=floor(p[0]); dms1[2]=(p[0]-dms1[1])*60.0; dms2[0]=floor(p[1]); p[1]=(p[1]-dms2[0])*60.0; dms2[1]=floor(p[1]); dms2[2]=(p[1]-dms2[1])*60.0; edit[0]->Text=s.sprintf("%.0f %02.0f %09.6f",s1*dms1[0],dms1[1],dms1[2]); edit[1]->Text=s.sprintf("%.0f %02.0f %09.6f",s2*dms2[0],dms2[1],dms2[2]); edit[2]->Text=s.sprintf("%.4f",p[2]); } else if (type==2) { /* x/y/z-ecef */ edit[0]->Text=s.sprintf("%.4f",pos[0]); edit[1]->Text=s.sprintf("%.4f",pos[1]); edit[2]->Text=s.sprintf("%.4f",pos[2]); } else { ecef2pos(pos,p); edit[0]->Text=s.sprintf("%.9f",p[0]*R2D); edit[1]->Text=s.sprintf("%.9f",p[1]*R2D); edit[2]->Text=s.sprintf("%.4f",p[2]); } }
// update information for current-cursor position --------------------------- void __fastcall TPlot::UpdatePoint(int x, int y) { gtime_t time; TPoint p(x,y); double enu[3]={0},rr[3],pos[3],xx,yy,r,xl[2],yl[2],q[2],az,el,snr; int i; char tstr[64]; AnsiString msg; trace(4,"UpdatePoint: x=%d y=%d\n",x,y); if (PlotType==PLOT_TRK) { // track-plot if (norm(OPos,3)>0.0) { GraphT->ToPos(p,enu[0],enu[1]); ecef2pos(OPos,pos); enu2ecef(pos,enu,rr); for (i=0;i<3;i++) rr[i]+=OPos[i]; ecef2pos(rr,pos); msg=LatLonStr(pos,8); } } else if (PlotType==PLOT_SKY||PlotType==PLOT_MPS) { // sky-plot GraphS->GetLim(xl,yl); GraphS->ToPos(p,q[0],q[1]); r=(xl[1]-xl[0]<yl[1]-yl[0]?xl[1]-xl[0]:yl[1]-yl[0])*0.45; if ((el=90.0-90.0*norm(q,2)/r)>0.0) { az=el>=90.0?0.0:ATAN2(q[0],q[1])*R2D; if (az<0.0) az+=360.0; msg.sprintf("AZ=%5.1f" CHARDEG " EL=%4.1f" CHARDEG,az,el); } } else if (PlotType==PLOT_SNRE) { // snr-el-plot GraphE[0]->ToPos(p,q[0],q[1]); msg.sprintf("EL=%4.1f " CHARDEG,q[0]); } else { GraphG[0]->ToPos(p,xx,yy); time=gpst2time(Week,xx); if (TimeLabel==2) time=utc2gpst(time); // UTC else if (TimeLabel==3) time=timeadd(gpst2utc(time),-9*3600.0); // JST TimeStr(time,0,1,tstr); msg=tstr; } Panel22->Visible=true; Message2->Caption=A2U(msg); }
// transform covariance to xyz-terms ---------------------------------------- void __fastcall TPlot::CovToXyz(const double *rr, const float *qr, int type, double *xyzs) { double pos[3],P[9],Q[9]; trace(4,"CovToXyz:\n"); if (type==0) { // xyz ecef2pos(rr,pos); P[0]=qr[0]; P[4]=qr[1]; P[8]=qr[2]; P[1]=P[3]=qr[3]; P[5]=P[7]=qr[4]; P[2]=P[6]=qr[5]; covenu(pos,P,Q); xyzs[0]=Q[0]; xyzs[1]=Q[4]; xyzs[2]=Q[8]; xyzs[3]=Q[1]; } else { // enu xyzs[0]=qr[0]; xyzs[1]=qr[1]; xyzs[2]=qr[2]; xyzs[3]=qr[3]; } }
// transform solution to xyz-terms ------------------------------------------ void __fastcall TPlot::PosToXyz(gtime_t time, const double *rr, int type, double *xyz) { double opos[3],pos[3],r[3],enu[3]; int i; trace(4,"SolToXyz:\n"); if (type==0) { // xyz for (i=0;i<3;i++) { opos[i]=OPos[i]; if (time.time==0.0||OEpoch.time==0.0) continue; opos[i]+=OVel[i]*timediff(time,OEpoch); } for (i=0;i<3;i++) r[i]=rr[i]-opos[i]; ecef2pos(opos,pos); ecef2enu(pos,r,enu); xyz[0]=enu[0]; xyz[1]=enu[1]; xyz[2]=enu[2]; } else { // enu xyz[0]=rr[0]; xyz[1]=rr[1]; xyz[2]=rr[2]; } }
/* estimate ionosphere -------------------------------------------------------*/ static int est_iono(obs_t *obs, nav_t *nav, double *rr, FILE *fp) { ssat_t ssat[MAXSAT]={{0}}; double tt,*x,*P,*v,*H,*R,pos[3],azel[MAXOBS*2]; int i,n,info,nx=NX,nv=MAXSAT*2; x=zeros(nx,1); P=zeros(nx,nx); v=mat(nv,1); H=mat(nx,nv); R=mat(nv,nv); /* receiver position */ ecef2pos(rr,pos); for (i=0;i<obs->n;i++) { for (n=1;i+n<obs->n;n++) { if (timediff(obs[i].time,obs->data[i+n].time)>1E-3) break; } /* satellite azimuth/elevation angle */ sat_azel(obs+i,n,nav,pos,azel); /* time update of parameters */ ud_state(obs+i,n,azel,x,P,nx,ssat); /* ionosphere residuals */ if ((nv=res_iono(obs+i,n,azel,x,nx,v,H,R))<=0) break; /* filter */ if ((info=rtklib_filter(x,P,H,v,R,nx,nv))) break; /* output ionopshere parameters */ out_iono(obs[i].time,x,P,nx,fp); } free(x); free(P); free(v); free(H); free(R); return 1; }
//--------------------------------------------------------------------------- TPoint __fastcall TMapDialog::PosToPoint(const double *pos) { double sc[]={ 0.01,0.02,0.05,0.1,0.2,0.5,1,2,5,10,20,50,100,200,500,1000,2000,5000,10000 }; double rr[3],posr[3],enu[3],fact=40.0/sc[Scale]; TPoint p; ecef2pos(RefPos,posr); for (int i=0;i<3;i++) rr[i]=pos[i]-RefPos[i]; ecef2enu(posr,rr,enu); p.x=Plot->Width /2+(int)(enu[0]*fact+0.5); p.y=Plot->Height/2-(int)(enu[1]*fact+0.5); if (BtnCenter->Down) { for (int i=0;i<3;i++) rr[i]=CurrentPos[i]-RefPos[i]; ecef2enu(posr,rr,enu); p.x-=(int)(enu[0]*fact+0.5); p.y+=(int)(enu[1]*fact+0.5); } else if (norm(CentPos,3)>0.0) { for (int i=0;i<3;i++) rr[i]=CentPos[i]-RefPos[i]; ecef2enu(posr,rr,enu); p.x-=(int)(enu[0]*fact+0.5); p.y+=(int)(enu[1]*fact+0.5); } return p; }
/* static int zdres(int base, const obsd_t *obs, int n, const double *rs, const double *dts, const int *svh, const nav_t *nav, const double *rr, const prcopt_t *opt, double *y, double *e, double *azel) Description: zero differenced residual Params: base I 1:base, 0: rover obs I n I number of obs data rs I sat positions/velocities dts I sat clock bias svh I nav I rr I receiver position y O zero diff residuals e O azel O {az/el} Return: 0 ok, !0 error */ static int zdres(int base, const obsd_t *obs, int n, const double *rs, const double *dts, const int *svh, const nav_t *nav, const double *rr, const prcopt_t *opt, int index, double *y, double *e, double *azel) { double r,rr_[3],pos[3]; double zhd, zazel[]= {0.0,90.0*D2R}; //tai sao init zazel = {0,90} int i; for (i=0; i<2*n; i++) y[i]=0.0; if (sos3(rr)<=0.0) return -1;//no receiver pos for (i=0; i<3; i++) rr_[i]=rr[i]; ecef2pos(rr_,pos); for (i=0; i<n; i++) { if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue; if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue; //satellite clock-bias r -= CLIGHT*dts[i*2]; /* troposphere delay model (hydrostatic) */ zhd=tropmodel(obs[0].time,pos,zazel,0.0);//tai sao chon rel_humi = 0??????????? r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd; /* undifferenced phase/code residual for satellite */ zdres_sat(base,r,obs+i,nav,azel+i*2,opt,y+i*2); } return 0; }
/* temporal update of tropospheric parameters --------------------------------*/ static void udtrop_ppp(rtk_t *rtk) { double pos[3],azel[]={0.0,PI/2.0},ztd,var; int i=IT(&rtk->opt),j; trace(3,"udtrop_ppp:\n"); if (rtk->x[i]==0.0) { ecef2pos(rtk->sol.rr,pos); ztd=sbstropcorr(rtk->sol.time,pos,azel,&var); initx(rtk,ztd,var,i); if (rtk->opt.tropopt>=TROPOPT_ESTG) { for (j=0;j<2;j++) initx(rtk,1E-6,VAR_GRA,++i); } } else { rtk->P[i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2])*fabs(rtk->tt); if (rtk->opt.tropopt>=TROPOPT_ESTG) { for (j=0;j<2;j++) { rtk->P[++i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2]*0.1)*fabs(rtk->tt); } } } }
/* output reference position -------------------------------------------------*/ static void outrpos(FILE *fp, const double *r, const solopt_t *opt) { double pos[3],dms1[3],dms2[3]; const char *sep=opt->sep; trace(3,"outrpos :\n"); if (opt->posf==SOLF_LLH||opt->posf==SOLF_ENU) { ecef2pos(r,pos); if (opt->degf) { deg2dms(pos[0]*R2D,dms1); deg2dms(pos[1]*R2D,dms2); fprintf(fp,"%3.0f%s%02.0f%s%08.5f%s%4.0f%s%02.0f%s%08.5f%s%10.4f", dms1[0],sep,dms1[1],sep,dms1[2],sep,dms2[0],sep,dms2[1], sep,dms2[2],sep,pos[2]); } else { fprintf(fp,"%13.9f%s%14.9f%s%10.4f",pos[0]*R2D,sep,pos[1]*R2D, sep,pos[2]); } } else if (opt->posf==SOLF_XYZ) { fprintf(fp,"%14.4f%s%14.4f%s%14.4f",r[0],sep,r[1],sep,r[2]); } }
//--------------------------------------------------------------------------- void __fastcall TPntDialog::FormShow(TObject *Sender) { TGridRect r={0}; AnsiString s; double pos[3]; int width[]={90,90,80,90}; FontScale=Screen->PixelsPerInch; for (int i=0;i<4;i++) { PntList->ColWidths[i]=width[i]*FontScale/96; } PntList->DefaultRowHeight=17*FontScale/96; for (int i=0;i<PntList->RowCount;i++) { if (i<Plot->NWayPnt) { ecef2pos(Plot->PntPos[i],pos); PntList->Cells[0][i]=s.sprintf("%.9f",pos[0]*R2D); PntList->Cells[1][i]=s.sprintf("%.9f",pos[1]*R2D); PntList->Cells[2][i]=s.sprintf("%.4f",pos[2]); PntList->Cells[3][i]=Plot->PntName[i]; } else { for (int j=0;j<PntList->ColCount;j++) PntList->Cells[j][i]=""; } } r.Top=r.Bottom=PntList->RowCount; PntList->Selection=r; }
/* output solution status for PPP --------------------------------------------*/ extern void pppoutsolstat(rtk_t *rtk, int level, FILE *fp) { ssat_t *ssat; double tow,pos[3],vel[3],acc[3]; int i,j,week,nfreq=1; char id[32]; if (level<=0||!fp) return; trace(3,"pppoutsolstat:\n"); tow=time2gpst(rtk->sol.time,&week); /* receiver position */ fprintf(fp,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],0.0,0.0,0.0); /* receiver velocity and acceleration */ if (rtk->opt.dynamics) { ecef2pos(rtk->sol.rr,pos); ecef2enu(pos,rtk->x+3,vel); ecef2enu(pos,rtk->x+6,acc); fprintf(fp,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2], 0.0,0.0,0.0,0.0,0.0,0.0); } /* receiver clocks */ i=IC(0,&rtk->opt); fprintf(fp,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", week,tow,rtk->sol.stat,1,rtk->x[i]*1E9/CLIGHT,rtk->x[i+1]*1E9/CLIGHT, 0.0,0.0); /* tropospheric parameters */ if (rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG) { i=IT(&rtk->opt); fprintf(fp,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, 1,rtk->x[i],0.0); } if (rtk->opt.tropopt==TROPOPT_ESTG) { i=IT(&rtk->opt); fprintf(fp,"$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n",week,tow, rtk->sol.stat,1,rtk->x[i+1],rtk->x[i+2],0.0,0.0); } if (rtk->sol.stat==SOLQ_NONE||level<=1) return; /* residuals and status */ for (i=0;i<MAXSAT;i++) { ssat=rtk->ssat+i; if (!ssat->vs) continue; satno2id(i+1,id); for (j=0;j<nfreq;j++) { fprintf(fp,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n", week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D, ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr[j]*0.25, ssat->fix[j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j], ssat->slipc[j],ssat->rejc[j]); } } }
/* ecef2pos() */ void utest1(void) { double r1[]={ 0.0, 0.0, 0.0}; double r2[]={10000000.0, 0.0, 0.0}; double r3[]={ 0.0,10000000.0, 0.0}; double r4[]={ 0.0, 0.0, 10000000.0}; double r5[]={ 0.0, 0.0,-10000000.0}; double r6[]={-3.5173197701E+06,4.1316679161E+06, 3.3412651227E+06}; double r7[]={-3.5173197701E+06,4.1316679161E+06,-3.3412651227E+06}; double pos[3]; ecef2pos(r1,pos); assert(pos[2]<0.0); ecef2pos(r2,pos); assert(pos[0]==0&&pos[1]==0&&pos[2]>0.0); ecef2pos(r3,pos); assert(pos[0]==0&&fabs(pos[1]-PI/2)<1E-6&&pos[2]>0.0); ecef2pos(r4,pos); assert(fabs(pos[0]-PI/2)<1E-6&&pos[2]>0.0); ecef2pos(r5,pos); assert(fabs(pos[0]+PI/2)<1E-6&&pos[2]>0.0); ecef2pos(r6,pos); assert(fabs(pos[0]*R2D-3.1796021375E+01)<1E-7&& fabs(pos[1]*R2D-1.3040799917E+02)<1E-7&& fabs(pos[2]-6.8863206206E+01)<1E-4); ecef2pos(r7,pos); assert(fabs(pos[0]*R2D+3.1796021375E+01)<1E-7&& fabs(pos[1]*R2D-1.3040799917E+02)<1E-7&& fabs(pos[2]-6.8863206206E+01)<1E-4); printf("%s utset1 : OK\n",__FILE__); }
static void RtkCommon__ecef2pos(JNIEnv* env, jclass clazz, jdouble x, jdouble y, jdouble z, jdoubleArray j_pos) { double r[3] = {x, y, z}; double pos[3]; ecef2pos(r, pos); (*env)->SetDoubleArrayRegion(env, j_pos, 0, 3, pos); }
//--------------------------------------------------------------------------- void __fastcall TMapAreaDialog::BtnCenterClick(TObject *Sender) { AnsiString s; double rr[3],pos[3]; if (!Plot->GetCenterPos(rr)) return; ecef2pos(rr,pos); Lat->Text=s.sprintf("%.7f",pos[0]*R2D); Lon->Text=s.sprintf("%.7f",pos[1]*R2D); UpdateMap(); }
static void possend(hcnraw_t *hcnraw){ int i; double r[3],e[3]; posstate_t posstate = { 0 }; char sys[] = { 0x01,0x04,0x20,0x20,0x01,0x04,0x20,0x01,0x04, 0x08,0x02,0x02,0x08,0x08 }; if(hcnraw->satpro.solsatnum <= 0) return; r[0]=hcnraw->satpro.x; r[1]=hcnraw->satpro.y; r[2]=hcnraw->satpro.z; ecef2pos(r,e); posstate.lat = e[0]*R2D; posstate.lon = e[1]*R2D; posstate.hgt = e[2]; posstate.postype = hcnraw->satpro.pvtstatus; posstate.difftime= hcnraw->satpro.age; for(i = 0;i < 4;i++) posstate.stnid[i] = hcnraw->satpro.stnid[i]; posstate.gpssec = (float)time2gpst(hcnraw->time,&posstate.gpsweek); posstate.pdop = hcnraw->satpro.pdop; posstate.hdop = hcnraw->satpro.hdop; posstate.vdop = hcnraw->satpro.vdop; posstate.solsatnu= hcnraw->satpro.solsatnum; for(i = 0;i < 5;i++) posstate.sateprn.satnu[i] = 0; for(i = 0;i < hcnraw->satpro.satnum;i++){ if(hcnraw->satpro.satprosat[i].satstatus != 0) continue; switch(sys[hcnraw->satpro.satprosat[i].sigtype]){ case SYS_GPS: posstate.sateprn.gps[posstate.sateprn.satnu[0]++]= hcnraw->satpro.satprosat[i].svid; break; case SYS_GLO: posstate.sateprn.glo[posstate.sateprn.satnu[1]++]= hcnraw->satpro.satprosat[i].svid; break; case SYS_GAL:posstate.sateprn.gali[posstate.sateprn.satnu[2]++]= hcnraw->satpro.satprosat[i].svid; break; case SYS_CMP: posstate.sateprn.cmp[posstate.sateprn.satnu[3]++]= hcnraw->satpro.satprosat[i].svid; break; case SYS_SBS: posstate.sateprn.sbas[posstate.sateprn.satnu[4]++]= hcnraw->satpro.satprosat[i].svid; break; } } pthread_mutex_lock(&lock); cmd_response_massage( sock,&msg_head[2],&posstate,sizeof(posstate_t)); pthread_mutex_unlock(&lock); }
/* estimate ionosphere -------------------------------------------------------*/ static int est_iono(obs_t *obs, nav_t *nav, const pcv_t *pcv, double *rr, double tint, FILE *fp) { sstat_t sstat[MAXSAT]={{{0}}}; ekf_t *ekf; gtime_t time; double pos[3],rs[MAXOBS*6],dts[MAXOBS*2],var[MAXOBS],e[3],azel[MAXOBS*2]; double *v,*H,*R,phw[MAXSAT]={0}; int i,j,n=0,info,nx=NX,nv=MAXSAT*2,svh[MAXOBS]; ekf=ekf_new(NX); v=mat(nv,1); H=mat(nx,nv); R=mat(nv,nv); /* receiver position */ ecef2pos(rr,pos); out_head(obs->data[0].time,pos,fp); for (i=0;i<obs->n;i+=n) { for (n=1;i+n<obs->n;n++) { if (timediff(obs->data[i+n].time,obs->data[i].time)>1E-3) break; } time=obs->data[i].time; /* satellite positions and clocks */ satposs(time,obs->data+i,n,nav,EPHOPT_BRDC,rs,dts,var,svh); /* satellite azimuth/elevation angle */ for (j=0;j<n;j++) { if (geodist(rs+j*6,rr,e)>0.0) satazel(pos,e,azel+j*2); else azel[j*2]=azel[1+j*2]=0.0; } /* time update of parameters */ ud_state(obs->data+i,n,nav,pos,azel,ekf,sstat); /* ionosphere residuals */ if ((nv=res_iono(obs->data+i,n,nav,rs,rr,pos,azel,pcv,ekf,phw,v,H,R))<=0) { continue; } /* filter */ if ((info=filter(ekf->x,ekf->P,H,v,R,ekf->nx,nv))) { fprintf(stderr,"filter error: info=%d\n",info); break; } /* output ionopshere parameters */ if (tint<=0.0||fmod(time2gpst(time,NULL)+0.005,tint)<0.01) { out_iono(obs->data[i].time,ekf,sstat,fp); } } ekf_free(ekf); free(v); free(H); free(R); return 1; }
/* set rtcm antenna and receiver info to options -----------------------------*/ static void rtcm2opt(const rtcm_t *rtcm, rnxopt_t *opt) { double pos[3],enu[3]; int i; trace(3,"rtcm2opt:\n"); /* comment */ sprintf(opt->comment[1]+strlen(opt->comment[1]),", station ID: %d", rtcm->staid); /* receiver and antenna info */ if (!*opt->rec[0]&&!*opt->rec[1]&&!*opt->rec[2]) { strcpy(opt->rec[0],rtcm->sta.recsno); strcpy(opt->rec[1],rtcm->sta.rectype); strcpy(opt->rec[2],rtcm->sta.recver); } if (!*opt->ant[0]&&!*opt->ant[1]&&!*opt->ant[2]) { strcpy(opt->ant[0],rtcm->sta.antsno); strcpy(opt->ant[1],rtcm->sta.antdes); if (rtcm->sta.antsetup) { sprintf(opt->ant[2],"%d",rtcm->sta.antsetup); } else *opt->ant[2]='\0'; } /* antenna approx position */ if (!opt->autopos&&norm(rtcm->sta.pos,3)>0.0) { for (i=0;i<3;i++) opt->apppos[i]=rtcm->sta.pos[i]; } /* antenna delta */ if (norm(rtcm->sta.del,3)>0.0) { if (!rtcm->sta.deltype&&norm(rtcm->sta.del,3)>0.0) { /* enu */ opt->antdel[0]=rtcm->sta.del[2]; /* h */ opt->antdel[1]=rtcm->sta.del[0]; /* e */ opt->antdel[2]=rtcm->sta.del[1]; /* n */ } else if (norm(rtcm->sta.pos,3)>0.0) { /* xyz */ ecef2pos(rtcm->sta.pos,pos); ecef2enu(pos,rtcm->sta.del,enu); opt->antdel[0]=enu[2]; /* h */ opt->antdel[1]=enu[0]; /* e */ opt->antdel[2]=enu[1]; /* n */ } } else { opt->antdel[0]=rtcm->sta.hgt; opt->antdel[0]=0.0; opt->antdel[0]=0.0; } }
/* doppler residuals ---------------------------------------------------------*/ static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts, const nav_t *nav, const double *rr, const double *x, const double *azel, const int *vsat, double *v, double *H) { double lam,rate,pos[3],E[9],a[3],e[3],vs[3],cosel; int i,j,nv=0; trace(3,"resdop : n=%d\n",n); ecef2pos(rr,pos); xyz2enu(pos,E); fprintf(output, " Begin doppler residuals calculation\n"); for (i=0;i<n&&i<MAXOBS;i++) { fprintf(output, " Satellite %i:\n", obs[i].sat); lam=nav->lam[obs[i].sat-1][0]; if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) { continue; } /* line-of-sight vector in ecef */ cosel=cos(azel[1+i*2]); a[0]=sin(azel[i*2])*cosel; a[1]=cos(azel[i*2])*cosel; a[2]=sin(azel[1+i*2]); matmul("TN",3,1,3,1.0,E,a,0.0,e); /* satellite velocity relative to receiver in ecef */ for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j]; /* range rate with earth rotation correction */ rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]- rs[3+i*6]*rr[1]-rs[ i*6]*x[1]); fprintf(output, " rate = %f\n", rate); /* doppler residual */ v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]); fprintf(output, " Residual = %f", v[nv]); /* design matrix */ for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0; nv++; } fprintf(output, " End residuals calculation, total %i residuals\n", nv); return nv; }
// update time-information for solution plot -------------------------------- void __fastcall TPlot::UpdateTimeSol(void) { const char *unit[]={"m","m/s","m/s2"},*u; const char *sol[]={"","FIX","FLOAT","SBAS","DGPS","Single","PPP"}; AnsiString msg,msgs[8],s; sol_t *data; double xyz[3],pos[3],r,az,el; int sel=BtnSol1->Down||!BtnSol2->Down?0:1,ind=SolIndex[sel]; char tstr[64]; trace(3,"UpdateTimeSol\n"); if ((BtnSol1->Down||BtnSol2->Down||BtnSol12->Down)&& (data=getsol(SolData+sel,ind))) { if (!ConnectState) msg.sprintf("[%d]",sel+1); else msg="[R]"; TimeStr(data->time,2,1,tstr); msg+=tstr; msg+=" : "; if (PLOT_SOLP<=PlotType&&PlotType<=PLOT_SOLA) { PosToXyz(data->time,data->rr,data->type,xyz); u=unit[PlotType-PLOT_SOLP]; msg+=s.sprintf("E=%7.4f%s N=%7.4f%s U=%7.4f%s Q=", xyz[0],u,xyz[1],u,xyz[2],u); } else if (PlotType==PLOT_NSAT) { msg+=s.sprintf("NS=%d AGE=%.1f RATIO=%.1f Q=",data->ns,data->age, data->ratio); } else if (!data->type) { ecef2pos(data->rr,pos); msg+=LatLonStr(pos,9)+s.sprintf(" %9.4fm Q=",pos[2]); } else { r=norm(data->rr,3); az=norm(data->rr,2)<=1E-12?0.0:atan2(data->rr[0],data->rr[1])*R2D; el=r<=1E-12?0.0:asin(data->rr[2]/r)*R2D; msg+=s.sprintf("B=%.3fm D=%6.2f" CHARDEG " %5.2f" CHARDEG " Q=", r,az<0.0?az+360.0:az,el); } if (1<=data->stat&&data->stat<=6) { msgs[data->stat-1]=s.sprintf("%d:%s",data->stat,sol[data->stat]); } } ShowMsg(msg); ShowLegend(msgs); }
//--------------------------------------------------------------------------- TPoint __fastcall TMapDialog::PosToGraphP(const double *pos, const double *ref, int index, int npos, TRect rect) { double sc[]={ 0.01,0.02,0.05,0.1,0.2,0.5,1,2,5,10,20,50,100,200,500,1000,2000,5000,10000 }; double rr[3],posr[3],enu[3],fact=40.0/sc[Scale]; TPoint p; ecef2pos(ref,posr); for (int i=0;i<3;i++) rr[i]=pos[i]-ref[i]; ecef2enu(posr,rr,enu); p.x=rect.Left+(int)((rect.Right-rect.Left)*index/(npos-1.0)+0.5); p.y=(rect.Top+rect.Bottom)/2-(int)(enu[2]*fact+0.5); return p; }
//--------------------------------------------------------------------------- void __fastcall TMapDialog::DispMouseMove(TObject *Sender, TShiftState Shift, int X, int Y) { double sc[]={ 0.01,0.02,0.05,0.1,0.2,0.5,1,2,5,10,20,50,100,200,500,1000,2000,5000,10000 }; double rr[3],posr[3],enu[3],fact=40.0/sc[Scale]; if (!Drag) return; enu[0]=(X0-X)/fact; enu[1]=(Y-Y0)/fact; enu[2]=0.0; ecef2pos(RefPos,posr); enu2ecef(posr,enu,rr); for (int i=0;i<3;i++) CentPos[i]=CentPos0[i]+rr[i]; MainForm->UpdateMap(); }
// get center position of plot ---------------------------------------------- int __fastcall TPlot::GetCenterPos(double *rr) { double xc,yc,pos[3],enu[3]={0},dr[3]; int i; trace(3,"GetCenterPos\n"); if (PLOT_OBS<=PlotType&&PlotType<=PLOT_DOP&&PlotType!=PLOT_TRK) return 0; if (norm(OPos,3)<=0.0) return 0; GraphT->GetCent(xc,yc); ecef2pos(OPos,pos); enu[0]=xc; enu[1]=yc; enu2ecef(pos,enu,dr); for (int i=0;i<3;i++) rr[i]=OPos[i]+dr[i]; return 1; }
//--------------------------------------------------------------------------- void __fastcall TOptDialog::BtnRefPosClick(TObject *Sender) { TEdit *edit[]={RefPos1,RefPos2,RefPos3}; double p[3],pos[3]; GetPos(RefPosTypeP->ItemIndex,edit,p); ecef2pos(p,pos); RefDialog->RovPos[0]=pos[0]*R2D; RefDialog->RovPos[1]=pos[1]*R2D; RefDialog->RovPos[2]=pos[2]; RefDialog->StaPosFile=StaPosFile->Text; RefDialog->Left=Left+Width/2-RefDialog->Width/2; RefDialog->Top=Top+Height/2-RefDialog->Height/2; if (RefDialog->ShowModal()!=mrOk) return; pos[0]=RefDialog->Pos[0]*D2R; pos[1]=RefDialog->Pos[1]*D2R; pos[2]=RefDialog->Pos[2]; pos2ecef(pos,p); SetPos(RefPosTypeP->ItemIndex,edit,p); }
/* pos2ecef() */ void utest2(void) { double lat,lon,h,pos[3],posi[3]; double r[3]; for (lat=-90.0;lat<=90.0;lat+=5.0) { for (lon=-180.0;lon<180.0;lon+=5.0) { for (h=-10.0;h<10000.0;h+=100.0) { pos[0]=lat*D2R; pos[1]=lon*D2R; pos[2]=h; pos2ecef(pos,r); ecef2pos(r,posi); assert(fabs(lat-posi[0]*R2D)<1E-7&& (lat==-90.0||lat==90.0?1:fabs(lon-posi[1]*R2D)<1E-7)&& fabs(h-posi[2])<1E-4); } } } printf("%s utset2 : OK\n",__FILE__); }
/* antenna phase center position ---------------------------------------------*/ static int antpos(prcopt_t *opt, int rcvno, const obs_t *obs, const nav_t *nav, const sta_t *sta, const char *posfile) { double *rr=rcvno==1?opt->ru:opt->rb,del[3],pos[3],dr[3]={0}; int i,postype=rcvno==1?opt->rovpos:opt->refpos; char *name; trace(3,"antpos : rcvno=%d\n",rcvno); if (postype==1) { /* average of single position */ if (!avepos(rr,rcvno,obs,nav,opt)) { showmsg("error : station pos computation"); return 0; } } else if (postype==2) { /* read from position file */ name=stas[rcvno==1?0:1].name; if (!getstapos(posfile,name,rr)) { showmsg("error : no position of %s in %s",name,posfile); return 0; } } else if (postype==3) { /* get from rinex header */ if (norm(stas[rcvno==1?0:1].pos,3)<=0.0) { showmsg("error : no position in rinex header"); trace(1,"no position position in rinex header\n"); return 0; } /* antenna delta */ if (stas[rcvno==1?0:1].deltype==0) { /* enu */ for (i=0;i<3;i++) del[i]=stas[rcvno==1?0:1].del[i]; del[2]+=stas[rcvno==1?0:1].hgt; ecef2pos(stas[rcvno==1?0:1].pos,pos); enu2ecef(pos,del,dr); } else { /* xyz */ for (i=0;i<3;i++) dr[i]=stas[rcvno==1?0:1].del[i]; } for (i=0;i<3;i++) rr[i]=stas[rcvno==1?0:1].pos[i]+dr[i]; } return 1; }
//--------------------------------------------------------------------------- void __fastcall TPntDialog::BtnAddClick(TObject *Sender) { TGridRect r={0}; AnsiString s; int i; double rr[3],pos[3]={0}; for (i=0;i<PntList->RowCount;i++) { if (PntList->Cells[3][i]=="") break; } if (i>=PntList->RowCount) return; if (!Plot->GetCurrentPos(rr)) return; if (norm(rr,3)<=0.0) return; ecef2pos(rr,pos); PntList->Cells[0][i]=s.sprintf("%.9f",pos[0]*R2D); PntList->Cells[1][i]=s.sprintf("%.9f",pos[1]*R2D); PntList->Cells[2][i]=s.sprintf("%.4f",pos[2]); PntList->Cells[3][i]=s.sprintf("new point %d",i+1); r.Top=r.Bottom=i; r.Left=0; r.Right=3; PntList->Selection=r; }
/* options to system options buffer ------------------------------------------*/ static void sysopts2buff(void) { double pos[3],*rr; char id[32],*p; int i,j,sat,*ps; elmask_ =prcopt_.elmin *R2D; elmaskar_ =prcopt_.elmaskar *R2D; elmaskhold_=prcopt_.elmaskhold*R2D; for (i=0;i<2;i++) { ps=i==0?&prcopt_.rovpos:&prcopt_.refpos; rr=i==0?prcopt_.ru:prcopt_.rb; if (*ps==0) { antpostype_[i]=0; ecef2pos(rr,pos); antpos_[i][0]=pos[0]*R2D; antpos_[i][1]=pos[1]*R2D; antpos_[i][2]=pos[2]; } else antpostype_[i]=*ps+1; } /* excluded satellites */ exsats_[0]='\0'; for (sat=1,p=exsats_;sat<=MAXSAT&&p-exsats_<(int)sizeof(exsats_)-32;sat++) { if (prcopt_.exsats[sat-1]) { satno2id(sat,id); p+=sprintf(p,"%s%s%s",p==exsats_?"":" ", prcopt_.exsats[sat-1]==2?"+":"",id); } } /* snrmask */ for (i=0;i<NFREQ;i++) { snrmask_[i][0]='\0'; p=snrmask_[i]; for (j=0;j<9;j++) { p+=sprintf(p,"%s%.0f",j>0?",":"",prcopt_.snrmask.mask[i][j]); } } }
/* set antenna parameters ----------------------------------------------------*/ static void setpcv(gtime_t time, prcopt_t *popt, nav_t *nav, const pcvs_t *pcvs, const pcvs_t *pcvr, const sta_t *sta) { pcv_t *pcv; double pos[3],del[3]; int i,j,mode=PMODE_DGPS<=popt->mode&&popt->mode<=PMODE_FIXED; char id[64]; /* set satellite antenna parameters */ for (i=0;i<MAXSAT;i++) { if (!(satsys(i+1,NULL)&popt->navsys)) continue; if (!(pcv=searchpcv(i+1,"",time,pcvs))) { satno2id(i+1,id); trace(2,"no satellite antenna pcv: %s\n",id); continue; } nav->pcvs[i]=*pcv; } for (i=0;i<(mode?2:1);i++) { if (!strcmp(popt->anttype[i],"*")) { /* set by station parameters */ strcpy(popt->anttype[i],sta[i].antdes); if (sta[i].deltype==1) { /* xyz */ if (norm(sta[i].pos,3)>0.0) { ecef2pos(sta[i].pos,pos); ecef2enu(pos,sta[i].del,del); for (j=0;j<3;j++) popt->antdel[i][j]=del[j]; } } else { /* enu */ for (j=0;j<3;j++) popt->antdel[i][j]=stas[i].del[j]; } } if (!(pcv=searchpcv(0,popt->anttype[i],time,pcvr))) { trace(2,"no receiver antenna pcv: %s\n",popt->anttype[i]); *popt->anttype[i]='\0'; continue; } strcpy(popt->anttype[i],pcv->type); popt->pcvr[i]=*pcv; } }
} char get_solution(double &lat, double &lon, double &height) { rtksvrlock(&svr); const sol_t *sol = &svr.solbuf[svr.nsol-1]; const double *rb = svr.rtk.rb; double pos[3]={0},Qr[9],Qe[9]={0},dms1[3]={0},dms2[3]={0},bl[3]={0}; double enu[3]={0},pitch=0.0,yaw=0.0,len; int i; int status; if (sol->time.time == 0 || !sol->stat) { rtksvrunlock(&svr); return -1; // no fix } if (1 <= sol->stat && sol->stat <= 2) status = 0; // fix if (sol->stat == 3) status = 1; // sbas fix else status = 2; // differential fix if (norm(sol->rr,3) > 0.0 && norm(rb,3) > 0.0) { for (i=0;i<3;i++) bl[i]=sol->rr[i]-rb[i]; } len=norm(bl,3); Qr[0]=sol->qr[0]; Qr[4]=sol->qr[1]; Qr[8]=sol->qr[2]; Qr[1]=Qr[3]=sol->qr[3]; Qr[5]=Qr[7]=sol->qr[4]; Qr[2]=Qr[6]=sol->qr[5]; if (norm(sol->rr,3) > 0.0) { ecef2pos(sol->rr,pos); covenu(pos,Qr,Qe); lat = pos[0]*R2D; lon = pos[1]*R2D; if (solopt[0].height == 1) pos[2]-=geoidh(pos); /* geodetic */ height = pos[2]; } svr.nsol=0; rtksvrunlock(&svr);
/* set rinex station and receiver info to options ----------------------------*/ static void rnx2opt(const rnxctr_t *rnx, rnxopt_t *opt) { double pos[3],enu[3]; int i; trace(3,"rnx2opt:\n"); /* receiver and antenna info */ if (!*opt->marker&&!*opt->markerno) { strcpy(opt->marker,rnx->sta.name); strcpy(opt->markerno,rnx->sta.marker); } if (!*opt->rec[0]&&!*opt->rec[1]&&!*opt->rec[2]) { strcpy(opt->rec[0],rnx->sta.recsno); strcpy(opt->rec[1],rnx->sta.rectype); strcpy(opt->rec[2],rnx->sta.recver); } if (!*opt->ant[0]&&!*opt->ant[1]) { strcpy(opt->ant[0],rnx->sta.antsno); strcpy(opt->ant[1],rnx->sta.antdes); } /* antenna approx position */ if (!opt->autopos&&norm(rnx->sta.pos,3)>0.0) { for (i=0;i<3;i++) opt->apppos[i]=rnx->sta.pos[i]; } /* antenna delta */ if (norm(rnx->sta.del,3)>0.0) { if (!rnx->sta.deltype) { /* enu */ opt->antdel[0]=rnx->sta.del[2]; /* h */ opt->antdel[1]=rnx->sta.del[0]; /* e */ opt->antdel[2]=rnx->sta.del[1]; /* n */ } else if (norm(opt->apppos,3)>0.0) { /* xyz */ ecef2pos(opt->apppos,pos); ecef2enu(pos,rnx->sta.del,enu); opt->antdel[0]=enu[2]; opt->antdel[1]=enu[0]; opt->antdel[2]=enu[1]; } } }