//--------------------------------------------------------------------------- void __fastcall TOptDialog::GetPos(int type, TEdit **edit, double *pos) { AnsiString edit0_Text=edit[0]->Text; AnsiString edit1_Text=edit[1]->Text; double p[3]={0},dms1[3]={0},dms2[3]={0}; if (type==1) { /* lat/lon/height dms/m */ sscanf(edit0_Text.c_str(),"%lf %lf %lf",dms1,dms1+1,dms1+2); sscanf(edit1_Text.c_str(),"%lf %lf %lf",dms2,dms2+1,dms2+2); p[0]=(dms1[0]<0?-1:1)*(fabs(dms1[0])+dms1[1]/60+dms1[2]/3600)*D2R; p[1]=(dms2[0]<0?-1:1)*(fabs(dms2[0])+dms2[1]/60+dms2[2]/3600)*D2R; p[2]=str2dbl(edit[2]->Text); pos2ecef(p,pos); } else if (type==2) { /* x/y/z-ecef */ pos[0]=str2dbl(edit[0]->Text); pos[1]=str2dbl(edit[1]->Text); pos[2]=str2dbl(edit[2]->Text); } else { p[0]=str2dbl(edit[0]->Text)*D2R; p[1]=str2dbl(edit[1]->Text)*D2R; p[2]=str2dbl(edit[2]->Text); pos2ecef(p,pos); } }
/* decode reference position -------------------------------------------------*/ static void decode_refpos(char *buff, const solopt_t *opt, double *rb) { double val[MAXFIELD],pos[3]; int i,n; const char *sep=opt2sep(opt); trace(3,"decode_refpos: buff=%s\n",buff); if ((n=tonum(buff,sep,val))<3) return; if ((opt->posf==SOLF_XYZ) || (opt->posf==SOLF_RVA)) { /* xyz */ for (i=0;i<3;i++) rb[i]=val[i]; } else if (opt->degf==0) { /* lat/lon/hgt (ddd.ddd) */ pos[0]=val[0]*D2R; pos[1]=val[1]*D2R; pos[2]=val[2]; pos2ecef(pos,rb); } else if (opt->degf==1&&n>=7) { /* lat/lon/hgt (ddd mm ss) */ pos[0]=dms2deg(val )*D2R; pos[1]=dms2deg(val+3)*D2R; pos[2]=val[6]; pos2ecef(pos,rb); } }
// read station position data ----------------------------------------------- void __fastcall TPlot::ReadStaPos(const char *file, const char *sta, double *rr) { FILE *fp; char buff[256],code[256],name[256]; double pos[3]; int sinex=0; if (!(fp=fopen(file,"r"))) return; while (fgets(buff,sizeof(buff),fp)) { if (strstr(buff,"%=SNX")==buff) sinex=1; if (buff[0]=='%'||buff[1]=='#') continue; if (sinex) { if (strlen(buff)<68||strncmp(buff+14,sta,4)) continue; if (!strncmp(buff+7,"STAX",4)) rr[0]=str2num(buff,47,21); if (!strncmp(buff+7,"STAY",4)) rr[1]=str2num(buff,47,21); if (!strncmp(buff+7,"STAZ",4)) { rr[2]=str2num(buff,47,21); break; } } else { if (sscanf(buff,"%lf %lf %lf %s",pos,pos+1,pos+2,code)<4) continue; if (strcmp(code,sta)) continue; pos[0]*=D2R; pos[1]*=D2R; pos2ecef(pos,rr); break; } } fclose(fp); }
/* station position from file ------------------------------------------------*/ static int getstapos(const char *file, char *name, double *r) { FILE *fp; char buff[256],sname[256],*p,*q; double pos[3]; trace(3,"getstapos: file=%s name=%s\n",file,name); if (!(fp=fopen(file,"r"))) { trace(1,"station position file open error: %s\n",file); return 0; } while (fgets(buff,sizeof(buff),fp)) { if ((p=strchr(buff,'%'))) *p='\0'; if (sscanf(buff,"%lf %lf %lf %s",pos,pos+1,pos+2,sname)<4) continue; for (p=sname,q=name;*p&&*q;p++,q++) { if (toupper((int)*p)!=toupper((int)*q)) break; } if (!*p) { pos[0]*=D2R; pos[1]*=D2R; pos2ecef(pos,r); fclose(fp); return 1; } } fclose(fp); trace(1,"no station position: %s %s\n",name,file); return 0; }
// read map path data ------------------------------------------------------- void __fastcall TPlot::ReadMapPath(AnsiString file) { FILE *fp; int n=0; char buff[1024]; double pos[3]; if (!(fp=fopen(file.c_str(),"r"))) return; while (fgets(buff,sizeof(buff),fp)&&n<MAXMAPPATH) { if (sscanf(buff,"%lf %lf %lf",pos,pos+1,pos+2)!=3) continue; pos[0]*=D2R; pos[1]*=D2R; pos2ecef(pos,MapPath+n*3); n++; } fclose(fp); NMapPath=n; BtnShowPoint->Down=true; UpdatePlot(); }
static void RtkCommon__pos2ecef(JNIEnv* env, jclass clazz, jdouble lat, jdouble lon, jdouble height, jdoubleArray j_pos) { double r[3] = {lat, lon, height}; double pos[3]; pos2ecef(r, pos); (*env)->SetDoubleArrayRegion(env, j_pos, 0, 3, pos); }
/* decode nmea gpgga: fix information ----------------------------------------*/ static int decode_nmeagga(char **val, int n, sol_t *sol) { gtime_t time; double tod=0.0,lat=0.0,lon=0.0,hdop=0.0,alt=0.0,msl=0.0,ep[6],tt; double pos[3]={0}; char ns='N',ew='E',ua=' ',um=' '; int i,solq=0,nrcv=0; trace(4,"decode_nmeagga: n=%d\n",n); for (i=0;i<n;i++) { switch (i) { case 0: tod =atof(val[i]); break; /* time in utc (hhmmss) */ case 1: lat =atof(val[i]); break; /* latitude (ddmm.mmm) */ case 2: ns =*val[i]; break; /* N=north,S=south */ case 3: lon =atof(val[i]); break; /* longitude (dddmm.mmm) */ case 4: ew =*val[i]; break; /* E=east,W=west */ case 5: solq=atoi(val[i]); break; /* fix quality */ case 6: nrcv=atoi(val[i]); break; /* # of satellite tracked */ case 7: hdop=atof(val[i]); break; /* hdop */ case 8: alt =atof(val[i]); break; /* altitude in msl */ case 9: ua =*val[i]; break; /* unit (M) */ case 10: msl =atof(val[i]); break; /* height of geoid */ case 11: um =*val[i]; break; /* unit (M) */ } } if ((ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) { trace(2,"invalid nmea gpgga format\n"); return 0; } if (sol->time.time==0.0) { trace(2,"no date info for nmea gpgga\n"); return 0; } pos[0]=(ns=='N'?1.0:-1.0)*dmm2deg(lat)*D2R; pos[1]=(ew=='E'?1.0:-1.0)*dmm2deg(lon)*D2R; pos[2]=alt+msl; time2epoch(sol->time,ep); septime(tod,ep+3,ep+4,ep+5); time=utc2gpst(epoch2time(ep)); tt=timediff(time,sol->time); if (tt<-43200.0) sol->time=timeadd(time, 86400.0); else if (tt> 43200.0) sol->time=timeadd(time,-86400.0); else sol->time=time; pos2ecef(pos,sol->rr); sol->stat=0<=solq&&solq<=8?solq_nmea[solq]:SOLQ_NONE; sol->ns=nrcv; sol->type=0; /* postion type = xyz */ trace(5,"decode_nmeagga: %s rr=%.3f %.3f %.3f stat=%d ns=%d hdop=%.1f ua=%c um=%c\n", time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns, hdop,ua,um); return 1; }
//--------------------------------------------------------------------------- void __fastcall TPntDialog::BtnOkClick(TObject *Sender) { double pos[3]={0}; int n=0; for (int i=0;i<PntList->RowCount;i++) { if (PntList->Cells[3][i]=="") continue; pos[0]=str2dbl(PntList->Cells[0][i])*D2R; pos[1]=str2dbl(PntList->Cells[1][i])*D2R; pos[2]=str2dbl(PntList->Cells[2][i]); pos2ecef(pos,Plot->PntPos[n]); Plot->PntName[n++]=PntList->Cells[3][i]; } Plot->NWayPnt=n; }
/* decode nmea gprmc: recommended minumum data for gps -----------------------*/ static int decode_nmearmc(char **val, int n, sol_t *sol) { double tod=0.0,lat=0.0,lon=0.0,vel=0.0,dir=0.0,date=0.0,ang=0.0,ep[6]; double pos[3]={0}; char act=' ',ns='N',ew='E',mew='E',mode='A'; int i; trace(4,"decode_nmearmc: n=%d\n",n); for (i=0;i<n;i++) { switch (i) { case 0: tod =atof(val[i]); break; /* time in utc (hhmmss) */ case 1: act =*val[i]; break; /* A=active,V=void */ case 2: lat =atof(val[i]); break; /* latitude (ddmm.mmm) */ case 3: ns =*val[i]; break; /* N=north,S=south */ case 4: lon =atof(val[i]); break; /* longitude (dddmm.mmm) */ case 5: ew =*val[i]; break; /* E=east,W=west */ case 6: vel =atof(val[i]); break; /* speed (knots) */ case 7: dir =atof(val[i]); break; /* track angle (deg) */ case 8: date=atof(val[i]); break; /* date (ddmmyy) */ case 9: ang =atof(val[i]); break; /* magnetic variation */ case 10: mew =*val[i]; break; /* E=east,W=west */ case 11: mode=*val[i]; break; /* mode indicator (>nmea 2) */ /* A=autonomous,D=differential */ /* E=estimated,N=not valid,S=simulator */ } } if ((act!='A'&&act!='V')||(ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) { trace(2,"invalid nmea gprmc format\n"); return 0; } pos[0]=(ns=='S'?-1.0:1.0)*dmm2deg(lat)*D2R; pos[1]=(ew=='W'?-1.0:1.0)*dmm2deg(lon)*D2R; septime(date,ep+2,ep+1,ep); septime(tod,ep+3,ep+4,ep+5); ep[0]+=ep[0]<80.0?2000.0:1900.0; sol->time=utc2gpst(epoch2time(ep)); pos2ecef(pos,sol->rr); sol->stat=mode=='D'?SOLQ_DGPS:SOLQ_SINGLE; sol->ns=0; sol->type=0; /* postion type = xyz */ trace(5,"decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d vel=%.2f dir=%.0f ang=%.0f mew=%c mode=%c\n", time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns, vel,dir,ang,mew,mode); return 1; }
/* system options buffer to options ------------------------------------------*/ static void buff2sysopts(void) { double pos[3],*rr; char buff[1024],*p,*id; int i,j,sat,*ps; prcopt_.elmin =elmask_ *D2R; prcopt_.elmaskar =elmaskar_ *D2R; prcopt_.elmaskhold=elmaskhold_*D2R; for (i=0;i<2;i++) { ps=i==0?&prcopt_.rovpos:&prcopt_.refpos; rr=i==0?prcopt_.ru:prcopt_.rb; if (antpostype_[i]==0) { /* lat/lon/hgt */ *ps=0; pos[0]=antpos_[i][0]*D2R; pos[1]=antpos_[i][1]*D2R; pos[2]=antpos_[i][2]; pos2ecef(pos,rr); } else if (antpostype_[i]==1) { /* xyz-ecef */ *ps=0; rr[0]=antpos_[i][0]; rr[1]=antpos_[i][1]; rr[2]=antpos_[i][2]; } else *ps=antpostype_[i]-1; } /* excluded satellites */ for (i=0;i<MAXSAT;i++) prcopt_.exsats[i]=0; if (exsats_[0]!='\0') { strcpy(buff,exsats_); for (p=strtok(buff," ");p;p=strtok(NULL," ")) { if (*p=='+') id=p+1; else id=p; if (!(sat=satid2no(id))) continue; prcopt_.exsats[sat-1]=*p=='+'?2:1; } } /* snrmask */ for (i=0;i<NFREQ;i++) { for (j=0;j<9;j++) prcopt_.snrmask.mask[i][j]=0.0; strcpy(buff,snrmask_[i]); for (p=strtok(buff,","),j=0;p&&j<9;p=strtok(NULL,",")) { prcopt_.snrmask.mask[i][j++]=atof(p); } } }
/* decode lat/lon/height -----------------------------------------------------*/ static int decode_solllh(char *buff, const solopt_t *opt, sol_t *sol) { double val[MAXFIELD],pos[3],Q[9]={0},P[9]; int i=0,n; const char *sep=opt2sep(opt); trace(4,"decode_solllh:\n"); n=tonum(buff,sep,val); if (!opt->degf) { if (n<3) return 0; pos[0]=val[i++]*D2R; /* lat/lon/hgt (ddd.ddd) */ pos[1]=val[i++]*D2R; pos[2]=val[i++]; } else { if (n<7) return 0; pos[0]=dms2deg(val )*D2R; /* lat/lon/hgt (ddd mm ss) */ pos[1]=dms2deg(val+3)*D2R; pos[2]=val[6]; i+=7; } pos2ecef(pos,sol->rr); if (i<n) sol->stat=(unsigned char)val[i++]; if (i<n) sol->ns =(unsigned char)val[i++]; if (i+3<n) { Q[4]=val[i]*val[i]; i++; /* sdn */ Q[0]=val[i]*val[i]; i++; /* sde */ Q[8]=val[i]*val[i]; i++; /* sdu */ if (i+3<n) { Q[1]=Q[3]=NSQR(val[i]); i++; /* sdne */ Q[2]=Q[6]=NSQR(val[i]); i++; /* sdeu */ Q[5]=Q[7]=NSQR(val[i]); i++; /* sdun */ } covecef(pos,Q,P); covtosol(P,sol->qr); } if (i<n) sol->age =(float)val[i++]; if (i<n) sol->ratio=(float)val[i]; sol->type=0; /* postion type = xyz */ sol->rva=0; /* postion type != rva */ if (MAXSOLQ<sol->stat) sol->stat=SOLQ_NONE; 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__); }
void rtkinit(rtk_t *rtk,const prcopt_t *opt) { gtime_t time0= {0}; // ambc_t ambc0={{0}}; ssat_t ssat0; double *x0=rtk->x; double *P0=rtk->P; double *xa=rtk->xa; double *Pa=rtk->Pa; char *errmsg=rtk->errbuf; int i,j,tmp; double pos[3]; rtk->opt=*opt; rtk->nx=NX(opt); rtk->na=NR(opt); //base position #ifdef BASE_POS_TYPE_LLH pos[0]=opt->rb[0]*D2R; pos[1]=opt->rb[1]*D2R; pos[2]=opt->rb[2]; pos2ecef(pos,rtk->rb); #else for (i=0; i<3; i++) rtk->rb[i]=opt->rb[i]; #endif //base velocity for (i=3; i<6; i++) { rtk->rb[i] = 0.0; } //rover position and velocities for (i=0; i<6; i++) rtk->sol.rr[i]=0.0; rtk->sol.dtr[0]=0.0; rtk->sol.time=time0; rtk->tt = 0.0;//time diff for (i=0,tmp=0; i<rtk->nx; i++) { x0[i]=0.0; for (j=0; j<rtk->nx; j++) { P0[tmp++]=0.0; } } for (i=0,tmp=0; i<rtk->na; i++) { xa[i]=0.0; for (j=0; j<rtk->na; j++) { Pa[tmp++]=0.0; } } rtk->nfix=rtk->neb=0; for (i=0; i<MAX_SAT; i++) { // rtk->ambc[i]=ambc0; rtk->ssat[i]=ssat0; } for (i=0; i<MAX_ERRMSG; i++) { errmsg[i]=0; } }
// generate visibility data ---------------------------------------------------- void __fastcall TPlot::GenVisData(void) { gtime_t time,ts,te; obsd_t data={{0}}; sta_t sta={0}; double tint,r,pos[3],rr[3],rs[6],e[3],azel[2]; int i,j,nobs=0; char name[16]; trace(3,"GenVisData\n"); ClearObs(); SimObs=1; ts=TimeStart; te=TimeEnd; tint=TimeInt; matcpy(pos,OOPos,3,1); pos2ecef(pos,rr); ReadWaitStart(); ShowLegend(NULL); ShowMsg("generating satellite visibility..."); Application->ProcessMessages(); for (time=ts;timediff(time,te)<=0.0;time=timeadd(time,tint)) { for (i=0;i<MAXSAT;i++) { satno2id(i+1,name); if (!tle_pos(time,name,"","",&TLEData,NULL,rs)) continue; if ((r=geodist(rs,rr,e))<=0.0) continue; if (satazel(pos,e,azel)<=0.0) continue; if (Obs.n>=Obs.nmax) { Obs.nmax=Obs.nmax<=0?4096:Obs.nmax*2; Obs.data=(obsd_t *)realloc(Obs.data,sizeof(obsd_t)*Obs.nmax); if (!Obs.data) { Obs.n=Obs.nmax=0; break; } } data.time=time; data.sat=i+1; for (j=0;j<NFREQ;j++) { data.P[j]=data.L[j]=0.0; data.code[j]=CODE_NONE; } data.code[0]=CODE_L1C; Obs.data[Obs.n++]=data; } if (++nobs>=MAX_SIMOBS) break; } if (Obs.n<=0) { ReadWaitEnd(); ShowMsg("no satellite visibility"); return; } UpdateObs(nobs); Caption="Satellite Visibility (Predicted)"; BtnSol1->Down=true; time2gpst(Obs.data[0].time,&Week); SolIndex[0]=SolIndex[1]=ObsIndex=0; if (PlotType<PLOT_OBS||PLOT_DOP<PlotType) { UpdateType(PLOT_OBS); } else { UpdatePlotType(); } FitTime(); ReadWaitEnd(); UpdateObsType(); UpdateTime(); UpdatePlot(); }
/* str2str -------------------------------------------------------------------*/ int main(int argc, char **argv) { static char cmd[MAXRCVCMD]=""; const char ss[]={'E','-','W','C','C'}; strconv_t *conv[MAXSTR]={NULL}; double pos[3],stapos[3]={0}; char *paths[MAXSTR],s[MAXSTR][MAXSTRPATH]={{0}},*cmdfile=""; char *local="",*proxy="",*msg="1004,1019",*opt="",buff[256],*p; char strmsg[MAXSTRMSG]=""; int i,n=0,dispint=5000,trlevel=0,opts[]={10000,10000,2000,32768,10,0,30}; int types[MAXSTR]={0},stat[MAXSTR]={0},byte[MAXSTR]={0},bps[MAXSTR]={0}; int fmts[MAXSTR],sta=0; for (i=0;i<MAXSTR;i++) paths[i]=s[i]; for (i=1;i<argc;i++) { if (!strcmp(argv[i],"-in")&&i+1<argc) { if (!decodepath(argv[++i],types,paths[0],fmts)) return -1; } else if (!strcmp(argv[i],"-out")&&i+1<argc&&n<MAXSTR-1) { if (!decodepath(argv[++i],types+n+1,paths[n+1],fmts+n+1)) return -1; n++; } else if (!strcmp(argv[i],"-p")&&i+3<argc) { pos[0]=atof(argv[++i])*D2R; pos[1]=atof(argv[++i])*D2R; pos[2]=atof(argv[++i]); pos2ecef(pos,stapos); } else if (!strcmp(argv[i],"-msg")&&i+1<argc) msg=argv[++i]; else if (!strcmp(argv[i],"-opt")&&i+1<argc) opt=argv[++i]; else if (!strcmp(argv[i],"-sta")&&i+1<argc) sta=atoi(argv[++i]); else if (!strcmp(argv[i],"-d" )&&i+1<argc) dispint=atoi(argv[++i]); else if (!strcmp(argv[i],"-s" )&&i+1<argc) opts[0]=atoi(argv[++i]); else if (!strcmp(argv[i],"-r" )&&i+1<argc) opts[1]=atoi(argv[++i]); else if (!strcmp(argv[i],"-n" )&&i+1<argc) opts[5]=atoi(argv[++i]); else if (!strcmp(argv[i],"-f" )&&i+1<argc) opts[6]=atoi(argv[++i]); else if (!strcmp(argv[i],"-c" )&&i+1<argc) cmdfile=argv[++i]; else if (!strcmp(argv[i],"-l" )&&i+1<argc) local=argv[++i]; else if (!strcmp(argv[i],"-x" )&&i+1<argc) proxy=argv[++i]; else if (!strcmp(argv[i],"-t" )&&i+1<argc) trlevel=atoi(argv[++i]); else if (*argv[i]=='-') printhelp(); } if (!*paths[0]) { fprintf(stderr,"specify input stream\n"); return -1; } if (n<=0) { fprintf(stderr,"specify output stream(s)\n"); return -1; } for (i=0;i<n;i++) { if (fmts[i+1]<0) continue; if (fmts[i+1]!=STRFMT_RTCM3) { fprintf(stderr,"unsupported output format\n"); return -1; } if (fmts[0]<0) { fprintf(stderr,"specify input format\n"); return -1; } if (!(conv[i]=strconvnew(fmts[0],fmts[i+1],msg,sta,sta!=0,opt))) { fprintf(stderr,"stream conversion error\n"); return -1; } } signal(SIGTERM,sigfunc); signal(SIGINT ,sigfunc); signal(SIGHUP ,SIG_IGN); signal(SIGPIPE,SIG_IGN); strsvrinit(&strsvr,n+1); if (trlevel>0) { traceopen(TRFILE); tracelevel(trlevel); } fprintf(stderr,"stream server start\n"); strsetdir(local); strsetproxy(proxy); if (*cmdfile) readcmd(cmdfile,cmd,0); /* start stream server */ if (!strsvrstart(&strsvr,opts,types,paths,conv,*cmd?cmd:NULL,stapos)) { fprintf(stderr,"stream server start error\n"); return -1; } for (intrflg=0;!intrflg;) { /* get stream server status */ strsvrstat(&strsvr,stat,byte,bps,strmsg); /* show stream server status */ for (i=0,p=buff;i<MAXSTR;i++) p+=sprintf(p,"%c",ss[stat[i]+1]); fprintf(stderr,"%s [%s] %10d B %7d bps %s\n", time_str(utc2gpst(timeget()),0),buff,byte[0],bps[0],strmsg); sleepms(dispint); } if (*cmdfile) readcmd(cmdfile,cmd,1); /* stop stream server */ strsvrstop(&strsvr,*cmd?cmd:NULL); for (i=0;i<n;i++) { strconvfree(conv[i]); } if (trlevel>0) { traceclose(); } fprintf(stderr,"stream server stop\n"); return 0; }
/* simgal main ---------------------------------------------------------------*/ int main(int argc, char **argv) { FILE *fp; rnxopt_t rnxopt={{0}}; obs_t obs={0}; nav_t nav={0}; gtime_t ts={0},te={0}; double es[]={2000,1,1,0,0,0},ee[]={2000,1,1,0,0,0},tint=30.0; double pos[3]={0},rr[3]; char *infile[16]={0},*outfile=""; int i,j,n=0,opt=0; for (i=1;i<argc;i++) { if (!strcmp(argv[i],"-o")&&i+1<argc) outfile=argv[++i]; else if (!strcmp(argv[i],"-ts")&&i+1<argc) { sscanf(argv[++i],"%lf/%lf/%lf %lf:%lf:%lf",es,es+1,es+2,es+3,es+4,es+5); ts=epoch2time(es); } else if (!strcmp(argv[i],"-te")&&i+1<argc) { sscanf(argv[++i],"%lf/%lf/%lf %lf:%lf:%lf",ee,ee+1,ee+2,ee+3,ee+4,ee+5); te=epoch2time(ee); } else if (!strcmp(argv[i],"-ti")&&i+1<argc) tint=atof(argv[++i]); else if (!strcmp(argv[i],"-r")&&i+3<argc) { for (j=0;j<3;j++) pos[j]=atof(argv[++i]); /* lat,lon,hgt */ } else infile[n++]=argv[i]; } if (n<=0) { fprintf(stderr,"no input file\n"); return -1; } if (!*outfile) { fprintf(stderr,"no output file\n"); return -1; } if (norm(pos,3)<=0.0) { fprintf(stderr,"no receiver pos\n"); return -1; } pos[0]*=D2R; pos[1]*=D2R; pos2ecef(pos,rr); /* read simulated/real rinex nav files */ readrnx(infile,n,&obs,&nav); if (nav.n<=0) { fprintf(stderr,"no nav data\n"); return -1; } /* generate simulated observation data */ if (!simobs(ts,te,tint,rr,&nav,&obs,opt)) return -1; /* output rinex obs file */ if (!(fp=fopen(outfile,"w"))) { fprintf(stderr,"error : outfile open %s\n",outfile); return -1; } fprintf(stderr,"saving...: %s\n",outfile); strcpy(rnxopt.prog,PROGNAME); strcpy(rnxopt.comment[0],"SIMULATED OBS DATA"); rnxopt.tstart=ts; rnxopt.tstart=te; rnxopt.navsys=SYS_ALL; rnxopt.obstype=OBSTYPE_PR|OBSTYPE_CP|OBSTYPE_SNR; rnxopt.freqtype=FREQTYPE_L1|FREQTYPE_L2|FREQTYPE_L5|FREQTYPE_L7; for (i=0;i<3;i++) rnxopt.apppos[i]=rr[i]; outrnxobsh(fp,&rnxopt); for (i=0;i<obs.n;i=j) { for (j=i;j<obs.n;j++) { if (timediff(obs.data[j].time,obs.data[i].time)>0.001) break; } outrnxobsb(fp,&rnxopt,obs.data+i,j-i,0); } fclose(fp); return 0; }
/* rnx2rtkp main -------------------------------------------------------------*/ int main(int argc, char **argv) { prcopt_t prcopt=prcopt_default; solopt_t solopt=solopt_default; filopt_t filopt={""}; gtime_t ts={0},te={0}; double tint=0.0,es[]={2000,1,1,0,0,0},ee[]={2000,12,31,23,59,59},pos[3]; int i,j,n,ret; char *infile[MAXFILE],*outfile=""; prcopt.mode =PMODE_KINEMA; prcopt.navsys=SYS_GPS|SYS_GLO; prcopt.refpos=1; prcopt.glomodear=1; sprintf(solopt.prog ,"%s ver.%s",PROGNAME,VER_RTKLIB); sprintf(filopt.trace,"%s.trace",PROGNAME); /* load options from configuration file */ for (i=1;i<argc;i++) { if (!strcmp(argv[i],"-k")&&i+1<argc) { resetsysopts(); if (!loadopts(argv[++i],sysopts)) return -1; getsysopts(&prcopt,&solopt,&filopt); } } for (i=1,n=0;i<argc;i++) { if (!strcmp(argv[i],"-o")&&i+1<argc) outfile=argv[++i]; else if (!strcmp(argv[i],"-ts")&&i+2<argc) { sscanf(argv[++i],"%lf/%lf/%lf",es,es+1,es+2); sscanf(argv[++i],"%lf:%lf:%lf",es+3,es+4,es+5); ts=epoch2time(es); } else if (!strcmp(argv[i],"-te")&&i+2<argc) { sscanf(argv[++i],"%lf/%lf/%lf",ee,ee+1,ee+2); sscanf(argv[++i],"%lf:%lf:%lf",ee+3,ee+4,ee+5); te=epoch2time(ee); } else if (!strcmp(argv[i],"-ti")&&i+1<argc) tint=atof(argv[++i]); else if (!strcmp(argv[i],"-k")&&i+1<argc) {++i; continue;} else if (!strcmp(argv[i],"-p")&&i+1<argc) prcopt.mode=atoi(argv[++i]); else if (!strcmp(argv[i],"-f")&&i+1<argc) prcopt.nf=atoi(argv[++i]); else if (!strcmp(argv[i],"-m")&&i+1<argc) prcopt.elmin=atof(argv[++i])*D2R; else if (!strcmp(argv[i],"-v")&&i+1<argc) prcopt.thresar[0]=atof(argv[++i]); else if (!strcmp(argv[i],"-s")&&i+1<argc) strcpy(solopt.sep,argv[++i]); else if (!strcmp(argv[i],"-d")&&i+1<argc) solopt.timeu=atoi(argv[++i]); else if (!strcmp(argv[i],"-b")) prcopt.soltype=1; else if (!strcmp(argv[i],"-c")) prcopt.soltype=2; else if (!strcmp(argv[i],"-i")) prcopt.modear=2; else if (!strcmp(argv[i],"-h")) prcopt.modear=3; else if (!strcmp(argv[i],"-t")) solopt.timef=1; else if (!strcmp(argv[i],"-u")) solopt.times=TIMES_UTC; else if (!strcmp(argv[i],"-e")) solopt.posf=SOLF_XYZ; else if (!strcmp(argv[i],"-a")) solopt.posf=SOLF_ENU; else if (!strcmp(argv[i],"-n")) solopt.posf=SOLF_NMEA; else if (!strcmp(argv[i],"-g")) solopt.degf=1; else if (!strcmp(argv[i],"-r")&&i+3<argc) { prcopt.refpos=0; for (j=0;j<3;j++) prcopt.rb[j]=atof(argv[++i]); } else if (!strcmp(argv[i],"-l")&&i+3<argc) { prcopt.refpos=0; for (j=0;j<3;j++) pos[j]=atof(argv[++i]); for (j=0;j<2;j++) pos[j]*=D2R; pos2ecef(pos,prcopt.rb); } else if (!strcmp(argv[i],"-y")&&i+1<argc) solopt.sstat=atoi(argv[++i]); else if (!strcmp(argv[i],"-x")&&i+1<argc) solopt.trace=atoi(argv[++i]); else if (*argv[i]=='-') printhelp(); else if (n<MAXFILE) infile[n++]=argv[i]; } if (n<=0) { showmsg("error : no input file"); return -2; } ret=postpos(ts,te,tint,0.0,&prcopt,&solopt,&filopt,infile,n,outfile,"",""); if (!ret) fprintf(stderr,"%40s\r",""); return ret; }
/* str2str -------------------------------------------------------------------*/ int main(int argc, char **argv) { static char cmd[MAXRCVCMD]=""; const char ss[]={'E','-','W','C','C'}; strconv_t *conv[MAXSTR]={NULL}; double pos[3],stapos[3]={0},off[3]={0}; char *paths[MAXSTR],s[MAXSTR][MAXSTRPATH]={{0}},*cmdfile=""; char *local="",*proxy="",*msg="1004,1019",*opt="",buff[256],*p; char strmsg[MAXSTRMSG]="",*antinfo="",*rcvinfo=""; char *ant[]={"","",""},*rcv[]={"","",""}; int i,j,n=0,dispint=5000,trlevel=0,opts[]={10000,10000,2000,32768,10,0,30}; int types[MAXSTR]={STR_FILE,STR_FILE},stat[MAXSTR]={0},byte[MAXSTR]={0}; int bps[MAXSTR]={0},fmts[MAXSTR]={0},sta=0; int ctl_port=2097; char ctl_buffer[256]; char *nodefile; int sockfd; char *rip; struct sockaddr_in serv_addr; rip=ctl_buffer; fd_set sockset; roverMsg rmsg; for (i=0;i<MAXSTR;i++) paths[i]=s[i]; for (i=1;i<argc;i++) { if (!strcmp(argv[i],"-in")&&i+1<argc) { if (!decodepath(argv[++i],types,paths[0],fmts)) return -1; } else if (!strcmp(argv[i],"-out")&&i+1<argc&&n<MAXSTR-1) { if (!decodepath(argv[++i],types+n+1,paths[n+1],fmts+n+1)) return -1; n++; } else if (!strcmp(argv[i],"-p")&&i+3<argc) { pos[0]=atof(argv[++i])*D2R; pos[1]=atof(argv[++i])*D2R; pos[2]=atof(argv[++i]); pos2ecef(pos,stapos); } else if (!strcmp(argv[i],"-o")&&i+3<argc) { off[0]=atof(argv[++i]); off[1]=atof(argv[++i]); off[2]=atof(argv[++i]); } else if (!strcmp(argv[i],"-msg")&&i+1<argc) msg=argv[++i]; else if (!strcmp(argv[i],"-opt")&&i+1<argc) opt=argv[++i]; else if (!strcmp(argv[i],"-sta")&&i+1<argc) sta=atoi(argv[++i]); else if (!strcmp(argv[i],"-d" )&&i+1<argc) dispint=atoi(argv[++i]); else if (!strcmp(argv[i],"-s" )&&i+1<argc) opts[0]=atoi(argv[++i]); else if (!strcmp(argv[i],"-r" )&&i+1<argc) opts[1]=atoi(argv[++i]); else if (!strcmp(argv[i],"-n" )&&i+1<argc) opts[5]=atoi(argv[++i]); else if (!strcmp(argv[i],"-f" )&&i+1<argc) opts[6]=atoi(argv[++i]); else if (!strcmp(argv[i],"-c" )&&i+1<argc) cmdfile=argv[++i]; else if (!strcmp(argv[i],"-a" )&&i+1<argc) antinfo=argv[++i]; else if (!strcmp(argv[i],"-i" )&&i+1<argc) rcvinfo=argv[++i]; else if (!strcmp(argv[i],"-l" )&&i+1<argc) local=argv[++i]; else if (!strcmp(argv[i],"-x" )&&i+1<argc) proxy=argv[++i]; else if (!strcmp(argv[i],"-t" )&&i+1<argc) trlevel=atoi(argv[++i]); else if (!strcmp(argv[i],"-port")&&i+1<argc) ctl_port=atoi(argv[++i]); else if (!strcmp(argv[i], "-nodefile")&& i+1<argc) nodefile=argv[++i]; else if (!strcmp(argv[i], "-rip")&&i+1<argc) rip=argv[++i]; else if (*argv[i]=='-') printhelp(); } if (n<=0) n=1; /* stdout */ fprintf(stderr,"argv parsed\n"); if(rip==ctl_buffer) { fprintf(stderr, "No rover ip address\n"); return(-1); } serv_addr.sin_family=AF_INET; inet_aton(rip, &serv_addr.sin_addr); serv_addr.sin_port=htons(ctl_port); sockfd=socket(AF_INET,SOCK_STREAM, 0); if (sockfd < 0) fprintf(stderr,"ERROR opening socket\n"); FD_ZERO(&sockset); FD_SET(sockfd, &sockset); for (i=0;i<n;i++) { if (fmts[i+1]<=0) continue; if (fmts[i+1]!=STRFMT_RTCM3) { fprintf(stderr,"unsupported output format\n"); return -1; } if (fmts[0]<0) { fprintf(stderr,"specify input format\n"); return -1; } if (!(conv[i]=strconvnew(fmts[0],fmts[i+1],msg,sta,sta!=0,opt))) { fprintf(stderr,"stream conversion error\n"); return -1; } strcpy(buff,antinfo); for (p=strtok(buff,","),j=0;p&&j<3;p=strtok(NULL,",")) ant[j++]=p; strcpy(conv[i]->out.sta.antdes,ant[0]); strcpy(conv[i]->out.sta.antsno,ant[1]); conv[i]->out.sta.antsetup=atoi(ant[2]); strcpy(buff,rcvinfo); for (p=strtok(buff,","),j=0;p&&j<3;p=strtok(NULL,",")) rcv[j++]=p; strcpy(conv[i]->out.sta.rectype,rcv[0]); strcpy(conv[i]->out.sta.recver ,rcv[1]); strcpy(conv[i]->out.sta.recsno ,rcv[2]); matcpy(conv[i]->out.sta.pos,pos,3,1); matcpy(conv[i]->out.sta.del,off,3,1); } signal(SIGTERM,sigfunc); signal(SIGINT ,sigfunc); signal(SIGHUP ,SIG_IGN); signal(SIGPIPE,SIG_IGN); strsvrinit(&strsvr,n+1); if (trlevel>0) { traceopen(TRFILE); tracelevel(trlevel); } fprintf(stderr,"stream server start\n"); strsetdir(local); strsetproxy(proxy); if (*cmdfile) readcmd(cmdfile,cmd,0); /* start stream server */ if (!strsvrstart(&strsvr,opts,types,paths,conv,*cmd?cmd:NULL,stapos)) { fprintf(stderr,"stream server start error\n"); return -1; } // Socket server start if(connect(sockfd,(struct sockaddr*)&serv_addr,sizeof(serv_addr))<0) fprintf(stderr,"error connecting\n"); if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) fprintf(stderr,"ERROR on binding"); for (intrflg=0;!intrflg;) { /* get stream server status */ strsvrstat(&strsvr,stat,byte,bps,strmsg); /* show stream server status */ for (i=0,p=buff;i<MAXSTR;i++) p+=sprintf(p,"%c",ss[stat[i]+1]); fprintf(stderr,"%s [%s] %10d B %7d bps %s\n", time_str(utc2gpst(timeget()),0),buff,byte[0],bps[0],strmsg); sleepms(dispint); } if (*cmdfile) readcmd(cmdfile,cmd,1); /* stop stream server */ strsvrstop(&strsvr,*cmd?cmd:NULL); for (i=0;i<n;i++) { strconvfree(conv[i]); } if (trlevel>0) { traceclose(); } fprintf(stderr,"stream server stop\n"); return 0; }
// update observation data index, azimuth/elevation, satellite list --------- void __fastcall TPlot::UpdateObs(int nobs) { AnsiString s; prcopt_t opt=prcopt_default; gtime_t time; sol_t sol={0}; double pos[3],rr[3],e[3],azel[MAXOBS*2]={0},rs[6],dts[2],var; int i,j,k,svh,per,per_=-1; char msg[128]; trace(3,"UpdateObs\n"); delete [] IndexObs; IndexObs=NULL; delete [] Az; Az=NULL; delete [] El; El=NULL; NObs=0; if (nobs<=0) return; IndexObs=new int[nobs]; Az=new double[Obs.n]; El=new double[Obs.n]; opt.err[0]=900.0; ReadWaitStart(); ShowLegend(NULL); for (i=0;i<Obs.n;i=j) { time=Obs.data[i].time; for (j=i;j<Obs.n;j++) { if (timediff(Obs.data[j].time,time)>TTOL) break; } IndexObs[NObs++]=i; if (Nav.n<=0&&Nav.ng<=0&&Nav.ns<=0) { for (k=0;k<j-i;k++) Az[i+k]=El[i+k]=0.0; continue; } if (RcvPos==0) { pntpos(Obs.data+i,j-i,&Nav,&opt,&sol,azel,NULL,msg); } else { if (RcvPos==1) { // lat/lon/height for (k=0;k<3;k++) pos[k]=OOPos[k]; pos2ecef(pos,rr); } else { // rinex header position for (k=0;k<3;k++) rr[k]=Sta.pos[k]; ecef2pos(rr,pos); } for (k=0;k<j-i;k++) { azel[k*2]=azel[1+k*2]=0.0; if (!satpos(time,time,Obs.data[i+k].sat,EPHOPT_BRDC,&Nav,rs,dts, &var,&svh)) continue; if (geodist(rs,rr,e)>0.0) satazel(pos,e,azel+k*2); } } for (k=0;k<j-i;k++) { Az[i+k]=azel[ k*2]; El[i+k]=azel[1+k*2]; if (Az[i+k]<0.0) Az[i+k]+=2.0*PI; } per=(i+1)*100/Obs.n; if (per!=per_) { ShowMsg(s.sprintf("updating azimuth/elevation... (%d%%)",(per_=per))); Application->ProcessMessages(); } } IndexObs[NObs]=Obs.n; UpdateSatList(); ReadWaitEnd(); }
/* generate simulated observation data ---------------------------------------*/ static int simobs(gtime_t ts, gtime_t te, double tint, const double *rr, nav_t *nav, obs_t *obs, int opt) { gtime_t time; obsd_t data[MAXSAT]={{{0}}}; double pos[3],rs[3*MAXSAT],dts[MAXSAT],r,e[3],azel[2]; double ecp[MAXSAT][NFREQ]={{0}},epr[MAXSAT][NFREQ]={{0}}; double snr[MAXSAT][NFREQ]={{0}},ers[MAXSAT][3]={{0}}; double iono,trop,fact,cp,pr,dtr=0.0,rref[3],bl; int i,j,k,n,ns,amb[MAXSAT][NFREQ]={{0}},sys,prn; char s[64]; double pref[]={36.106114294,140.087190410,70.3010}; /* ref station */ trace(3,"simobs:nnav=%d ngnav=%d\n",nav->n,nav->ng); for (i=0;i<2;i++) pref[i]*=D2R; pos2ecef(pref,rref); for (i=0;i<3;i++) rref[i]-=rr[i]; bl=norm(rref,3)/1E4; /* baseline (10km) */ srand(0); /* ephemeris error */ for (i=0;i<MAXSAT;i++) { data[i].sat=i+1; data[i].P[0]=2E7; for (j=0;j<3;j++) ers[i][j]=randn(0.0,erreph); } srand(tickget()); ecef2pos(rr,pos); n=(int)(timediff(te,ts)/tint+1.0); for (i=0;i<n;i++) { time=timeadd(ts,tint*i); time2str(time,s,0); for (j=0;j<MAXSAT;j++) data[j].time=time; for (j=0;j<3;j++) { /* iteration for pseudorange */ satpos(time,data,MAXSAT,nav,rs,dts); for (k=0;k<MAXSAT;k++) { if ((r=geodist(rs+k*3,rr,e))<=0.0) continue; data[k].P[0]=r+CLIGHT*(dtr-dts[k]); } } satpos(time,data,MAXSAT,nav,rs,dts); for (j=ns=0;j<MAXSAT;j++) { /* add ephemeris error */ for (k=0;k<3;k++) rs[k+j*3]+=ers[j][k]; if ((r=geodist(rs+j*3,rr,e))<=0.0) continue; satazel(pos,e,azel); if (azel[1]<minel*D2R) continue; iono=ionmodel(time,nav->ion,pos,azel); trop=tropmodel(pos,azel,0.3); /* add ionospheric error */ iono+=errion*bl*ionmapf(pos,azel); snrmodel(azel,snr[j]); errmodel(azel,snr[j],ecp[j],epr[j]); sys=satsys(data[j].sat,&prn); for (k=0;k<NFREQ;k++) { data[j].L[k]=data[j].P[k]=0.0; data[j].SNR[k]=0; data[j].LLI[k]=0; if (sys==SYS_GPS) { if (k>=3) continue; /* no L5a/L5b in gps */ if (k>=2&&!gpsblock[prn-1]) continue; /* no L5 in block II */ } else if (sys==SYS_GLO) { if (k>=3) continue; } else if (sys==SYS_GAL) { if (k==1) continue; /* no L2 in galileo */ } else continue; /* generate observation data */ fact=lam[k]*lam[k]/lam[0]/lam[0]; cp=r+CLIGHT*(dtr-dts[j])-fact*iono+trop+ecp[j][k]; pr=r+CLIGHT*(dtr-dts[j])+fact*iono+trop+epr[j][k]; if (amb[j][k]==0) amb[j][k]=(int)(-cp/lam[k]); data[j].L[k]=cp/lam[k]+amb[j][k]; data[j].P[k]=pr; data[j].SNR[k]=(unsigned char)snr[j][k]; data[j].LLI[k]=data[j].SNR[k]<slipthres?1:0; } if (obs->nmax<=obs->n) { if (obs->nmax==0) obs->nmax=65532; else obs->nmax+=65532; if (!(obs->data=(obsd_t *)realloc(obs->data,sizeof(obsd_t)*obs->nmax))) { fprintf(stderr,"malloc error\n"); return 0; } } obs->data[obs->n++]=data[j]; ns++; } fprintf(stderr,"time=%s nsat=%2d\r",s,ns); } fprintf(stderr,"\n"); return 1; }
} /* start rtk server ----------------------------------------------------------*/ static int startsvr(vt_t *vt) { double pos[3],npos[3]; char s[3][MAXRCVCMD]={"","",""},*cmds[]={NULL,NULL,NULL}; char *ropts[]={"","",""}; char *paths[]={ strpath[0],strpath[1],strpath[2],strpath[3],strpath[4],strpath[5], strpath[6],strpath[7] }; int i,ret,stropt[8]={0}; trace(3,"startsvr:\n"); /* read start commads from command files */ for (i=0;i<3;i++) { if (!*rcvcmds[i]) continue; if (!readcmd(rcvcmds[i],s[i],0)) { printvt(vt,"no command file: %s\n",rcvcmds[i]); } else cmds[i]=s[i]; } /* confirm overwrite */ for (i=3;i<8;i++) { if (strtype[i]==STR_FILE&&!confwrite(vt,strpath[i])) return 0; } if (prcopt.refpos==4) { /* rtcm */ for (i=0;i<3;i++) prcopt.rb[i]=0.0; } pos[0]=nmeapos[0]*D2R; pos[1]=nmeapos[1]*D2R; pos[2]=0.0; pos2ecef(pos,npos); /* read antenna file */ readant(vt,&prcopt,&svr.nav); /* open geoid data file */ if (solopt[0].geoid>0&&!opengeoid(solopt[0].geoid,filopt.geoid)) { trace(2,"geoid data open error: %s\n",filopt.geoid); printvt(vt,"geoid data open error: %s\n",filopt.geoid); } for (i=0;*rcvopts[i].name;i++) modflgr[i]=0; for (i=0;*sysopts[i].name;i++) modflgs[i]=0; /* set stream options */ stropt[0]=timeout; stropt[1]=reconnect; stropt[2]=1000; stropt[3]=buffsize; stropt[4]=fswapmargin; strsetopt(stropt); if (strfmt[2]==8) strfmt[2]=STRFMT_SP3; /* set ftp/http directory and proxy */ strsetdir(filopt.tempdir); strsetproxy(proxyaddr); /* execute start command */ if (*startcmd&&(ret=system(startcmd))) { trace(2,"command exec error: %s (%d)\n",startcmd,ret); printvt(vt,"command exec error: %s (%d)\n",startcmd,ret); } solopt[0].posf=strfmt[3]; solopt[1].posf=strfmt[4]; /* start rtk server */ if (!rtksvrstart(&svr,svrcycle,buffsize,strtype,paths,strfmt,navmsgsel, cmds,ropts,nmeacycle,nmeareq,npos,&prcopt,solopt,&moni)) { trace(2,"rtk server start error\n"); printvt(vt,"rtk server start error\n"); return 0; }