/* 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__);
}
// 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);
}
//---------------------------------------------------------------------------
void __fastcall TMapDialog::DispMouseMove(TObject *Sender, TShiftState Shift,
int X, int Y)
{
double sc[]={
0.01,0.02,0.05,0.1,0.2,0.5,1,2,5,10,20,50,100,200,500,1000,2000,5000,10000
};
double rr[3],posr[3],enu[3],fact=40.0/sc[Scale];
if (!Drag) return;
enu[0]=(X0-X)/fact;
enu[1]=(Y-Y0)/fact;
enu[2]=0.0;
ecef2pos(RefPos,posr);
enu2ecef(posr,enu,rr);
for (int i=0;i<3;i++) CentPos[i]=CentPos0[i]+rr[i];
MainForm->UpdateMap();
}
// get center position of plot ----------------------------------------------
int __fastcall TPlot::GetCenterPos(double *rr)
{
double xc,yc,pos[3],enu[3]={0},dr[3];
int i;
trace(3,"GetCenterPos\n");
if (PLOT_OBS<=PlotType&&PlotType<=PLOT_DOP&&PlotType!=PLOT_TRK) return 0;
if (norm(OPos,3)<=0.0) return 0;
GraphT->GetCent(xc,yc);
ecef2pos(OPos,pos);
enu[0]=xc;
enu[1]=yc;
enu2ecef(pos,enu,dr);
for (int i=0;i<3;i++) rr[i]=OPos[i]+dr[i];
return 1;
}
/* antenna phase center position ---------------------------------------------*/
static int antpos(prcopt_t *opt, int rcvno, const obs_t *obs, const nav_t *nav,
const sta_t *sta, const char *posfile)
{
double *rr=rcvno==1?opt->ru:opt->rb,del[3],pos[3],dr[3]={0};
int i,postype=rcvno==1?opt->rovpos:opt->refpos;
char *name;
trace(3,"antpos : rcvno=%d\n",rcvno);
if (postype==1) { /* average of single position */
if (!avepos(rr,rcvno,obs,nav,opt)) {
showmsg("error : station pos computation");
return 0;
}
}
else if (postype==2) { /* read from position file */
name=stas[rcvno==1?0:1].name;
if (!getstapos(posfile,name,rr)) {
showmsg("error : no position of %s in %s",name,posfile);
return 0;
}
}
else if (postype==3) { /* get from rinex header */
if (norm(stas[rcvno==1?0:1].pos,3)<=0.0) {
showmsg("error : no position in rinex header");
trace(1,"no position position in rinex header\n");
return 0;
}
/* antenna delta */
if (stas[rcvno==1?0:1].deltype==0) { /* enu */
for (i=0;i<3;i++) del[i]=stas[rcvno==1?0:1].del[i];
del[2]+=stas[rcvno==1?0:1].hgt;
ecef2pos(stas[rcvno==1?0:1].pos,pos);
enu2ecef(pos,del,dr);
}
else { /* xyz */
for (i=0;i<3;i++) dr[i]=stas[rcvno==1?0:1].del[i];
}
for (i=0;i<3;i++) rr[i]=stas[rcvno==1?0:1].pos[i]+dr[i];
}
return 1;
}
/* update rtk server struct --------------------------------------------------*/
static void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
sbsmsg_t *sbsmsg, int index, int iobs)
{
eph_t *eph1,*eph2,*eph3;
geph_t *geph1,*geph2,*geph3;
gtime_t tof;
double pos[3],del[3],dr[3];
int i,n=0,prn=0,sbssat=svr->rtk.opt.sbassatsel,sys=0,iode=0;
INIT_ZERO(del);
INIT_ZERO(dr);
INIT_ZERO(pos);
INIT_ZERO(prn);
tracet(4,"updatesvr: ret=%d sat=%2d index=%d\n",ret,sat,index);
if (ret==1) { /* observation data */
if (iobs<MAXOBSBUF) {
for (i=0;i<obs->n;i++) {
if (svr->rtk.opt.exsats[obs->data[i].sat-1]==1||
!(satsys(obs->data[i].sat,NULL)&svr->rtk.opt.navsys)) continue;
svr->obs[index][iobs].data[n]=obs->data[i];
svr->obs[index][iobs].data[n++].rcv=index+1;
}
svr->obs[index][iobs].n=n;
sortobs(&svr->obs[index][iobs]);
}
svr->nmsg[index][0]++;
}
else if (ret==2) { /* ephemeris */
if (satsys(sat,&prn)!=SYS_GLO) {
if (!svr->navsel||svr->navsel==index+1) {
eph1=nav->eph+sat-1;
eph2=svr->nav.eph+sat-1;
eph3=svr->nav.eph+sat-1+MAXSAT;
if (eph2->ttr.time==0||
(eph1->iode!=eph3->iode&&eph1->iode!=eph2->iode)||
(timediff(eph1->toe,eph3->toe)!=0.0&&
timediff(eph1->toe,eph2->toe)!=0.0)) {
*eph3=*eph2;
*eph2=*eph1;
updatenav(&svr->nav);
}
}
svr->nmsg[index][1]++;
}
else {
if (!svr->navsel||svr->navsel==index+1) {
geph1=nav->geph+prn-1;
geph2=svr->nav.geph+prn-1;
geph3=svr->nav.geph+prn-1+MAXPRNGLO;
if (geph2->tof.time==0||
(geph1->iode!=geph3->iode&&geph1->iode!=geph2->iode)) {
*geph3=*geph2;
*geph2=*geph1;
updatenav(&svr->nav);
updatefcn(svr);
}
}
svr->nmsg[index][6]++;
}
}
else if (ret==3) { /* sbas message */
if (sbsmsg&&(sbssat==sbsmsg->prn||sbssat==0)) {
if (svr->nsbs<MAXSBSMSG) {
svr->sbsmsg[svr->nsbs++]=*sbsmsg;
}
else {
for (i=0;i<MAXSBSMSG-1;i++) svr->sbsmsg[i]=svr->sbsmsg[i+1];
svr->sbsmsg[i]=*sbsmsg;
}
sbsupdatecorr(sbsmsg,&svr->nav);
}
svr->nmsg[index][3]++;
}
else if (ret==9) { /* ion/utc parameters */
if (svr->navsel==index||svr->navsel>=3) {
for (i=0;i<8;i++) svr->nav.ion_gps[i]=nav->ion_gps[i];
for (i=0;i<4;i++) svr->nav.utc_gps[i]=nav->utc_gps[i];
for (i=0;i<4;i++) svr->nav.ion_gal[i]=nav->ion_gal[i];
for (i=0;i<4;i++) svr->nav.utc_gal[i]=nav->utc_gal[i];
for (i=0;i<8;i++) svr->nav.ion_qzs[i]=nav->ion_qzs[i];
for (i=0;i<4;i++) svr->nav.utc_qzs[i]=nav->utc_qzs[i];
svr->nav.leaps=nav->leaps;
}
svr->nmsg[index][2]++;
}
else if (ret==5) { /* antenna postion parameters */
if (svr->rtk.opt.refpos==4&&index==1) {
for (i=0;i<3;i++) {
svr->rtk.rb[i]=svr->rtcm[1].sta.pos[i];
}
/* antenna delta */
ecef2pos(svr->rtk.rb,pos);
if (svr->rtcm[1].sta.deltype) { /* xyz */
del[2]=svr->rtcm[1].sta.hgt;
enu2ecef(pos,del,dr);
for (i=0;i<3;i++) {
svr->rtk.rb[i]+=svr->rtcm[1].sta.del[i]+dr[i];
}
}
else { /* enu */
enu2ecef(pos,svr->rtcm[1].sta.del,dr);
for (i=0;i<3;i++) {
svr->rtk.rb[i]+=dr[i];
}
}
}
svr->nmsg[index][4]++;
}
else if (ret==7) { /* dgps correction */
svr->nmsg[index][5]++;
}
else if (ret==10) { /* ssr message */
for (i=0;i<MAXSAT;i++) {
if (!svr->rtcm[index].ssr[i].update) continue;
svr->rtcm[index].ssr[i].update=0;
iode=svr->rtcm[index].ssr[i].iode;
sys=satsys(i+1,&prn);
/* check corresponding ephemeris exists */
if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS) {
if (svr->nav.eph[i ].iode!=iode&&
svr->nav.eph[i+MAXSAT].iode!=iode) {
continue;
}
}
else if (sys==SYS_GLO) {
if (svr->nav.geph[prn-1 ].iode!=iode&&
svr->nav.geph[prn-1+MAXPRNGLO].iode!=iode) {
continue;
}
}
svr->nav.ssr[i]=svr->rtcm[index].ssr[i];
}
svr->nmsg[index][7]++;
}
else if (ret==31) { /* lex message */
lexupdatecorr(&svr->raw[index].lexmsg,&svr->nav,&tof);
svr->nmsg[index][8]++;
}
else if (ret==-1) { /* error */
svr->nmsg[index][9]++;
}
}
