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