Beispiel #1
0
//---------------------------------------------------------------------------
TPoint __fastcall TMapDialog::PosToPoint(const double *pos)
{
	double sc[]={
		0.01,0.02,0.05,0.1,0.2,0.5,1,2,5,10,20,50,100,200,500,1000,2000,5000,10000
	};
	double rr[3],posr[3],enu[3],fact=40.0/sc[Scale];
	TPoint p;
	ecef2pos(RefPos,posr);
	for (int i=0;i<3;i++) rr[i]=pos[i]-RefPos[i];
	ecef2enu(posr,rr,enu);
	p.x=Plot->Width /2+(int)(enu[0]*fact+0.5);
	p.y=Plot->Height/2-(int)(enu[1]*fact+0.5);
	if (BtnCenter->Down) {
		for (int i=0;i<3;i++) rr[i]=CurrentPos[i]-RefPos[i];
		ecef2enu(posr,rr,enu);
		p.x-=(int)(enu[0]*fact+0.5);
		p.y+=(int)(enu[1]*fact+0.5);
	}
	else if (norm(CentPos,3)>0.0) {
		for (int i=0;i<3;i++) rr[i]=CentPos[i]-RefPos[i];
		ecef2enu(posr,rr,enu);
		p.x-=(int)(enu[0]*fact+0.5);
		p.y+=(int)(enu[1]*fact+0.5);
	}
	return p;
}
Beispiel #2
0
/* ecef2enu(), enu2ecef() */
void utest3(void)
{
    double pos1[]={ 0.000*D2R,  0.000*D2R,0.0};
    double pos2[]={35.000*D2R,140.000*D2R,0.0};
    double r1[]={1.0,0.0,0.0};
    double r2[]={0.0,1.0,0.0};
    double r3[]={0.0,0.0,1.0};
    double r4[]={0.3,0.4,0.5};
    double e[3],r[3];
    ecef2enu(pos1,r1,e);
        assert(e[0]==0.0&&e[1]==0.0&&e[2]==1.0);
    ecef2enu(pos1,r2,e);
        assert(e[0]==1.0&&e[1]==0.0&&e[2]==0.0);
    ecef2enu(pos1,r3,e);
        assert(e[0]==0.0&&e[1]==1.0&&e[2]==0.0);
    ecef2enu(pos2,r4,e);
        assert(fabs(e[0]+0.499254)<1E-6&&
               fabs(e[1]-0.393916)<1E-6&&
               fabs(e[2]-0.309152)<1E-6);
    enu2ecef(pos2,e,r);
        assert(fabs(r[0]-r4[0])<1E-6&&
               fabs(r[1]-r4[1])<1E-6&&
               fabs(r[2]-r4[2])<1E-6);

    printf("%s utset3 : OK\n",__FILE__);
}
Beispiel #3
0
/* output solution status for PPP --------------------------------------------*/
extern void pppoutsolstat(rtk_t *rtk, int level, FILE *fp)
{
    ssat_t *ssat;
    double tow,pos[3],vel[3],acc[3];
    int i,j,week,nfreq=1;
    char id[32];
    
    if (level<=0||!fp) return;
    
    trace(3,"pppoutsolstat:\n");
    
    tow=time2gpst(rtk->sol.time,&week);
    
    /* receiver position */
    fprintf(fp,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
            rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],0.0,0.0,0.0);
    
    /* receiver velocity and acceleration */
    if (rtk->opt.dynamics) {
        ecef2pos(rtk->sol.rr,pos);
        ecef2enu(pos,rtk->x+3,vel);
        ecef2enu(pos,rtk->x+6,acc);
        fprintf(fp,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
                week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2],
                0.0,0.0,0.0,0.0,0.0,0.0);
    }
    /* receiver clocks */
    i=IC(0,&rtk->opt);
    fprintf(fp,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
            week,tow,rtk->sol.stat,1,rtk->x[i]*1E9/CLIGHT,rtk->x[i+1]*1E9/CLIGHT,
            0.0,0.0);
    
    /* tropospheric parameters */
    if (rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG) {
        i=IT(&rtk->opt);
        fprintf(fp,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
                1,rtk->x[i],0.0);
    }
    if (rtk->opt.tropopt==TROPOPT_ESTG) {
        i=IT(&rtk->opt);
        fprintf(fp,"$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n",week,tow,
                rtk->sol.stat,1,rtk->x[i+1],rtk->x[i+2],0.0,0.0);
    }
    if (rtk->sol.stat==SOLQ_NONE||level<=1) return;
    
    /* residuals and status */
    for (i=0;i<MAXSAT;i++) {
        ssat=rtk->ssat+i;
        if (!ssat->vs) continue;
        satno2id(i+1,id);
        for (j=0;j<nfreq;j++) {
            fprintf(fp,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
                    week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
                    ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr[j]*0.25,
                    ssat->fix[j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j],
                    ssat->slipc[j],ssat->rejc[j]);
        }
    }
}
Beispiel #4
0
// transform solution to xyz-terms ------------------------------------------
void __fastcall TPlot::PosToXyz(gtime_t time, const double *rr, int type,
                                double *xyz)
{
    double opos[3],pos[3],r[3],enu[3];
    int i;
    
    trace(4,"SolToXyz:\n");
    
    if (type==0) { // xyz
        for (i=0;i<3;i++) {
            opos[i]=OPos[i];
            if (time.time==0.0||OEpoch.time==0.0) continue;
            opos[i]+=OVel[i]*timediff(time,OEpoch);
        }
        for (i=0;i<3;i++) r[i]=rr[i]-opos[i];
        ecef2pos(opos,pos);
        ecef2enu(pos,r,enu);
        xyz[0]=enu[0];
        xyz[1]=enu[1];
        xyz[2]=enu[2];
    }
    else { // enu
        xyz[0]=rr[0];
        xyz[1]=rr[1];
        xyz[2]=rr[2];
    }
}
Beispiel #5
0
static void RtkCommon__ecef2enu(JNIEnv* env, jclass clazz, jdouble j_lat,
      jdouble j_lon, jdoubleArray j_r, jdoubleArray j_e)
{
   double pos[2] = {j_lat, j_lon};
   double r[3];
   double e[3];

   (*env)->GetDoubleArrayRegion(env, j_r, 0, 3, r);
   if ((*env)->ExceptionOccurred(env) != NULL)
      return;
   ecef2enu(pos, r, e);
   (*env)->SetDoubleArrayRegion(env, j_e, 0, 3, e);
}
Beispiel #6
0
/* set rtcm antenna and receiver info to options -----------------------------*/
static void rtcm2opt(const rtcm_t *rtcm, rnxopt_t *opt)
{
    double pos[3],enu[3];
    int i;
    
    trace(3,"rtcm2opt:\n");
    
    /* comment */
    sprintf(opt->comment[1]+strlen(opt->comment[1]),", station ID: %d",
            rtcm->staid);
    
    /* receiver and antenna info */
    if (!*opt->rec[0]&&!*opt->rec[1]&&!*opt->rec[2]) {
        strcpy(opt->rec[0],rtcm->sta.recsno);
        strcpy(opt->rec[1],rtcm->sta.rectype);
        strcpy(opt->rec[2],rtcm->sta.recver);
    }
    if (!*opt->ant[0]&&!*opt->ant[1]&&!*opt->ant[2]) {
        strcpy(opt->ant[0],rtcm->sta.antsno);
        strcpy(opt->ant[1],rtcm->sta.antdes);
        if (rtcm->sta.antsetup) {
            sprintf(opt->ant[2],"%d",rtcm->sta.antsetup);
        }
        else *opt->ant[2]='\0';
    }
    /* antenna approx position */
    if (!opt->autopos&&norm(rtcm->sta.pos,3)>0.0) {
        for (i=0;i<3;i++) opt->apppos[i]=rtcm->sta.pos[i];
    }
    /* antenna delta */
    if (norm(rtcm->sta.del,3)>0.0) {
        if (!rtcm->sta.deltype&&norm(rtcm->sta.del,3)>0.0) { /* enu */
            opt->antdel[0]=rtcm->sta.del[2]; /* h */
            opt->antdel[1]=rtcm->sta.del[0]; /* e */
            opt->antdel[2]=rtcm->sta.del[1]; /* n */
        }
        else if (norm(rtcm->sta.pos,3)>0.0) { /* xyz */
            ecef2pos(rtcm->sta.pos,pos);
            ecef2enu(pos,rtcm->sta.del,enu);
            opt->antdel[0]=enu[2]; /* h */
            opt->antdel[1]=enu[0]; /* e */
            opt->antdel[2]=enu[1]; /* n */
        }
    }
    else {
        opt->antdel[0]=rtcm->sta.hgt;
        opt->antdel[0]=0.0;
        opt->antdel[0]=0.0;
    }
}
Beispiel #7
0
//---------------------------------------------------------------------------
TPoint __fastcall TMapDialog::PosToGraphP(const double *pos,
	const double *ref, int index, int npos, TRect rect)
{
	double sc[]={
		0.01,0.02,0.05,0.1,0.2,0.5,1,2,5,10,20,50,100,200,500,1000,2000,5000,10000
	};
	double rr[3],posr[3],enu[3],fact=40.0/sc[Scale];
	TPoint p;
	ecef2pos(ref,posr);
	for (int i=0;i<3;i++) rr[i]=pos[i]-ref[i];
	ecef2enu(posr,rr,enu);
	p.x=rect.Left+(int)((rect.Right-rect.Left)*index/(npos-1.0)+0.5);
	p.y=(rect.Top+rect.Bottom)/2-(int)(enu[2]*fact+0.5);
	return p;
}
Beispiel #8
0
/* set antenna parameters ----------------------------------------------------*/
static void setpcv(gtime_t time, prcopt_t *popt, nav_t *nav, const pcvs_t *pcvs,
                   const pcvs_t *pcvr, const sta_t *sta)
{
    pcv_t *pcv;
    double pos[3],del[3];
    int i,j,mode=PMODE_DGPS<=popt->mode&&popt->mode<=PMODE_FIXED;
    char id[64];
    
    /* set satellite antenna parameters */
    for (i=0;i<MAXSAT;i++) {
        if (!(satsys(i+1,NULL)&popt->navsys)) continue;
        if (!(pcv=searchpcv(i+1,"",time,pcvs))) {
            satno2id(i+1,id);
            trace(2,"no satellite antenna pcv: %s\n",id);
            continue;
        }
        nav->pcvs[i]=*pcv;
    }
    for (i=0;i<(mode?2:1);i++) {
        if (!strcmp(popt->anttype[i],"*")) { /* set by station parameters */
            strcpy(popt->anttype[i],sta[i].antdes);
            if (sta[i].deltype==1) { /* xyz */
                if (norm(sta[i].pos,3)>0.0) {
                    ecef2pos(sta[i].pos,pos);
                    ecef2enu(pos,sta[i].del,del);
                    for (j=0;j<3;j++) popt->antdel[i][j]=del[j];
                }
            }
            else { /* enu */
                for (j=0;j<3;j++) popt->antdel[i][j]=stas[i].del[j];
            }
        }
        if (!(pcv=searchpcv(0,popt->anttype[i],time,pcvr))) {
            trace(2,"no receiver antenna pcv: %s\n",popt->anttype[i]);
            *popt->anttype[i]='\0';
            continue;
        }
        strcpy(popt->anttype[i],pcv->type);
        popt->pcvr[i]=*pcv;
    }
}
Beispiel #9
0
/* set rinex station and receiver info to options ----------------------------*/
static void rnx2opt(const rnxctr_t *rnx, rnxopt_t *opt)
{
    double pos[3],enu[3];
    int i;
    
    trace(3,"rnx2opt:\n");
    
    /* receiver and antenna info */
    if (!*opt->marker&&!*opt->markerno) {
        strcpy(opt->marker,rnx->sta.name);
        strcpy(opt->markerno,rnx->sta.marker);
    }
    if (!*opt->rec[0]&&!*opt->rec[1]&&!*opt->rec[2]) {
        strcpy(opt->rec[0],rnx->sta.recsno);
        strcpy(opt->rec[1],rnx->sta.rectype);
        strcpy(opt->rec[2],rnx->sta.recver);
    }
    if (!*opt->ant[0]&&!*opt->ant[1]) {
        strcpy(opt->ant[0],rnx->sta.antsno);
        strcpy(opt->ant[1],rnx->sta.antdes);
    }
    /* antenna approx position */
    if (!opt->autopos&&norm(rnx->sta.pos,3)>0.0) {
        for (i=0;i<3;i++) opt->apppos[i]=rnx->sta.pos[i];
    }
    /* antenna delta */
    if (norm(rnx->sta.del,3)>0.0) {
        if (!rnx->sta.deltype) { /* enu */
            opt->antdel[0]=rnx->sta.del[2]; /* h */
            opt->antdel[1]=rnx->sta.del[0]; /* e */
            opt->antdel[2]=rnx->sta.del[1]; /* n */
        }
        else if (norm(opt->apppos,3)>0.0) { /* xyz */
            ecef2pos(opt->apppos,pos);
            ecef2enu(pos,rnx->sta.del,enu);
            opt->antdel[0]=enu[2];
            opt->antdel[1]=enu[0];
            opt->antdel[2]=enu[1];
        }
    }
}
Beispiel #10
0
//---------------------------------------------------------------------------
void __fastcall TMapDialog::UpdateMap(const double *sol, const double *ref,
	const double *vel, const int *stat, int psol, int psols, int psole,
	int nsol, AnsiString *solstr, int currentstat)
{
	TColor color[]={clWhite,clGreen,CLORANGE,clFuchsia,clBlue,clRed,clTeal};
	TCanvas *c=Plot->Canvas;
	AnsiString s;
	TPoint p0;
	double pos[3],posr[3],rr[3],enu[3],enuv[3],dr[3],dir,*pntpos;
	int gint[]={20,20,16,20,20,16,20,20,16,20,20,16,20,20,16,20,20,16,20};
	int ng[]={10,5,5,10,5,5,10,5,5,10,5,5,10,5,5,10,5,5,10};
	TColor colsol=color[stat[psol]];
	
	TRect r(0,0,Plot->Width,Plot->Height);
	c->Brush->Style=bsSolid;
	c->Brush->Color=clWhite;
	c->FillRect(r);
	
	for (int i=0;i<3;i++) {
		CurrentPos[i]=sol[i+psol*3];
	}
	if (norm(RefPos,3)<=0.0) {
		if (ref&&norm(ref+psol,3)>0.0) {
			RefName="Base Station";
			for (int i=0;i<3;i++) RefPos[i]=ref[i+psol*3];
			UpdatePntList();
		}
		else  {
			for (int i=0;i<3;i++) RefPos[i]=CurrentPos[i];
		}
	}
	ecef2pos(RefPos,posr);
	ecef2enu(posr,vel+psol*3,enuv);
	
	DrawGrid(PosToPoint(RefPos),gint[Scale],ng[Scale],CLLGRAY,clSilver);
	
	if (BtnPnt->Down) {
		for (int i=0;i<MainForm->NMapPnt+1;i++) {
			DrawPoint(i==0?RefPos:MainForm->PntPos[i-1],PntList->Items->Strings[i],
					  i==PntIndex?clGray:clSilver);
		}
	}
	if (BtnTrack->Down) {
		TPoint *p  =new TPoint[nsol];
		TColor *col=new TColor[nsol];
		int n=0;
		for (int i=psols;i!=psole;) {
			p[n]=PosToPoint(sol+i*3);
			col[n++]=color[stat[i]];
			if (++i>=nsol) i=0;
		}
		c->Pen->Color=CLLGRAY;
		c->Pen->Style=psSolid;
		c->Polyline(p,n-1);
		for (int i=0;i<n;i++) {
			DrawCircle(p[i],HISSIZE,col[i],col[i]);
		}
		delete [] p;
		delete [] col;
	}
	p0=PosToPoint(CurrentPos);
	c->Pen->Color=clBlack;
	c->MoveTo(p0.x-SOLSIZE/2-4,p0.y); c->LineTo(p0.x+SOLSIZE/2+4,p0.y);
	c->MoveTo(p0.x,p0.y-SOLSIZE/2-4); c->LineTo(p0.x,p0.y+SOLSIZE/2+4);
	DrawCircle(p0,SOLSIZE+4,clBlack,clWhite);
	DrawCircle(p0,SOLSIZE,clBlack,color[currentstat]);
	
	DrawVel(enuv);
	DrawScale();
	
	c->Brush->Style=bsSolid;
	c->Brush->Color=clWhite;
	DrawText(6,1,solstr[0],currentstat?colsol:clGray,0);
	TSize off=c->TextExtent(solstr[2]);
	DrawText(Plot->Width-off.cx-6,1,solstr[2],clGray,0);
	DrawText(50,1,solstr[1],clBlack,0);
	
	if (BtnPnt->Down) {
		pntpos=PntIndex>0?MainForm->PntPos[PntIndex-1]:RefPos;
		for (int i=0;i<3;i++) {
			rr[i]=pntpos[i]-CurrentPos[i];
		}
		ecef2pos(pntpos,pos);
		ecef2enu(posr,rr,enu);
		dir=norm(enu,2)<1E-9?0.0:atan2(enu[0],enu[1])*R2D;
		if (dir<0.0) dir+=360.0;
		s.sprintf("To %s: Distance %.3fm Direction %.1f%s",
			PntList->Items->Strings[PntIndex].c_str(),norm(enu,2),dir,CHARDEG);
		int y=Plot->Height-off.cy/2-2;
		if (BtnGraph->Down) y-=GRPHEIGHT+2;
		DrawText(Plot->Width/2,y,s,clGray,1);
	}
	if (BtnGraph->Down) {
		DrawVertGraph(sol,stat,psol,psols,psole, nsol,currentstat);
	}
	Disp->Invalidate();
}