/* tle_pos() accuracy --------------------------------------------------------*/
static void utest3(void)
{
const char *file1="../data/tle/brdc3050.12*";
const char *file2="../data/tle/TLE_GNSS_20121101.txt";
const char *file3="../data/tle/igs17127.erp";
const double ep[6]={2012,10,31,0,0,0};
nav_t nav={0};
erp_t erp={0};
tle_t tle={0};
gtime_t time;
char sat[32];
double rs1[6],rs2[6],ds[6],dts[2],var;
int i,j,k,stat,svh;
readrnx(file1,0,"",NULL,&nav,NULL);
assert(nav.n>0);
stat=readerp(file3,&erp);
assert(stat);
stat=tle_read(file2,&tle);
assert(stat);
for (i=0;i<MAXSAT;i++) {
satno2id(i+1,sat);
fprintf(OUT,"SAT=%s\n",sat);
for (j=0;j<96;j++) {
time=timeadd(epoch2time(ep),900.0*j);
if (!satpos(time,time,i+1,EPHOPT_BRDC,&nav,rs1,dts,&var,&svh)) continue;
if (satsys(i+1,NULL)==SYS_QZS) svh&=0xFE;
if (svh) continue;
stat=tle_pos(time,sat,"","",&tle,&erp,rs2);
assert(stat);
for (k=0;k<3;k++) ds[k]=rs2[k]-rs1[k];
fprintf(OUT,"%6.0f %11.3f %11.3f %11.3f %11.3f\n",900.0*j,
ds[0]/1e3,ds[1]/1e3,ds[2]/1e3,norm(ds,3)/1e3);
assert(norm(ds,3)/1e3<300.0);
}
fprintf(OUT,"\n");
}
fprintf(OUT,"%s utest3 : OK\n",__FILE__);
}
// save dop -----------------------------------------------------------------
void __fastcall TPlot::SaveDop(AnsiString file)
{
FILE *fp;
gtime_t time;
double azel[MAXOBS*2],dop[4],tow;
int i,j,ns,week;
char tstr[64],*tlabel;
trace(3,"SaveDop: file=%s\n",file.c_str());
if (!(fp=fopen(file.c_str(),"w"))) return;
tlabel=TimeLabel<=1?"TIME (GPST)":(TimeLabel<=2?"TIME (UTC)":"TIME (JST)");
fprintf(fp,"%% %-*s %6s %8s %8s %8s %8s (EL>=%.0fdeg)\n",TimeLabel==0?13:19,
tlabel,"NSAT","GDOP","PDOP","HDOP","VDOP",ElMask);
for (i=0;i<NObs;i++) {
ns=0;
for (j=IndexObs[i];j<Obs.n&&j<IndexObs[i+1];j++) {
if (SatMask[Obs.data[j].sat-1]) continue;
if (El[j]<ElMask*D2R) continue;
if (ElMaskP&&El[j]<ElMaskData[(int)(Az[j]*R2D+0.5)]) continue;
azel[ ns*2]=Az[j];
azel[1+ns*2]=El[j];
ns++;
}
if (ns<=0) continue;
dops(ns,azel,ElMask*D2R,dop);
time=Obs.data[IndexObs[i]].time;
if (TimeLabel==0) {
tow=time2gpst(time,&week);
sprintf(tstr,"%4d %8.1f ",week,tow);
}
else if (TimeLabel==1) {
time2str(time,tstr,1);
}
else if (TimeLabel==2) {
time2str(gpst2utc(time),tstr,1);
}
else {
time2str(timeadd(gpst2utc(time),9*3600.0),tstr,1);
}
fprintf(fp,"%s %6d %8.1f %8.1f %8.1f %8.1f\n",tstr,ns,dop[0],dop[1],
dop[2], dop[3]);
}
fclose(fp);
}
/* decode type 19: rtk uncorrected pseudorange -------------------------------*/
static int decode_type19(rtcm_t *rtcm)
{
gtime_t time;
double usec,pr,tt;
int i=48,index,freq,sync=1,code,sys,prn,sat;
rtklib_trace(4,"decode_type19: len=%d\n",rtcm->len);
if (i+24<=rtcm->len*8) {
freq=getbitu(rtcm->buff,i, 2); i+= 2+2;
usec=getbitu(rtcm->buff,i,20); i+=20;
}
else {
rtklib_trace(2,"rtcm2 19 length error: len=%d\n",rtcm->len);
return -1;
}
if (freq&0x1) {
rtklib_trace(2,"rtcm2 19 not supported frequency: freq=%d\n",freq);
return -1;
}
freq>>=1;
while (i+48<=rtcm->len*8&&rtcm->obs.n<MAXOBS) {
sync=getbitu(rtcm->buff,i, 1); i+= 1;
code=getbitu(rtcm->buff,i, 1); i+= 1;
sys =getbitu(rtcm->buff,i, 1); i+= 1;
prn =getbitu(rtcm->buff,i, 5); i+= 5+8;
pr =getbitu(rtcm->buff,i,32); i+=32;
if (prn==0) prn=32;
if (!(sat=satno(sys?SYS_GLO:SYS_GPS,prn))) {
rtklib_trace(2,"rtcm2 19 satellite number error: sys=%d prn=%d\n",sys,prn);
continue;
}
time=timeadd(rtcm->time,usec*1E-6);
if (sys) time=utc2gpst(time); /* convert glonass time to gpst */
tt=timediff(rtcm->obs.data[0].time,time);
if (rtcm->obsflag||fabs(tt)>1E-9) {
rtcm->obs.n=rtcm->obsflag=0;
}
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);
}
}
rtcm->obsflag=!sync;
return sync?0:1;
}
// 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);
}
// get position regarding time ----------------------------------------------
double __fastcall TPlot::TimePos(gtime_t time)
{
double tow;
int week;
if (TimeLabel<=1) { // www/ssss or gpst
tow=time2gpst(time,&week);
}
else if (TimeLabel==2) { // utc
tow=time2gpst(gpst2utc(time),&week);
}
else { // jst
tow=time2gpst(timeadd(gpst2utc(time),9*3600.0),&week);
}
return tow+(week-Week)*86400.0*7;
}
/* output reference ----------------------------------------------------------*/
static int out_ref(FILE *ofp, gtime_t ts)
{
double ep[6];
char file[1024];
int i;
fprintf(ofp," <TR>\n");
fprintf(ofp," <TD><FONT size=\"-1\"><B>Report</B></FONT></TD>\n");
for (i=0; i<24; i++) {
time2epoch(timeadd(ts,i*3600.0),ep);
sprintf(file,REP_FILE,ep[0],ep[1],ep[2],ep[3]);
fprintf(ofp," <TD align=\"right\"><FONT size=\"-1\"><A href=\"../%s\">%02.0fh</A></FONT></TD>\n",
file,ep[3]);
}
fprintf(ofp," </TR>\n");
return 1;
}
/* satellite position and clock by broadcast ephemeris -----------------------*/
static int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int iode, double *rs, double *dts, double *var, int *svh,char **msg)
{
eph_t *eph;
double rst[3],dtst[1],tt=1E-3;
int i;
*svh=-1;
if (!(eph=seleph(teph,sat,iode,nav,msg))) return 0;
//start=TIM2->CNT;
eph2pos(time,eph,rs,dts,var);
//SendIntStr(TIM2->CNT-start);
time=timeadd(time,tt);
eph2pos(time,eph,rst,dtst,var);
*svh=eph->svh;
/* satellite velocity and clock drift by differential approx */
for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt;
dts[1]=(dtst[0]-dts[0])/tt;
return 1;
}
/* satellite position/clock by precise ephemeris/clock -------------------------
* compute satellite position/clock with precise ephemeris/clock
* args : gtime_t time I time (gpst)
* int sat I satellite number
* nav_t *nav I navigation data
* int opt I sat postion option
* (0: center of mass, 1: antenna phase center)
* double *rs O sat position and velocity (ecef)
* {x,y,z,vx,vy,vz} (m|m/s)
* double *dts O sat clock {bias,drift} (s|s/s)
* double *var IO sat position and clock error variance (m)
* (NULL: no output)
* return : status (1:ok,0:error or data outage)
* notes : clock includes relativistic correction but does not contain code bias
* before calling the function, nav->peph, nav->ne, nav->pclk and
* nav->nc must be set by calling readsp3(), readrnx() or readrnxt()
* if precise clocks are not set, clocks in sp3 are used instead
*-----------------------------------------------------------------------------*/
extern int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
double *rs, double *dts, double *var)
{
double rss[3],rst[3],dtss[1],dtst[1],dant[3]={0},vare=0.0,varc=0.0,tt=1E-3;
int i;
trace(4,"peph2pos: time=%s sat=%2d opt=%d\n",time_str(time,3),sat,opt);
if (sat<=0||MAXSAT<sat) return 0;
/* satellite position and clock bias */
if (!pephpos(time,sat,nav,rss,dtss,&vare,&varc)||
!pephclk(time,sat,nav,dtss,&varc)) return 0;
time=timeadd(time,tt);
if (!pephpos(time,sat,nav,rst,dtst,NULL,NULL)||
!pephclk(time,sat,nav,dtst,NULL)) return 0;
/* satellite antenna offset correction */
if (opt) {
satantoff(time,rss,nav->pcvs+sat-1,dant);
}
for (i=0;i<3;i++) {
rs[i ]=rss[i]+dant[i];
rs[i+3]=(rst[i]-rss[i])/tt;
}
/* relativistic effect correction */
if (dtss[0]!=0.0) {
dts[0]=dtss[0]-2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT;
dts[1]=(dtst[0]-dtss[0])/tt;
}
else { /* no precise clock */
dts[0]=dts[1]=0.0;
}
if (var) *var=vare+varc;
return 1;
}
/* decode raw ephemereris ----------------------------------------------------*/
static int decode_gw10reph(raw_t *raw)
{
eph_t eph={0};
double tow;
int i,week,prn,sat;
unsigned char *p=raw->buff+2,buff[90];
trace(4,"decode_gw10reph: len=%d\n",raw->len);
prn=U1(p);
if (!(sat=satno(SYS_GPS,prn))) {
trace(2,"gw10 raw ephemeris satellite number error: prn=%d\n",prn);
return -1;
}
for (i=0;i<90;i++) {
buff[i]=*(p+1+i);
}
if (decode_frame(buff ,&eph,NULL,NULL,NULL,NULL)!=1||
decode_frame(buff+ 30,&eph,NULL,NULL,NULL,NULL)!=2||
decode_frame(buff+ 60,&eph,NULL,NULL,NULL,NULL)!=3) {
trace(2,"gw10 raw ephemeris navigation frame error: prn=%d\n",prn);
return -1;
}
/* set time if no time avaliable */
if (raw->time.time==0) {
tow=getbitu(buff,24,17)*6.0;
week=getbitu(buff,48,10)+OFFWEEK;
raw->time=timeadd(gpst2time(week,tow),24.0);
}
if (!strstr(raw->opt,"-EPHALL")) {
if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */
}
eph.sat=sat;
raw->nav.eph[sat-1]=eph;
raw->ephsat=sat;
return 2;
}
/* convert nav message -------------------------------------------------------*/
static void convnav(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n)
{
gtime_t ts1,te1,ts2,te2;
int sys,prn;
trace(3,"convnav :\n");
ts1=opt->ts; if (ts1.time!=0) ts1=timeadd(ts1,-MAXDTOE);
te1=opt->te; if (te1.time!=0) te1=timeadd(te1, MAXDTOE);
ts2=opt->ts; if (ts2.time!=0) ts2=timeadd(ts2,-MAXDTOE_GLO);
te2=opt->te; if (te2.time!=0) te2=timeadd(te2, MAXDTOE_GLO);
sys=satsys(str->sat,&prn)&opt->navsys;
if (sys==SYS_GPS) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]) {
/* output rinex nav */
outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
n[1]++;
}
}
else if (sys==SYS_GLO) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts2,te2,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxgnavb(ofp[1],opt,str->nav->geph+prn-1);
n[1]++;
}
if (ofp[2]&&opt->rnxver<=2.99) {
/* output rinex gnav */
outrnxgnavb(ofp[2],opt,str->nav->geph+prn-1);
n[2]++;
}
}
else if (sys==SYS_SBS) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxhnavb(ofp[1],opt,str->nav->seph+prn-MINPRNSBS);
n[1]++;
}
if (ofp[3]&&opt->rnxver<=2.99) {
/* output rinex hnav */
outrnxhnavb(ofp[3],opt,str->nav->seph+prn-MINPRNSBS);
n[3]++;
}
}
else if (sys==SYS_QZS) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
n[1]++;
}
if (ofp[4]&&opt->rnxver<=2.99) {
/* output rinex qnav */
outrnxnavb(ofp[4],opt,str->nav->eph+str->sat-1);
n[4]++;
}
}
else if (sys==SYS_GAL) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
n[1]++;
}
if (ofp[5]&&opt->rnxver<=2.99) {
/* output rinex lnav */
outrnxnavb(ofp[5],opt,str->nav->eph+str->sat-1);
n[5]++;
}
}
else if (sys==SYS_CMP) {
if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
n[1]++;
}
}
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i=0;i<MAXITR;i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++) x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}
/* rinex converter for single-session ----------------------------------------*/
static int convrnx_s(int sess, int format, rnxopt_t *opt, const char *file,
char **ofile)
{
FILE *ofp[NOUTFILE]={NULL};
strfile_t *str;
gtime_t ts={0},te={0},tend={0},time={0};
unsigned char slips[MAXSAT][NFREQ+NEXOBS]={{0}};
int i,j,nf,type,n[NOUTFILE+1]={0},abort=0;
char path[1024],*paths[NOUTFILE],s[NOUTFILE][1024];
char *epath[MAXEXFILE]={0},*staid=*opt->staid?opt->staid:"0000";
trace(3,"convrnx_s: sess=%d format=%d file=%s ofile=%s %s %s %s %s %s %s\n",
sess,format,file,ofile[0],ofile[1],ofile[2],ofile[3],ofile[4],
ofile[5],ofile[6]);
/* replace keywords in input file */
if (reppath(file,path,opt->ts,staid,"")<0) {
showmsg("no time for input file: %s",file);
return 0;
}
/* expand wild-cards in input file */
for (i=0;i<MAXEXFILE;i++) {
if (!(epath[i]=(char *)malloc(1024))) {
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
return 0;
}
}
nf=expath(path,epath,MAXEXFILE);
if (format==STRFMT_RTCM2||format==STRFMT_RTCM3) time=opt->trtcm;
if (opt->scanobs) {
/* scan observation types */
if (!scan_obstype(format,epath[0],opt,&time)) return 0;
}
else {
/* set observation types by format */
set_obstype(format,opt);
}
if (!(str=gen_strfile(format,opt->rcvopt,time))) {
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
return 0;
}
time=opt->ts.time?opt->ts:(time.time?timeadd(time,TSTARTMARGIN):time);
/* replace keywords in output file */
for (i=0;i<NOUTFILE;i++) {
paths[i]=s[i];
if (reppath(ofile[i],paths[i],time,staid,"")<0) {
showmsg("no time for output path: %s",ofile[i]);
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
free_strfile(str);
return 0;
}
}
/* open output files */
if (!openfile(ofp,paths,path,opt,str->nav)) {
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
free_strfile(str);
return 0;
}
for (i=0;i<nf&&!abort;i++) {
/* open stream file */
if (!open_strfile(str,epath[i])) continue;
/* input message */
for (j=0;(type=input_strfile(str))>=-1;j++) {
if (j%11==1&&(abort=showstat(sess,te,te,n))) break;
/* avioid duplicated if overlapped data */
if (tend.time&&timediff(str->time,tend)<=0.0) continue;
/* convert message */
switch (type) {
case 1: convobs(ofp,opt,str,n,slips); break;
case 2: convnav(ofp,opt,str,n); break;
case 3: convsbs(ofp,opt,str,n); break;
case 31: convlex(ofp,opt,str,n); break;
case -1: n[NOUTFILE]++; break; /* error */
}
te=str->time; if (ts.time==0) ts=te;
/* set approx position */
if (type==1&&!opt->autopos&&norm(opt->apppos,3)<=0.0) {
setapppos(str,opt);
}
if (opt->te.time&&timediff(te,opt->te)>10.0) break;
}
/* close stream file */
close_strfile(str);
tend=te; /* end time of a file */
}
/* set receiver and antenna information to option */
if (format==STRFMT_RTCM2||format==STRFMT_RTCM3) {
rtcm2opt(&str->rtcm,opt);
}
else if (format==STRFMT_RINEX) {
rnx2opt(&str->rnx,opt);
}
/* close output files */
closefile(ofp,opt,str->nav);
/* remove empty output files */
for (i=0;i<NOUTFILE;i++) {
if (ofp[i]&&n[i]<=0) remove(ofile[i]);
}
if (ts.time>0) showstat(sess,ts,te,n);
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
free_strfile(str);
if (opt->tstart.time==0) opt->tstart=opt->ts;
if (opt->tend .time==0) opt->tend =opt->te;
return abort?-1:1;
}
int main() {
struct timeval now,next,diff,delay;
int success;
printf("%s: Starting dmx deamon\n", ProgName);
// intialize USB device
success = initUSB();
if ( !success ) {
printf ( "%s: Error initializing USB interface\n" , ProgName );
(*exitAddr)++;
return ( -1 );
}
// initialize shared memory segment
success = initSHM();
if ( !success ) {
printf ( "%s: Error initializing shared memory\n" , ProgName );
(*exitAddr)++;
return ( -2 );
}
// start timer
delay.tv_sec = 0;
delay.tv_usec= UpdateInt;
gettimeofday ( &next , NULL );
printf("%s: Init done. Entering loop cycle...\n", ProgName);
// loop until commanded to shutdown
while( !*exitAddr ) {
// send DMX data
success = sendDMX();
if ( !success ) {
printf ( "%s: DMX send error\n" , ProgName );
(*exitAddr)++;
}
// wait for update interval
timeadd ( &next , &next , &delay );
gettimeofday ( &now , NULL );
timediff ( &diff, &next , &now );
while (diff.tv_sec || diff.tv_usec) {
select ( 0, NULL, NULL, NULL, &diff );
gettimeofday ( &now, NULL );
timediff ( &diff, &next, &now );
};
}
printf ( "%s: dmx deamon is shutting down\n" , ProgName );
// on shutdown reset all DMX channels
memset ( chanData , 0, 512 * sizeof (ubyte) );
sendDMX();
// exit the system
exitUSB();
exitSHM();
return ( 0 );
}
static void *rtksvrthread(void *arg)
#endif
{
rtksvr_t *svr=(rtksvr_t *)arg;
obs_t obs;
obsd_t data[MAXOBS*2];
double tt;
unsigned int tick,ticknmea;
unsigned char *p,*q;
int i,j,n,fobs[3],cycle,cputime;
INIT_ZERO(fobs);
tracet(3,"rtksvrthread:\n");
svr->state=1; obs.data=data;
svr->tick=tickget();
ticknmea=svr->tick-1000;
for (cycle=0;svr->state;cycle++) {
tick=tickget();
for (i=0;i<3;i++) {
p=svr->buff[i]+svr->nb[i]; q=svr->buff[i]+svr->buffsize;
/* read receiver raw/rtcm data from input stream */
if ((n=strread(svr->stream+i,p,q-p))<=0) {
continue;
}
/* write receiver raw/rtcm data to log stream */
strwrite(svr->stream+i+5,p,n);
svr->nb[i]+=n;
/* save peek buffer */
rtksvrlock(svr);
n=n<svr->buffsize-svr->npb[i]?n:svr->buffsize-svr->npb[i];
memcpy(svr->pbuf[i]+svr->npb[i],p,n);
svr->npb[i]+=n;
rtksvrunlock(svr);
}
for (i=0;i<3;i++) {
if (svr->format[i]==STRFMT_SP3||svr->format[i]==STRFMT_RNXCLK) {
/* decode download file */
decodefile(svr,i);
}
else {
/* decode receiver raw/rtcm data */
fobs[i]=decoderaw(svr,i);
}
}
for (i=0;i<fobs[0];i++) { /* for each rover observation data */
obs.n=0;
for (j=0;j<svr->obs[0][i].n&&obs.n<MAXOBS*2;j++) {
obs.data[obs.n++]=svr->obs[0][i].data[j];
}
for (j=0;j<svr->obs[1][0].n&&obs.n<MAXOBS*2;j++) {
obs.data[obs.n++]=svr->obs[1][0].data[j];
}
/* rtk positioning */
rtksvrlock(svr);
rtkpos(&svr->rtk,obs.data,obs.n,&svr->nav);
rtksvrunlock(svr);
if (svr->rtk.sol.stat!=SOLQ_NONE) {
/* adjust current time */
tt=(int)(tickget()-tick)/1000.0+DTTOL;
timeset(gpst2utc(timeadd(svr->rtk.sol.time,tt)));
/* write solution */
writesol(svr,i);
}
/* if cpu overload, inclement obs outage counter and break */
if ((int)(tickget()-tick)>=svr->cycle) {
svr->prcout+=fobs[0]-i-1;
#if 0 /* omitted v.2.4.1 */
break;
#endif
}
}
/* send null solution if no solution (1hz) */
if (svr->rtk.sol.stat==SOLQ_NONE&&cycle%(1000/svr->cycle)==0) {
writesol(svr,0);
}
/* send nmea request to base/nrtk input stream */
if (svr->nmeacycle>0&&(int)(tick-ticknmea)>=svr->nmeacycle) {
if (svr->stream[1].state==1) {
if (svr->nmeareq==1) {
strsendnmea(svr->stream+1,svr->nmeapos);
}
else if (svr->nmeareq==2&&norm(svr->rtk.sol.rr,3)>0.0) {
strsendnmea(svr->stream+1,svr->rtk.sol.rr);
}
}
ticknmea=tick;
}
if ((cputime=(int)(tickget()-tick))>0) svr->cputime=cputime;
/* sleep until next cycle */
sleepms(svr->cycle-cputime);
}
for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i);
for (i=0;i<3;i++) {
svr->nb[i]=svr->npb[i]=0;
free(svr->buff[i]); svr->buff[i]=NULL;
free(svr->pbuf[i]); svr->pbuf[i]=NULL;
free_raw (svr->raw +i);
free_rtcm(svr->rtcm+i);
}
for (i=0;i<2;i++) {
svr->nsb[i]=0;
free(svr->sbuf[i]); svr->sbuf[i]=NULL;
}
return 0;
}
void stop_worker(int which) {
workers[which].status = DOOMED;
workers[which].expiration = timeadd(getnow(),DOOM_DELAY);
kill(workers[which].pid,SIGTERM);
reap_workers();
}
/* combine forward/backward solutions and output results ---------------------*/
static void combres(FILE *fp, const prcopt_t *popt, const solopt_t *sopt)
{
gtime_t time={0};
sol_t sols={{0}},sol={{0}};
double tt,Qf[9],Qb[9],Qs[9],rbs[3]={0},rb[3]={0};
int i,j,k,solstatic,pri[]={0,1,2,3,4,5,1,6};
trace(3,"combres : isolf=%d isolb=%d\n",isolf,isolb);
solstatic=sopt->solstatic&&
(popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC);
for (i=0,j=isolb-1;i<isolf&&j>=0;i++,j--) {
if ((tt=timediff(solf[i].time,solb[j].time))<-DTTOL) {
sols=solf[i];
for (k=0;k<3;k++) rbs[k]=rbf[k+i*3];
j++;
}
else if (tt>DTTOL) {
sols=solb[j];
for (k=0;k<3;k++) rbs[k]=rbb[k+j*3];
i--;
}
else if (solf[i].stat<solb[j].stat) {
sols=solf[i];
for (k=0;k<3;k++) rbs[k]=rbf[k+i*3];
}
else if (solf[i].stat>solb[j].stat) {
sols=solb[j];
for (k=0;k<3;k++) rbs[k]=rbb[k+j*3];
}
else {
sols=solf[i];
sols.time=timeadd(sols.time,-tt/2.0);
if ((popt->mode==PMODE_KINEMA||popt->mode==PMODE_MOVEB)&&
sols.stat==SOLQ_FIX) {
/* degrade fix to float if validation failed */
if (!valcomb(solf+i,solb+j)) sols.stat=SOLQ_FLOAT;
}
for (k=0;k<3;k++) {
Qf[k+k*3]=solf[i].qr[k];
Qb[k+k*3]=solb[j].qr[k];
}
Qf[1]=Qf[3]=solf[i].qr[3];
Qf[5]=Qf[7]=solf[i].qr[4];
Qf[2]=Qf[6]=solf[i].qr[5];
Qb[1]=Qb[3]=solb[j].qr[3];
Qb[5]=Qb[7]=solb[j].qr[4];
Qb[2]=Qb[6]=solb[j].qr[5];
if (smoother(solf[i].rr,Qf,solb[j].rr,Qb,3,sols.rr,Qs)) continue;
sols.qr[0]=(float)Qs[0];
sols.qr[1]=(float)Qs[4];
sols.qr[2]=(float)Qs[8];
sols.qr[3]=(float)Qs[1];
sols.qr[4]=(float)Qs[5];
sols.qr[5]=(float)Qs[2];
}
if (!solstatic) {
outsol(fp,&sols,rbs,sopt);
}
else if (time.time==0||pri[sols.stat]<=pri[sol.stat]) {
sol=sols;
for (k=0;k<3;k++) rb[k]=rbs[k];
if (time.time==0||timediff(sols.time,time)<0.0) {
time=sols.time;
}
}
}
if (solstatic&&time.time!=0.0) {
sol.time=time;
outsol(fp,&sol,rb,sopt);
}
}
int main(int argc, char* argv[])
{
struct timespec event_ts; // local event timestamp
int num_thrds = 0; // number of user threads to simulate
thread_data_t *tarr = NULL; // array of thread data structures
int sel_sid = -1; // index of thread selected by scheduler
thread_data_t *tsel = NULL; // pointer to the selected thread
int tmp = 0;
int (*sched_func)() = NULL; // scheduling function picked at run time
if (argc < 3) {
fprintf(stderr, usage, argv[0]);
exit(1);
}
tmp = strlen(argv[1]);
if (argv[1][tmp-1] == '+') {
unblock_signal = 1;
argv[1][tmp-1] = '\0';
}
for (int i=0 ; i < NUM_SCHED ; i++) {
if (strcmp(argv[1], MY_SCHED[i].name) == 0) {
sched_func = MY_SCHED[i].func;
break;
}
}
if (sched_func == NULL) {
errx(1, "Scheduling function %s not found!", argv[1]);
}
num_thrds = strtol(argv[2], NULL, 0);
if (argc > 3)
min_time = strtol(argv[3], NULL, 0);
if (argc > 4)
max_time = strtol(argv[4], NULL, 0);
// Get simulation start time for logging reference
clock_gettime(CLOCK_REALTIME, &sim_ts);
pthread_mutex_init(&simlock, NULL);
pthread_cond_init(&sched_cv, NULL);
pthread_cond_init(&run_cv, NULL);
init_thread_Q(&unblockQ, num_thrds);
init_thread_Q(&readyQ, num_thrds);
init_thread_Q(&blockedQ, num_thrds);
pthread_mutex_lock(&simlock);
// Create and fork number of threads as requested
tarr = calloc(num_thrds, sizeof(thread_data_t));
for (int i=0 ; i < num_thrds ; i++) {
tarr[i].sid = i;
// Thread is put to blocked state right after start
tarr[i].state = BLOCKED;
add_to_Q_back(&blockedQ, &tarr[i]);
fork_thread(&tarr[i]);
LOG("Thread tid=%d created with sid=%d\n", tarr[i].tid, tarr[i].sid);
}
run_sid = -1; // no thread is initially running
block_sid = -1; // no thread is initially entering block state
do {
if (block_sid != -1) {
// Scheduler was waken up by the running thread blocking
// Handle thread entering blocked state from running state
if (run_sid != -1)
errx(1, "thread[%d] was blocked while thread[%d] is still running",
block_sid, run_sid);
add_to_Q_back(&blockedQ, &tarr[block_sid]);
block_sid = -1;
} else if (run_sid != -1) {
// Scheduler was waken up by the tick_ts timeout
// Preempt running (and non-blocking) thread to ready state
thread_data_t *t = &tarr[run_sid];
if (t->state != RUNNING)
errx(1, "thread[%d] to be preempted is in %d state",
run_sid, t->state);
clock_gettime(CLOCK_REALTIME, &event_ts);
t->remain_usec -= TIMEUSEC(&t->event_ts, &event_ts);
t->event_ts = event_ts;
LOG("%8ld: scheduler preempts running thread[%d] with remain_usec=%d\n",
TIMEUSEC(&sim_ts, &event_ts), run_sid, t->remain_usec);
// Change the preempted thread into ready state
t->state = READY;
add_to_Q_back(&readyQ, t);
run_sid = -1;
// Signal the thread to stop running
pthread_cond_signal(&run_cv);
}
while (unblockQ.head >= 0) {
// Handle threads unblocking from blocked state into ready state
thread_data_t *t = remove_from_Q_front(&unblockQ);
clock_gettime(CLOCK_REALTIME, &event_ts);
t->event_ts = event_ts;
LOG("%8ld: scheduler changes thread[%d] with remain_usec=%d to ready\n",
TIMEUSEC(&sim_ts, &event_ts), t->sid, t->remain_usec);
// Change the unblocking thread into ready state
t->state = READY;
remove_from_Q(&blockedQ, t->sid);
add_to_Q_back(&readyQ, t);
// Signal the thread to become formally unblocked
pthread_cond_signal(&t->block_cv);
}
if (unblockQ.head >= 0) // unblockQ is not empty
errx(1, "unblockQ has pending threads from %d to %d!?!",
unblockQ.head, unblockQ.tail);
if (run_sid != -1)
errx(1, "scheduler is running with running thread[%d]!?!", run_sid);
if (block_sid != -1)
errx(1, "thread[%d] was blocking but not handled!?!", block_sid);
if (readyQ.head == -1 && blockedQ.head == -1) {
// no thread is running, neither in ready or blocked list
clock_gettime(CLOCK_REALTIME, &event_ts);
LOG("%8ld: all threads appear to have terminated\n",
TIMEUSEC(&sim_ts, &event_ts));
break;
}
if (tsel != NULL && tsel->state == RUNNING) {
// Scheduler was waken up (by unblocking or terminating threads) before
// last selected thread could startrunning -- do not select again
/*** Do nothing ***/
} else {
// Either scheduler did not select a thread last time, or the selected
// thread is not running any more (blocked, preempted or finished)
// Run the scheduling algorithm to select a new thread
sel_sid = sched_func();
clock_gettime(CLOCK_REALTIME, &event_ts);
if (sel_sid < 0) {
tsel = NULL;
LOG("%8ld: scheduler cannot find any thread to run\n",
TIMEUSEC(&sim_ts, &event_ts));
} else {
tsel = &tarr[sel_sid];
if (tsel->state != READY)
errx(1, "scheduler selects thread[%d] not in ready state", sel_sid);
LOG("%8ld: scheduler selects thread[%d] with remain_usec=%d to run\n",
TIMEUSEC(&sim_ts, &event_ts), sel_sid, tsel->remain_usec);
// Change the selected thread to running state
tsel->state = RUNNING;
remove_from_Q(&readyQ, tsel->sid);
// Signal the selected thread to run
pthread_cond_signal(&tsel->ready_cv);
}
}
// Scheduler wait for either a signal or tick_ts until next preemption
timeadd(&event_ts, &event_ts, &tick_ts);
pthread_cond_timedwait(&sched_cv, &simlock, &event_ts);
} while (1);
pthread_mutex_unlock(&simlock);
sched_yield();
for (int i=0 ; i < num_thrds ; i++) {
int rv;
int tid = wait(&rv); // wait for an (only) child to exit
LOG("Thread tid=%d exited with status %d\n", tid, WEXITSTATUS(rv));
}
for (int i=0 ; i < num_thrds ; i++) {
clear_thread(&tarr[i]);
}
free(tarr);
destroy_thread_Q(&unblockQ);
pthread_mutex_destroy(&simlock);
pthread_cond_destroy(&sched_cv);
pthread_cond_destroy(&run_cv);
return 0;
}
/* execute local file test -----------------------------------------------------
* execute local file test
* args : gtime_t ts,te I time start and end
* double tint I time interval (s)
* url_t *urls I download urls
* int nurl I number of urls
* char **stas I stations
* int nsta I number of stations
* char *dir I local directory
* int ncol I number of column
* int datefmt I date format (0:year-dow,1:year-dd/mm,2:week)
* FILE *fp IO log test result file pointer
* return : status (1:ok,0:error,-1:aborted)
*-----------------------------------------------------------------------------*/
extern void dl_test(gtime_t ts, gtime_t te, double ti, const url_t *urls,
int nurl, char **stas, int nsta, const char *dir,
int ncol, int datefmt, FILE *fp)
{
gtime_t time;
double tow;
char year[32],date[32],date_p[32];
int i,j,n,m,*nc,*nt,week,flag,abort=0;
if (ncol<1) ncol=1; else if (ncol>200) ncol=200;
fprintf(fp,"** LOCAL DATA AVAILABILITY (%s, %s) **\n\n",
time_str(timeget(),0),*dir?dir:"*");
for (i=n=0;i<nurl;i++) {
n+=strstr(urls[i].path,"%s")||strstr(urls[i].path,"%S")?nsta:1;
}
nc=imat(n,1);
nt=imat(n,1);
for (i=0;i<n;i++) nc[i]=nt[i]=0;
for (;timediff(ts,te)<1E-3&&!abort;ts=timeadd(ts,ti*ncol)) {
genpath(datefmt==0?" %Y-":"%Y/%m/","",ts,0,year);
if (datefmt<=1) fprintf(fp,"%s %s",datefmt==0?"DOY ":"DATE",year);
else fprintf(fp,"WEEK ");
*date_p='\0'; flag=0;
m=datefmt==2?1:2;
for (i=0;i<(ncol+m-1)/m;i++) {
time=timeadd(ts,ti*i*m);
if (timediff(time,te)>=1E-3) break;
if (datefmt<=1) {
genpath(datefmt==0?"%n":"%d","",time,0,date);
fprintf(fp,"%-4s",strcmp(date,date_p)?date:"");
}
else {
if (fabs(time2gpst(time,&week))<1.0) {
fprintf(fp,"%04d",week); flag=1;
}
else {
fprintf(fp,"%s",flag?"":" "); flag=0;
}
}
strcpy(date_p,date);
}
fprintf(fp,"\n");
for (i=j=0;i<nurl&&!abort;i++) {
time=timeadd(ts,ti*ncol-1.0);
if (timediff(time,te)>=0.0) time=te;
/* test local files */
abort=test_locals(ts,time,ti,urls+i,stas,nsta,dir,nc+j,nt+j,fp);
j+=strstr(urls[i].path,"%s")||strstr(urls[i].path,"%S")?nsta:1;
}
fprintf(fp,"\n");
}
fprintf(fp,"# COUNT : FILES/TOTAL\n");
for (i=j=0;i<nurl;i++) {
j+=print_total(urls+i,stas,nsta,nc+j,nt+j,fp);
}
free(nc); free(nt);
}
int main(int argc,char **argv)
{
ros::init(argc, argv, "rtk_robot");
ROS_INFO("RTKlib for ROS Robot Edition");
ros::NodeHandle nn;
ros::NodeHandle pn("~");
ros::Subscriber ecef_sub;
if(pn.getParam("base_position/x", ecef_base_station.position.x) && pn.getParam("base_position/y", ecef_base_station.position.y) && pn.getParam("base_position/z", ecef_base_station.position.z))
{
ROS_INFO("RTK -- Loading base station parameters from the parameter server...");
XmlRpc::XmlRpcValue position_covariance;
if( pn.getParam("base_position/covariance", position_covariance) )
{
ROS_ASSERT(position_covariance.getType() == XmlRpc::XmlRpcValue::TypeArray);
if(position_covariance.size() != 9)
{
ROS_WARN("RTK -- The base station covariances are not complete! Using default values...");
}
else
{
for(int i=0 ; i<position_covariance.size() ; ++i)
{
ROS_ASSERT(position_covariance[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
ecef_base_station.position_covariance[i] = static_cast<double>(position_covariance[i]);
}
}
}
}
else
{
ROS_INFO("RTK -- Subscribing to the base station for online parameters...");
ecef_sub = nn.subscribe("base_station/gps/ecef", 50, ecefCallback);
}
double rate;
pn.param("rate", rate, 2.0);
std::string gps_frame_id;
pn.param<std::string>("gps_frame_id", gps_frame_id, "gps");
std::string port;
pn.param<std::string>("port", port, "ttyACM0");
int baudrate;
pn.param("baudrate", baudrate, 115200);
ros::Publisher gps_pub = nn.advertise<sensor_msgs::NavSatFix>("gps/fix", 50);
ros::Publisher status_pub = nn.advertise<rtk_msgs::Status>("gps/status", 50);
ros::Subscriber gps_sub = nn.subscribe("base_station/gps/raw_data", 50, baseStationCallback);
int n;
//********************* rtklib stuff *********************
rtksvrinit(&server);
if(server.state)
{
ROS_FATAL("RTK -- Failed to initialize rtklib server!");
ROS_BREAK();
}
gtime_t time, time0 = {0};
int format[] = {STRFMT_UBX, STRFMT_UBX, STRFMT_RTCM2};
prcopt_t options = prcopt_default;
options.mode = 2;
options.nf = 1;
options.navsys = SYS_GPS | SYS_SBS;
options.modear = 3;
options.glomodear = 0;
options.minfix = 3;
options.ionoopt = IONOOPT_BRDC;
options.tropopt = TROPOPT_SAAS;
options.rb[0] = ecef_base_station.position.x;
options.rb[1] = ecef_base_station.position.y;
options.rb[2] = ecef_base_station.position.z;
strinitcom();
server.cycle = 10;
server.nmeacycle = 1000;
server.nmeareq = 0;
for(int i=0 ; i<3 ; i++) server.nmeapos[i] = 0;
server.buffsize = BUFFSIZE;
for(int i=0 ; i<3 ; i++) server.format[i] = format[i];
server.navsel = 0;
server.nsbs = 0;
server.nsol = 0;
server.prcout = 0;
rtkfree(&server.rtk);
rtkinit(&server.rtk, &options);
for(int i=0 ; i<3 ; i++)
{
server.nb[i] = server.npb[i] = 0;
if(!(server.buff[i]=(unsigned char *)malloc(BUFFSIZE)) || !(server.pbuf[i]=(unsigned char *)malloc(BUFFSIZE)))
{
ROS_FATAL("RTK -- Failed to initialize rtklib server - malloc error!");
ROS_BREAK();
}
for(int j=0 ; j<10 ; j++) server.nmsg[i][j] = 0;
for(int j=0 ; j<MAXOBSBUF ; j++) server.obs[i][j].n = 0;
/* initialize receiver raw and rtcm control */
init_raw(server.raw + i);
init_rtcm(server.rtcm + i);
/* set receiver option */
strcpy(server.raw[i].opt, "");
strcpy(server.rtcm[i].opt, "");
/* connect dgps corrections */
server.rtcm[i].dgps = server.nav.dgps;
}
/* output peek buffer */
for(int i=0 ; i<2 ; i++)
{
if (!(server.sbuf[i]=(unsigned char *)malloc(BUFFSIZE)))
{
ROS_FATAL("RTK -- Failed to initialize rtklib server - malloc error!");
ROS_BREAK();
}
}
/* set solution options */
solopt_t sol_options[2];
sol_options[0] = solopt_default;
sol_options[1] = solopt_default;
for(int i=0 ; i<2 ; i++) server.solopt[i] = sol_options[i];
/* set base station position */
for(int i=0 ; i<6 ; i++) server.rtk.rb[i] = i < 3 ? options.rb[i] : 0.0;
/* update navigation data */
for(int i=0 ; i<MAXSAT*2 ; i++) server.nav.eph[i].ttr = time0;
for(int i=0 ; i<NSATGLO*2 ; i++) server.nav.geph[i].tof = time0;
for(int i=0 ; i<NSATSBS*2 ; i++) server.nav.seph[i].tof = time0;
updatenav(&server.nav);
/* set monitor stream */
server.moni = NULL;
/* open input streams */
int stream_type[8] = {STR_SERIAL, 0, 0, 0, 0, 0, 0, 0};
char gps_path[64];
sprintf(gps_path, "%s:%d:8:n:1:off", port.c_str(), baudrate);
char * paths[] = {gps_path, "localhost:27015", "", "", "", "", "", ""};
char * cmds[] = {"", "", ""};
int rw;
for(int i=0 ; i<8 ; i++)
{
rw = i < 3 ? STR_MODE_R : STR_MODE_W;
if(stream_type[i] != STR_FILE) rw |= STR_MODE_W;
if(!stropen(server.stream+i, stream_type[i], rw, paths[i]))
{
ROS_ERROR("RTK -- Failed to open stream %s", paths[i]);
for(i-- ; i>=0 ; i--) strclose(server.stream+i);
ROS_FATAL("RTK -- Failed to initialize rtklib server - failed to open all streams!");
ROS_BREAK();
}
/* set initial time for rtcm and raw */
if(i<3)
{
time = utc2gpst(timeget());
server.raw[i].time = stream_type[i] == STR_FILE ? strgettime(server.stream+i) : time;
server.rtcm[i].time = stream_type[i] == STR_FILE ? strgettime(server.stream+i) : time;
}
}
/* sync input streams */
strsync(server.stream, server.stream+1);
strsync(server.stream, server.stream+2);
/* write start commands to input streams */
for(int i=0 ; i<3 ; i++)
{
if(cmds[i]) strsendcmd(server.stream+i, cmds[i]);
}
/* write solution header to solution streams */
for(int i=3 ; i<5 ; i++)
{
unsigned char buff[1024];
int n;
n = outsolheads(buff, server.solopt+i-3);
strwrite(server.stream+i, buff, n);
}
//********************************************************
obs_t obs;
obsd_t data[MAXOBS*2];
server.state=1;
obs.data=data;
double tt;
unsigned int tick;
int fobs[3] = {0};
server.tick = tickget();
ROS_DEBUG("RTK -- Initialization complete.");
ros::Rate r(rate);
while(ros::ok())
{
tick = tickget();
unsigned char *p = server.buff[RTK_ROBOT]+server.nb[RTK_ROBOT];
unsigned char *q = server.buff[RTK_ROBOT]+server.buffsize;
ROS_DEBUG("RTK -- Getting data from GPS...");
/* read receiver raw/rtcm data from input stream */
n = strread(server.stream, p, q-p);
/* write receiver raw/rtcm data to log stream */
strwrite(server.stream+5, p, n);
server.nb[RTK_ROBOT] += n;
/* save peek buffer */
rtksvrlock(&server);
n = n < server.buffsize - server.npb[RTK_ROBOT] ? n : server.buffsize - server.npb[RTK_ROBOT];
memcpy(server.pbuf[RTK_ROBOT] + server.npb[RTK_ROBOT], p, n);
server.npb[RTK_ROBOT] += n;
rtksvrunlock(&server);
ROS_DEBUG("RTK -- Decoding GPS data...");
/* decode data */
fobs[RTK_ROBOT] = decoderaw(&server, RTK_ROBOT);
fobs[RTK_BASE_STATION] = decoderaw(&server, RTK_BASE_STATION);
ROS_DEBUG("RTK -- Got %d observations.", fobs[RTK_ROBOT]);
/* for each rover observation data */
for(int i=0 ; i<fobs[RTK_ROBOT] ; i++)
{
obs.n = 0;
for(int j=0 ; j<server.obs[RTK_ROBOT][i].n && obs.n<MAXOBS*2 ; j++)
{
obs.data[obs.n++] = server.obs[RTK_ROBOT][i].data[j];
}
for(int j=0 ; j<server.obs[1][0].n && obs.n<MAXOBS*2 ; j++)
{
obs.data[obs.n++] = server.obs[1][0].data[j];
}
ROS_DEBUG("RTK -- Calculating RTK positioning...");
/* rtk positioning */
rtksvrlock(&server);
rtkpos(&server.rtk, obs.data, obs.n, &server.nav);
rtksvrunlock(&server);
sensor_msgs::NavSatFix gps_msg;
gps_msg.header.stamp = ros::Time::now();
gps_msg.header.frame_id = gps_frame_id;
rtk_msgs::Status status_msg;
status_msg.stamp = gps_msg.header.stamp;
if(server.rtk.sol.stat != SOLQ_NONE)
{
/* adjust current time */
tt = (int)(tickget()-tick)/1000.0+DTTOL;
timeset(gpst2utc(timeadd(server.rtk.sol.time,tt)));
/* write solution */
unsigned char buff[1024];
n = outsols(buff, &server.rtk.sol, server.rtk.rb, server.solopt);
if(n==141 && buff[0]>'0' && buff[0]<'9')
{
int ano,mes,dia,horas,minutos,Q,nsat;
double segundos,lat,longi,alt,sde,sdn,sdu,sdne,sdeu,sdun;
sscanf((const char *)(buff),"%d/%d/%d %d:%d:%lf %lf %lf %lf %d %d %lf %lf %lf %lf %lf %lf", &ano, &mes, &dia, &horas, &minutos, &segundos, &lat, &longi, &alt, &Q, &nsat, &sdn, &sde, &sdu, &sdne, &sdeu, &sdun);
gps_msg.latitude = lat;
gps_msg.longitude = longi;
gps_msg.altitude = alt;
gps_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
gps_msg.position_covariance[0] = sde + ecef_base_station.position_covariance[0];
gps_msg.position_covariance[1] = sdne + ecef_base_station.position_covariance[1];
gps_msg.position_covariance[2] = sdeu + ecef_base_station.position_covariance[2];
gps_msg.position_covariance[3] = sdne + ecef_base_station.position_covariance[3];
gps_msg.position_covariance[4] = sdn + ecef_base_station.position_covariance[4];
gps_msg.position_covariance[5] = sdun + ecef_base_station.position_covariance[5];
gps_msg.position_covariance[6] = sdeu + ecef_base_station.position_covariance[6];
gps_msg.position_covariance[7] = sdun + ecef_base_station.position_covariance[7];
gps_msg.position_covariance[8] = sdu + ecef_base_station.position_covariance[8];
gps_msg.status.status = Q==5 ? sensor_msgs::NavSatStatus::STATUS_FIX : sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
status_msg.fix_quality = Q;
status_msg.number_of_satellites = nsat;
}
}
else
{
gps_msg.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
}
ROS_DEBUG("RTK -- Publishing ROS msg...");
gps_pub.publish(gps_msg);
status_pub.publish(status_msg);
}
ros::spinOnce();
r.sleep();
}
return(0);
}
/* decode ublox rxm-raw: raw measurement data --------------------------------*/
static int decode_rxmraw(raw_t *raw)
{
gtime_t time;
double tow,tt,tadj=0.0,toff=0.0,tn;
int i,j,prn,sat,n=0,nsat,week;
unsigned char *p=raw->buff+6;
char *q,tstr[32];
trace(4,"decode_rxmraw: len=%d\n",raw->len);
/* time tag adjustment option (-tadj) */
if ((q=strstr(raw->opt,"-tadj"))) {
sscanf(q,"-tadj %lf",&tadj);
}
tow =U4(p );
week=U2(p+4);
nsat=U1(p+6);
if (raw->len<12+24*nsat) {
trace(2,"ubx rxmraw length error: len=%d nsat=%d\n",raw->len,nsat);
return -1;
}
time=gpst2time(week,tow*0.001);
/* time tag adjustment */
if (tadj>0.0) {
tn=time2gpst(time,&week)/tadj;
toff=(tn-floor(tn+0.5))*tadj;
time=timeadd(time,-toff);
}
if (fabs(tt=timediff(time,raw->time))<=1e-3) {
time2str(time,tstr,3);
trace(2,"ubx rxmraw time tag duplicated: time=%s\n",tstr);
return 0;
}
for (i=0,p+=8;i<nsat&&i<MAXOBS;i++,p+=24) {
raw->obs.data[n].time=time;
raw->obs.data[n].L[0] =R8(p )-toff*FREQ1;
raw->obs.data[n].P[0] =R8(p+ 8)-toff*CLIGHT;
raw->obs.data[n].D[0] =R4(p+16);
prn =U1(p+20);
raw->obs.data[n].SNR[0]=(unsigned char)(I1(p+22)*4.0+0.5);
raw->obs.data[n].LLI[0]=U1(p+23);
raw->obs.data[n].code[0]=CODE_L1C;
/* phase polarity flip option (-invcp) */
if (strstr(raw->opt,"-invcp")) {
raw->obs.data[n].L[0]=-raw->obs.data[n].L[0];
}
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"ubx rxmraw sat number error: prn=%d\n",prn);
continue;
}
raw->obs.data[n].sat=sat;
if (raw->obs.data[n].LLI[0]&1) raw->lockt[sat-1][0]=0.0;
else if (tt<0.0||10.0<tt) raw->lockt[sat-1][0]=0.0;
else raw->lockt[sat-1][0]+=tt;
for (j=1;j<NFREQ;j++) {
raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
raw->obs.data[n].D[j]=0.0;
raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
raw->obs.data[n].code[j]=CODE_NONE;
}
n++;
}
raw->time=time;
raw->obs.n=n;
return 1;
}
/* 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;
}
/* set approx position -------------------------------------------------------*/
static void setapppos(strfile_t *str, rnxopt_t *opt)
{
prcopt_t prcopt=prcopt_default;
sol_t sol={{0}};
char msg[128];
prcopt.navsys=opt->navsys;
/* point positioning with last obs data */
#ifdef WAAS_STUDY
if (!pntpos(str->obs->data,str->obs->n,str->nav,&prcopt,&sol,NULL,NULL,
NULL,msg)) {
#else
if (!pntpos(str->obs->data,str->obs->n,str->nav,&prcopt,&sol,NULL,NULL,
msg)) {
#endif
trace(2,"point position error (%s)\n",msg);
return;
}
matcpy(opt->apppos,sol.rr,3,1);
}
/* show status message -------------------------------------------------------*/
static int showstat(int sess, gtime_t ts, gtime_t te, int *n)
{
const char type[]="ONGHQLSE";
char msg[1024]="",*p=msg,s[64];
int i;
if (sess>0) {
p+=sprintf(p,"(%d) ",sess);
}
if (ts.time!=0) {
time2str(ts,s,0);
p+=sprintf(p,"%s",s);
}
if (te.time!=0&&timediff(te,ts)>0.9) {
time2str(te,s,0);
p+=sprintf(p,"-%s",s+5);
}
p+=sprintf(p,": ");
for (i=0;i<NOUTFILE+1;i++) {
if (n[i]==0) continue;
p+=sprintf(p,"%c=%d%s",type[i],n[i],i<NOUTFILE?" ":"");
}
return showmsg(msg);
}
/* rinex converter for single-session ----------------------------------------*/
static int convrnx_s(int sess, int format, rnxopt_t *opt, const char *file,
char **ofile)
{
FILE *ofp[NOUTFILE]={NULL};
strfile_t *str;
gtime_t ts={0},te={0},tend={0},time={0};
unsigned char slips[MAXSAT][NFREQ+NEXOBS]={{0}};
int i,j,nf,type,n[NOUTFILE+1]={0},abort=0;
char path[1024],*paths[NOUTFILE],s[NOUTFILE][1024];
char *epath[MAXEXFILE]={0},*staid=*opt->staid?opt->staid:"0000";
trace(3,"convrnx_s: sess=%d format=%d file=%s ofile=%s %s %s %s %s %s %s\n",
sess,format,file,ofile[0],ofile[1],ofile[2],ofile[3],ofile[4],
ofile[5],ofile[6]);
/* replace keywords in input file */
if (reppath(file,path,opt->ts,staid,"")<0) {
showmsg("no time for input file: %s",file);
return 0;
}
/* expand wild-cards in input file */
for (i=0;i<MAXEXFILE;i++) {
if (!(epath[i]=(char *)malloc(1024))) {
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
return 0;
}
}
nf=expath(path,epath,MAXEXFILE);
if (format==STRFMT_RTCM2||format==STRFMT_RTCM3) time=opt->trtcm;
if (opt->scanobs) {
/* scan observation types */
if (!scan_obstype(format,epath[0],opt,&time)) return 0;
}
else {
/* set observation types by format */
set_obstype(format,opt);
}
if (!(str=gen_strfile(format,opt->rcvopt,time))) {
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
return 0;
}
time=opt->ts.time?opt->ts:(time.time?timeadd(time,TSTARTMARGIN):time);
/* replace keywords in output file */
for (i=0;i<NOUTFILE;i++) {
paths[i]=s[i];
if (reppath(ofile[i],paths[i],time,staid,"")<0) {
showmsg("no time for output path: %s",ofile[i]);
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
free_strfile(str);
return 0;
}
}
/* open output files */
if (!openfile(ofp,paths,path,opt,str->nav)) {
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
free_strfile(str);
return 0;
}
for (i=0;i<nf&&!abort;i++) {
/* open stream file */
if (!open_strfile(str,epath[i])) continue;
/* input message */
for (j=0;(type=input_strfile(str))>=-1;j++) {
if (j%11==1&&(abort=showstat(sess,te,te,n))) break;
/* avioid duplicated if overlapped data */
if (tend.time&&timediff(str->time,tend)<=0.0) continue;
/* convert message */
switch (type) {
case 1: convobs(ofp,opt,str,n,slips); break;
case 2: convnav(ofp,opt,str,n); break;
case 3: convsbs(ofp,opt,str,n); break;
case 31: convlex(ofp,opt,str,n); break;
case -1: n[NOUTFILE]++; break; /* error */
}
te=str->time; if (ts.time==0) ts=te;
/* set approx position */
if (type==1&&!opt->autopos&&norm(opt->apppos,3)<=0.0) {
setapppos(str,opt);
}
if (opt->te.time&&timediff(te,opt->te)>10.0) break;
}
/* close stream file */
close_strfile(str);
tend=te; /* end time of a file */
}
/* set receiver and antenna information to option */
if (format==STRFMT_RTCM2||format==STRFMT_RTCM3) {
rtcm2opt(&str->rtcm,opt);
}
else if (format==STRFMT_RINEX) {
rnx2opt(&str->rnx,opt);
}
/* close output files */
closefile(ofp,opt,str->nav);
/* remove empty output files */
for (i=0;i<NOUTFILE;i++) {
if (ofp[i]&&n[i]<=0) remove(ofile[i]);
}
if (ts.time>0) showstat(sess,ts,te,n);
for (i=0;i<MAXEXFILE;i++) free(epath[i]);
free_strfile(str);
if (opt->tstart.time==0) opt->tstart=opt->ts;
if (opt->tend .time==0) opt->tend =opt->te;
return abort?-1:1;
}
/* main ----------------------------------------------------------------------*/
int main(int argc, char **argv)
{
FILE *fp=NULL;
nav_t nav={0};
lex_t lex={0};
gtime_t t0,time;
double ep0[]={2000,1,1,0,0,0},tspan=24.0,tint=7200,dpos=1;
double rpos[]={35,137};
char *ifile="",*ofile="";
int i,trl=0,index=0,nlat=45,nlon=45;
t0=epoch2time(ep0);
for (i=1;i<argc;i++) {
if (!strcmp(argv[i],"-t0")&&i+2<argc) {
if (sscanf(argv[++i],"%lf/%lf/%lf",ep0 ,ep0+1,ep0+2)<3||
sscanf(argv[++i],"%lf:%lf:%lf",ep0+3,ep0+4,ep0+5)<1) {
fprintf(stderr,"invalid time\n");
return -1;
}
}
else if (!strcmp(argv[i],"-ts")&&i+1<argc) tspan=atof(argv[++i]);
else if (!strcmp(argv[i],"-ti")&&i+1<argc) tint =atof(argv[++i]);
else if (!strcmp(argv[i],"-u" )&&i+2<argc) {
rpos[0]=atof(argv[++i]);
rpos[1]=atof(argv[++i]);
}
else if (!strcmp(argv[i],"-n" )&&i+2<argc) {
nlat=atoi(argv[++i]);
nlon=atoi(argv[++i]);
}
else if (!strcmp(argv[i],"-d" )&&i+1<argc) dpos =atof(argv[++i]);
else if (!strcmp(argv[i],"-o" )&&i+1<argc) ofile=argv[++i];
else if (!strcmp(argv[i],"-x" )&&i+1<argc) trl =atof(argv[++i]);
else ifile=argv[i];
}
if (trl>0) {
traceopen("diffeph.trace");
tracelevel(trl);
}
t0=epoch2time(ep0);
if (!lexreadmsg(ifile,0,&lex)) {
fprintf(stderr,"file read error: %s\n",ifile);
return -1;
}
if (!(fp=fopen(ofile,"w"))) {
fprintf(stderr,"file open error: %s\n",ofile);
return -1;
}
fprintf(fp,"epoch=[%.0f %.0f %.0f %.0f %.0f %.0f];\n",
ep0[0],ep0[1],ep0[2],ep0[3],ep0[4],ep0[5]);
for (i=0;i<(int)(tspan*3600.0/tint);i++) {
time=timeadd(t0,tint*i);
fprintf(stderr,"time=%s\r",time_str(time,0));
index=updatelex(index,time,&lex,&nav);
printtec(i+1,time,tint*i,&nav,rpos,nlat,nlon,dpos,fp);
}
fclose(fp);
if (trl>0) traceclose();
return 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();
}
/* generate data qc page -----------------------------------------------------*/
static int gen_page3(gtime_t time, const char *temp, const char *page)
{
FILE *ifp,*ofp;
gtime_t ts,te;
double ep[6],hour[24],mp1[MAXSTA][24]= {{0}},mp2[MAXSTA][24]= {{0}};
double slip[MAXSTA][24]= {{0}},val1[MAXSTA],val2[MAXSTA],val3[MAXSTA];
char buff[1024],file[1024],*sta[MAXSTA],str[MAXSTA][32];
int i,j,n,nsta=0,sec=0;
ts=timeadd(time,-90000.0);
te=timeadd(time, -3600.0);
for (i=0; i<MAXSTA; i++) sta[i]=str[i];
for (i=0; i<24; i++) {
time2epoch(timeadd(ts,i*3600.0),ep);
sprintf(file,QC_FILE,ep[0],ep[1],ep[2],ep[3]);
if (!(n=read_qc(file,sta,val1,val2,val3))) continue;
for (j=0; j<n; j++) {
mp1 [j][i]=val1[j];
mp2 [j][i]=val2[j];
slip[j][i]=val3[j];
}
hour[i]=ep[3];
nsta=n;
}
if (!(ifp=fopen(temp,"r"))) {
fprintf(stderr,"file open error: %s\n",temp);
return 0;
}
if (!(ofp=fopen(page,"w"))) {
fprintf(stderr,"file open error: %s\n",page);
fclose(ifp);
return 0;
}
while (fgets(buff,sizeof(buff),ifp)) {
if (strstr(buff,"@DATE START" )) sec=1;
else if (strstr(buff,"@TABLE0 START")) sec=2;
else if (strstr(buff,"@TABLE1 START")) sec=3;
else if (strstr(buff,"@TABLE2 START")) sec=4;
else if (strstr(buff,"@DATE END" )) sec=0;
else if (strstr(buff,"@TABLE0 END" )) sec=0;
else if (strstr(buff,"@TABLE1 END" )) sec=0;
else if (strstr(buff,"@TABLE2 END" )) sec=0;
switch (sec) {
case 0:
fputs(buff,ofp);
break;
case 1:
out_date(ofp,ts,te);
break;
case 2:
out_table1(ofp,sta,nsta,hour,mp1 ,3);
break;
case 3:
out_table1(ofp,sta,nsta,hour,mp2 ,3);
break;
case 4:
out_table1(ofp,sta,nsta,hour,slip,0);
break;
}
}
fclose(ifp);
fclose(ofp);
return 1;
}
/* decode NVS xf5-raw: raw measurement data ----------------------------------*/
static int decode_xf5raw(raw_t *raw)
{
gtime_t time;
double tadj=0.0,toff=0.0,tn;
int dTowInt;
double dTowUTC, dTowGPS, dTowFrac, L1, P1, D1;
double gpsutcTimescale;
unsigned char rcvTimeScaleCorr, sys, carrNo;
int i,j,prn,sat,n=0,nsat,week;
unsigned char *p=raw->buff+2;
char *q,tstr[32],flag;
trace(4,"decode_xf5raw: len=%d\n",raw->len);
/* time tag adjustment option (-TADJ) */
if ((q=strstr(raw->opt,"-tadj"))) {
sscanf(q,"-TADJ=%lf",&tadj);
}
dTowUTC =R8(p);
week = U2(p+8);
gpsutcTimescale = R8(p+10);
/* glonassutcTimescale = R8(p+18); */
rcvTimeScaleCorr = I1(p+26);
/* check gps week range */
if (week>=4096) {
trace(2,"nvs xf5raw obs week error: week=%d\n",week);
return -1;
}
week=adjgpsweek(week);
if ((raw->len - 31)%30) {
/* Message length is not correct: there could be an error in the stream */
trace(2,"nvs xf5raw len=%d seems not be correct\n",raw->len);
return -1;
}
nsat = (raw->len - 31)/30;
dTowGPS = dTowUTC + gpsutcTimescale;
/* Tweak pseudoranges to allow Rinex to represent the NVS time of measure */
dTowInt = 10.0*floor((dTowGPS/10.0)+0.5);
dTowFrac = dTowGPS - (double) dTowInt;
time=gpst2time(week, dTowInt*0.001);
/* time tag adjustment */
if (tadj>0.0) {
tn=time2gpst(time,&week)/tadj;
toff=(tn-floor(tn+0.5))*tadj;
time=timeadd(time,-toff);
}
/* check time tag jump */
if (raw->time.time&&fabs(timediff(time,raw->time))>86400.0) {
time2str(time,tstr,3);
trace(2,"nvs xf5raw time tag jump error: time=%s\n",tstr);
return 0;
}
if (fabs(timediff(time,raw->time))<=1e-3) {
time2str(time,tstr,3);
trace(2,"nvs xf5raw time tag duplicated: time=%s\n",tstr);
return 0;
}
for (i=0,p+=27;(i<nsat) && (n<MAXOBS); i++,p+=30) {
raw->obs.data[n].time = time;
sys = (U1(p)==1)?SYS_GLO:((U1(p)==2)?SYS_GPS:((U1(p)==4)?SYS_SBS:SYS_NONE));
prn = U1(p+1);
if (sys == SYS_SBS) prn += 120; /* Correct this */
if (!(sat=satno(sys,prn))) {
trace(2,"nvs xf5raw satellite number error: sys=%d prn=%d\n",sys,prn);
continue;
}
carrNo = I1(p+2);
L1 = R8(p+ 4);
P1 = R8(p+12);
D1 = R8(p+20);
/* check range error */
if (L1<-1E10||L1>1E10||P1<-1E10||P1>1E10||D1<-1E5||D1>1E5) {
trace(2,"nvs xf5raw obs range error: sat=%2d L1=%12.5e P1=%12.5e D1=%12.5e\n",
sat,L1,P1,D1);
continue;
}
raw->obs.data[n].SNR[0]=(unsigned char)(I1(p+3)*4.0+0.5);
if (sys==SYS_GLO) {
raw->obs.data[n].L[0] = L1 - toff*(FREQ1_GLO+DFRQ1_GLO*carrNo);
} else {
raw->obs.data[n].L[0] = L1 - toff*FREQ1;
}
raw->obs.data[n].P[0] = (P1-dTowFrac)*CLIGHT*0.001 - toff*CLIGHT; /* in ms, needs to be converted */
raw->obs.data[n].D[0] = (float)D1;
/* set LLI if meas flag 4 (carrier phase present) off -> on */
flag=U1(p+28);
raw->obs.data[n].LLI[0]=(flag&0x08)&&!(raw->halfc[sat-1][0]&0x08)?1:0;
raw->halfc[sat-1][0]=flag;
#if 0
if (raw->obs.data[n].SNR[0] > 160) {
time2str(time,tstr,3);
trace(2,"%s, obs.data[%d]: SNR=%.3f LLI=0x%02x\n", tstr,
n, (raw->obs.data[n].SNR[0])/4.0, U1(p+28) );
}
#endif
raw->obs.data[n].code[0] = CODE_L1C;
raw->obs.data[n].sat = sat;
for (j=1;j<NFREQ+NEXOBS;j++) {
raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
raw->obs.data[n].D[j]=0.0;
raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
raw->obs.data[n].code[j]=CODE_NONE;
}
n++;
}
raw->time=time;
raw->obs.n=n;
return 1;
}
/* post-processing positioning -------------------------------------------------
* post-processing positioning
* args : gtime_t ts I processing start time (ts.time==0: no limit)
* : gtime_t te I processing end time (te.time==0: no limit)
* double ti I processing interval (s) (0:all)
* double tu I processing unit time (s) (0:all)
* prcopt_t *popt I processing options
* solopt_t *sopt I solution options
* filopt_t *fopt I file options
* char **infile I input files (see below)
* int n I number of input files
* char *outfile I output file ("":stdout, see below)
* char *rov I rover id list (separated by " ")
* char *base I base station id list (separated by " ")
* return : status (0:ok,0>:error,1:aborted)
* notes : input files should contain observation data, navigation data, precise
* ephemeris/clock (optional), sbas log file (optional), ssr message
* log file (optional) and tec grid file (optional). only the first
* observation data file in the input files is recognized as the rover
* data.
*
* the type of an input file is recognized by the file extention as ]
* follows:
* .sp3,.SP3,.eph*,.EPH*: precise ephemeris (sp3c)
* .sbs,.SBS,.ems,.EMS : sbas message log files (rtklib or ems)
* .lex,.LEX : qzss lex message log files
* .rtcm3,.RTCM3 : ssr message log files (rtcm3)
* .*i,.*I : tec grid files (ionex)
* others : rinex obs, nav, gnav, hnav, qnav or clock
*
* inputs files can include wild-cards (*). if an file includes
* wild-cards, the wild-card expanded multiple files are used.
*
* inputs files can include keywords. if an file includes keywords,
* the keywords are replaced by date, time, rover id and base station
* id and multiple session analyses run. refer reppath() for the
* keywords.
*
* the output file can also include keywords. if the output file does
* not include keywords. the results of all multiple session analyses
* are output to a single output file.
*
* ssr corrections are valid only for forward estimation.
*-----------------------------------------------------------------------------*/
extern int postpos(gtime_t ts, gtime_t te, double ti, double tu,
const prcopt_t *popt, const solopt_t *sopt,
const filopt_t *fopt, char **infile, int n, char *outfile,
const char *rov, const char *base)
{
gtime_t tts,tte,ttte;
double tunit,tss;
int i,j,k,nf,stat=0,week,flag=1,index[MAXINFILE]={0};
char *ifile[MAXINFILE],ofile[1024],*ext;
trace(3,"postpos : ti=%.0f tu=%.0f n=%d outfile=%s\n",ti,tu,n,outfile);
/* open processing session */
if (!openses(popt,sopt,fopt,&navs,&pcvss,&pcvsr)) return -1;
if (ts.time!=0&&te.time!=0&&tu>=0.0) {
if (timediff(te,ts)<0.0) {
showmsg("error : no period");
closeses(&navs,&pcvss,&pcvsr);
return 0;
}
for (i=0;i<MAXINFILE;i++) {
if (!(ifile[i]=(char *)malloc(1024))) {
for (;i>=0;i--) free(ifile[i]);
closeses(&navs,&pcvss,&pcvsr);
return -1;
}
}
if (tu==0.0||tu>86400.0*MAXPRCDAYS) tu=86400.0*MAXPRCDAYS;
settspan(ts,te);
tunit=tu<86400.0?tu:86400.0;
tss=tunit*(int)floor(time2gpst(ts,&week)/tunit);
for (i=0;;i++) { /* for each periods */
tts=gpst2time(week,tss+i*tu);
tte=timeadd(tts,tu-DTTOL);
if (timediff(tts,te)>0.0) break;
if (timediff(tts,ts)<0.0) tts=ts;
if (timediff(tte,te)>0.0) tte=te;
strcpy(proc_rov ,"");
strcpy(proc_base,"");
if (checkbrk("reading : %s",time_str(tts,0))) {
stat=1;
break;
}
for (j=k=nf=0;j<n;j++) {
ext=strrchr(infile[j],'.');
if (ext&&(!strcmp(ext,".rtcm3")||!strcmp(ext,".RTCM3"))) {
strcpy(ifile[nf++],infile[j]);
}
else {
/* include next day precise ephemeris or rinex brdc nav */
ttte=tte;
if (ext&&(!strcmp(ext,".sp3")||!strcmp(ext,".SP3")||
!strcmp(ext,".eph")||!strcmp(ext,".EPH"))) {
ttte=timeadd(ttte,3600.0);
}
else if (strstr(infile[j],"brdc")) {
ttte=timeadd(ttte,7200.0);
}
nf+=reppaths(infile[j],ifile+nf,MAXINFILE-nf,tts,ttte,"","");
}
while (k<nf) index[k++]=j;
if (nf>=MAXINFILE) {
trace(2,"too many input files. trancated\n");
break;
}
}
if (!reppath(outfile,ofile,tts,"","")&&i>0) flag=0;
/* execute processing session */
stat=execses_b(tts,tte,ti,popt,sopt,fopt,flag,ifile,index,nf,ofile,
rov,base);
if (stat==1) break;
}
for (i=0;i<MAXINFILE;i++) free(ifile[i]);
}
else if (ts.time!=0) {
for (i=0;i<n&&i<MAXINFILE;i++) {
if (!(ifile[i]=(char *)malloc(1024))) {
for (;i>=0;i--) free(ifile[i]); return -1;
}
reppath(infile[i],ifile[i],ts,"","");
index[i]=i;
}
reppath(outfile,ofile,ts,"","");
/* execute processing session */
stat=execses_b(ts,te,ti,popt,sopt,fopt,1,ifile,index,n,ofile,rov,
base);
for (i=0;i<n&&i<MAXINFILE;i++) free(ifile[i]);
}
else {
for (i=0;i<n;i++) index[i]=i;
/* execute processing session */
stat=execses_b(ts,te,ti,popt,sopt,fopt,1,infile,index,n,outfile,rov,
base);
}
/* close processing session */
closeses(&navs,&pcvss,&pcvsr);
return stat;
}
int
main(int argc, char *argv[])
{
struct pollfd pfd[1];
char *ep, *cp;
int on = 1;
int send_time = 180; /* Default time, 180 seconds (3 minutes) */
struct sockaddr_in m_sin;
uid_t unpriv_uid;
gid_t unpriv_gid;
long tmp;
time_t delta = 0;
struct timeval next, now;
if (getuid())
errx(1, "not super user");
run_as(&unpriv_uid, &unpriv_gid);
argv++; argc--;
while (argc > 0 && *argv[0] == '-') {
if (strcmp(*argv, "-m") == 0) {
if (argc > 1 && *(argv + 1)[0] != '-') {
/* Argument has been given */
argv++, argc--;
multicast_mode = SCOPED_MULTICAST;
tmp = strtol(*argv, &ep, 10);
if (*ep != '\0' || tmp < INT_MIN || tmp > INT_MAX)
errx(1, "invalid ttl: %s", *argv);
multicast_scope = (int)tmp;
if (multicast_scope > MAX_MULTICAST_SCOPE)
errx(1, "ttl must not exceed %u",
MAX_MULTICAST_SCOPE);
}
else multicast_mode = PER_INTERFACE_MULTICAST;
} else if (strcmp(*argv, "-g") == 0) {
if (argc > 1 && *(argv + 1)[0] != '-') {
argv++, argc--;
send_time = (int)strtol(*argv, &ep, 10);
if (send_time <= 0)
errx(1, "time must be greater than 0");
if (ep[0] != '\0') {
if (ep[1] != '\0')
errx(1, "invalid argument: %s", *argv);
if (*ep == 'M' || *ep == 'm') {
/* Time in minutes. */
send_time *= 60;
} else
errx(1, "invalid argument: %s", *argv);
}
if (send_time > 180)
errx(1, "cannot be greater than 180 seconds (3 minutes)");
} else
errx(1, "missing argument");
} else if (strcmp(*argv, "-i") == 0)
insecure_mode = 1;
else if (strcmp(*argv, "-l") == 0)
quiet_mode = 1;
else if (strcmp(*argv, "-p") == 0)
iff_flag = 0;
else
usage();
argv++, argc--;
}
if (argc > 0)
usage();
#ifndef DEBUG
struct pidfh *pfh = NULL;
pfh = pidfile_open(NULL, 600, NULL);
daemon(1, 0);
pidfile_write(pfh);
#endif
signal(SIGHUP, hup);
openlog("rwhod", LOG_PID, LOG_DAEMON);
sp = getservbyname("who", "udp");
if (sp == NULL)
quit("udp/who: unknown service", WITHOUT_ERRNO);
if (chdir(_PATH_RWHODIR) < 0)
quit(_PATH_RWHODIR, WITH_ERRNO);
/*
* Establish host name as returned by system.
*/
if (gethostname(myname, sizeof(myname)) < 0)
quit("gethostname", WITH_ERRNO);
if ((cp = strchr(myname, '.')) != NULL)
*cp = '\0';
strlcpy(mywd.wd_hostname, myname, sizeof(mywd.wd_hostname));
utmpf = open(_PATH_UTMP, O_RDONLY|O_CREAT, 0644);
if (utmpf < 0)
quit(_PATH_UTMP, WITH_ERRNO);
getboottime();
if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
quit("socket", WITH_ERRNO);
if (setsockopt(s, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) < 0)
quit("setsockopt SO_BROADCAST", WITH_ERRNO);
memset(&m_sin, 0, sizeof(m_sin));
m_sin.sin_len = sizeof(m_sin);
m_sin.sin_family = AF_INET;
m_sin.sin_port = sp->s_port;
if (bind(s, (struct sockaddr *)&m_sin, sizeof(m_sin)) < 0)
quit("bind", WITH_ERRNO);
setgid(unpriv_gid);
setgroups(1, &unpriv_gid); /* XXX BOGUS groups[0] = egid */
setuid(unpriv_uid);
if (!configure(s))
exit(1);
if (!quiet_mode) {
send_host_information();
delta = send_time;
gettimeofday(&now, NULL);
timeadd(&now, delta, &next);
}
pfd[0].fd = s;
pfd[0].events = POLLIN;
for (;;) {
int n;
n = poll(pfd, 1, 1000);
if (onsighup) {
onsighup = 0;
getboottime();
}
if (n == 1)
handleread(s);
if (!quiet_mode) {
gettimeofday(&now, NULL);
if (now.tv_sec > next.tv_sec) {
send_host_information();
timeadd(&now, delta, &next);
}
}
}
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg, rtk_t *rtk)
{
double x[NX]={0},dx[NX],Q[NX*NX],sig;
int i,j,k,info,stat,nv,ns;
double rms = 0; //added by yuan;
double RMS = 0;
int nv_ = 0; double temp = 0; int kk = 0;
double v[(MAXOBS_+4)*1];double H[(MAXOBS_+4)*135];double var[MAXOBS_+4*1];
for (i=0;i<3;i++) x[i]=sol->rr[i];
for (i = 0; i < MAXITR; i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns,rtk);
if (nv<4) {
//sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
//added by yuan, ̽��ϴ�ֲ���н�ȷ�������������;
if ( (rtk->counter>=1) && (i>=1) ){
for (j = 0; j < nv;j++){
if (fabs(v[j]) != 0.0){
rms += v[j];
nv_++;
}
}
rms = rms / nv_;
for (j = 0; j < nv_; j++){
RMS += (v[j] - rms)*(v[j] - rms);
}
RMS = sqrt(RMS/nv_); //�в�ľ�����;
if (RMS>2.5){
temp = fabs(v[0] - rms);
for (j = 1; j < nv_;j++){
if (temp <= fabs(v[j] - rms)){
temp = fabs(v[j] - rms);
kk = j;
}
else{
continue;
}
}
opt->exsats[rtk->sat_[kk] - 1] = 1;
}
rms = 0; RMS = 0; nv_ = 0; temp = 0; kk = 0;
//nv = nv - 1; //����һ������;
}
if (nv < NX) {
//sprintf(msg, "lack of valid sats ns=%d", nv);
break;
}
/* weight by variance */
for (j = 0; j < nv; j++) {
sig = sqrt(var[j]);
v[j] /= sig;
for (k = 0; k < NX; k++) H[k + j*NX] /= sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
//sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++)
x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) {
sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;
}
return stat;
}
}
return 0;
}
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
for (i=0;i<3;i++) x[i]=sol->rr[i];
fprintf(output, " Begin coordinates calculation in single point positioning mode\n");
fprintf(output, " Current coordinates: X = %f, Y = %f, Z = %f\n", x[0], x[1], x[2]);
for (i=0;i<MAXITR;i++) {
fprintf(output, " Begin iteration %i:\n", i);
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
fprintf(output, " Begin least-square estimation:\n");
fprintf(output, " ");
for(j = 0; j < NX; j++)
fprintf(output, " X[%i] = %f,", j, dx[j]);
fprintf(output, "\n");
fprintf(output, " ");
for(j = 0; j < nv; j++)
fprintf(output, " v[%i] = %f,", j, v[j]);
fprintf(output, "\n");
for(j = 0; j < nv; j++)
{
fprintf(output, " ");
for(k = 0; k < NX; k++)
fprintf(output, " H[%i][%i] = %f,", j, k, H[k + j * NX]);
fprintf(output, "\n");
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
fprintf(output, " End least-square estimation\n");
fprintf(output, " Results of least square estimation: dx1 = %f, dx2 = %f, dx3 = %f, dx4 = %f, dx5 = %f, dx6 = %f, dx7 = %f\n", dx[0], dx[1], dx[2], dx[3], dx[4], dx[5], dx[6]);
for (j=0;j<NX;j++) x[j]+=dx[j];
fprintf(output, " New state of vector X: X[0] = %f, X[1] = %f, X[2] = %f, X[3] = %f, X[4] = %f, X[5] = %f, X[6] = %f\n", x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
fprintf(output, " norm(dx) = %f\n", norm(dx, NX));
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;
sol->age=sol->ratio=0.0;
/* validate solution */
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
fprintf(output, " End of coordinates calculation\n");
return stat;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}
