//---------------------------------------------------------------------------
void __fastcall TOptDialog::SetPos(int type, TEdit **edit, double *pos)
{
AnsiString s;
double p[3],dms1[3],dms2[3],s1,s2;
if (type==1) { /* lat/lon/height dms/m */
ecef2pos(pos,p); s1=p[0]<0?-1:1; s2=p[1]<0?-1:1;
p[0]=fabs(p[0])*R2D+1E-12; p[1]=fabs(p[1])*R2D+1E-12;
dms1[0]=floor(p[0]); p[0]=(p[0]-dms1[0])*60.0;
dms1[1]=floor(p[0]); dms1[2]=(p[0]-dms1[1])*60.0;
dms2[0]=floor(p[1]); p[1]=(p[1]-dms2[0])*60.0;
dms2[1]=floor(p[1]); dms2[2]=(p[1]-dms2[1])*60.0;
edit[0]->Text=s.sprintf("%.0f %02.0f %09.6f",s1*dms1[0],dms1[1],dms1[2]);
edit[1]->Text=s.sprintf("%.0f %02.0f %09.6f",s2*dms2[0],dms2[1],dms2[2]);
edit[2]->Text=s.sprintf("%.4f",p[2]);
}
else if (type==2) { /* x/y/z-ecef */
edit[0]->Text=s.sprintf("%.4f",pos[0]);
edit[1]->Text=s.sprintf("%.4f",pos[1]);
edit[2]->Text=s.sprintf("%.4f",pos[2]);
}
else {
ecef2pos(pos,p);
edit[0]->Text=s.sprintf("%.9f",p[0]*R2D);
edit[1]->Text=s.sprintf("%.9f",p[1]*R2D);
edit[2]->Text=s.sprintf("%.4f",p[2]);
}
}
// 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);
}
// transform covariance to xyz-terms ----------------------------------------
void __fastcall TPlot::CovToXyz(const double *rr, const float *qr, int type,
double *xyzs)
{
double pos[3],P[9],Q[9];
trace(4,"CovToXyz:\n");
if (type==0) { // xyz
ecef2pos(rr,pos);
P[0]=qr[0];
P[4]=qr[1];
P[8]=qr[2];
P[1]=P[3]=qr[3];
P[5]=P[7]=qr[4];
P[2]=P[6]=qr[5];
covenu(pos,P,Q);
xyzs[0]=Q[0];
xyzs[1]=Q[4];
xyzs[2]=Q[8];
xyzs[3]=Q[1];
}
else { // enu
xyzs[0]=qr[0];
xyzs[1]=qr[1];
xyzs[2]=qr[2];
xyzs[3]=qr[3];
}
}
// 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];
}
}
/* estimate ionosphere -------------------------------------------------------*/
static int est_iono(obs_t *obs, nav_t *nav, double *rr, FILE *fp)
{
ssat_t ssat[MAXSAT]={{0}};
double tt,*x,*P,*v,*H,*R,pos[3],azel[MAXOBS*2];
int i,n,info,nx=NX,nv=MAXSAT*2;
x=zeros(nx,1); P=zeros(nx,nx); v=mat(nv,1); H=mat(nx,nv); R=mat(nv,nv);
/* receiver position */
ecef2pos(rr,pos);
for (i=0;i<obs->n;i++) {
for (n=1;i+n<obs->n;n++) {
if (timediff(obs[i].time,obs->data[i+n].time)>1E-3) break;
}
/* satellite azimuth/elevation angle */
sat_azel(obs+i,n,nav,pos,azel);
/* time update of parameters */
ud_state(obs+i,n,azel,x,P,nx,ssat);
/* ionosphere residuals */
if ((nv=res_iono(obs+i,n,azel,x,nx,v,H,R))<=0) break;
/* filter */
if ((info=rtklib_filter(x,P,H,v,R,nx,nv))) break;
/* output ionopshere parameters */
out_iono(obs[i].time,x,P,nx,fp);
}
free(x); free(P); free(v); free(H); free(R);
return 1;
}
//---------------------------------------------------------------------------
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;
}
/*
static int zdres(int base, const obsd_t *obs, int n, const double *rs,
const double *dts, const int *svh, const nav_t *nav,
const double *rr, const prcopt_t *opt, double *y,
double *e, double *azel)
Description: zero differenced residual
Params: base I 1:base, 0: rover
obs I
n I number of obs data
rs I sat positions/velocities
dts I sat clock bias
svh I
nav I
rr I receiver position
y O zero diff residuals
e O
azel O {az/el}
Return: 0 ok, !0 error
*/
static int zdres(int base, const obsd_t *obs, int n, const double *rs,
const double *dts, const int *svh, const nav_t *nav,
const double *rr, const prcopt_t *opt, int index, double *y,
double *e, double *azel)
{
double r,rr_[3],pos[3];
double zhd, zazel[]= {0.0,90.0*D2R}; //tai sao init zazel = {0,90}
int i;
for (i=0; i<2*n; i++) y[i]=0.0;
if (sos3(rr)<=0.0) return -1;//no receiver pos
for (i=0; i<3; i++) rr_[i]=rr[i];
ecef2pos(rr_,pos);
for (i=0; i<n; i++)
{
if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue;
if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue;
//satellite clock-bias
r -= CLIGHT*dts[i*2];
/* troposphere delay model (hydrostatic) */
zhd=tropmodel(obs[0].time,pos,zazel,0.0);//tai sao chon rel_humi = 0???????????
r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd;
/* undifferenced phase/code residual for satellite */
zdres_sat(base,r,obs+i,nav,azel+i*2,opt,y+i*2);
}
return 0;
}
/* temporal update of tropospheric parameters --------------------------------*/
static void udtrop_ppp(rtk_t *rtk)
{
double pos[3],azel[]={0.0,PI/2.0},ztd,var;
int i=IT(&rtk->opt),j;
trace(3,"udtrop_ppp:\n");
if (rtk->x[i]==0.0) {
ecef2pos(rtk->sol.rr,pos);
ztd=sbstropcorr(rtk->sol.time,pos,azel,&var);
initx(rtk,ztd,var,i);
if (rtk->opt.tropopt>=TROPOPT_ESTG) {
for (j=0;j<2;j++) initx(rtk,1E-6,VAR_GRA,++i);
}
}
else {
rtk->P[i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2])*fabs(rtk->tt);
if (rtk->opt.tropopt>=TROPOPT_ESTG) {
for (j=0;j<2;j++) {
rtk->P[++i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2]*0.1)*fabs(rtk->tt);
}
}
}
}
/* output reference position -------------------------------------------------*/
static void outrpos(FILE *fp, const double *r, const solopt_t *opt)
{
double pos[3],dms1[3],dms2[3];
const char *sep=opt->sep;
trace(3,"outrpos :\n");
if (opt->posf==SOLF_LLH||opt->posf==SOLF_ENU) {
ecef2pos(r,pos);
if (opt->degf) {
deg2dms(pos[0]*R2D,dms1);
deg2dms(pos[1]*R2D,dms2);
fprintf(fp,"%3.0f%s%02.0f%s%08.5f%s%4.0f%s%02.0f%s%08.5f%s%10.4f",
dms1[0],sep,dms1[1],sep,dms1[2],sep,dms2[0],sep,dms2[1],
sep,dms2[2],sep,pos[2]);
}
else {
fprintf(fp,"%13.9f%s%14.9f%s%10.4f",pos[0]*R2D,sep,pos[1]*R2D,
sep,pos[2]);
}
}
else if (opt->posf==SOLF_XYZ) {
fprintf(fp,"%14.4f%s%14.4f%s%14.4f",r[0],sep,r[1],sep,r[2]);
}
}
//---------------------------------------------------------------------------
void __fastcall TPntDialog::FormShow(TObject *Sender)
{
TGridRect r={0};
AnsiString s;
double pos[3];
int width[]={90,90,80,90};
FontScale=Screen->PixelsPerInch;
for (int i=0;i<4;i++) {
PntList->ColWidths[i]=width[i]*FontScale/96;
}
PntList->DefaultRowHeight=17*FontScale/96;
for (int i=0;i<PntList->RowCount;i++) {
if (i<Plot->NWayPnt) {
ecef2pos(Plot->PntPos[i],pos);
PntList->Cells[0][i]=s.sprintf("%.9f",pos[0]*R2D);
PntList->Cells[1][i]=s.sprintf("%.9f",pos[1]*R2D);
PntList->Cells[2][i]=s.sprintf("%.4f",pos[2]);
PntList->Cells[3][i]=Plot->PntName[i];
}
else {
for (int j=0;j<PntList->ColCount;j++) PntList->Cells[j][i]="";
}
}
r.Top=r.Bottom=PntList->RowCount;
PntList->Selection=r;
}
/* 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]);
}
}
}
/* ecef2pos() */
void utest1(void)
{
double r1[]={ 0.0, 0.0, 0.0};
double r2[]={10000000.0, 0.0, 0.0};
double r3[]={ 0.0,10000000.0, 0.0};
double r4[]={ 0.0, 0.0, 10000000.0};
double r5[]={ 0.0, 0.0,-10000000.0};
double r6[]={-3.5173197701E+06,4.1316679161E+06, 3.3412651227E+06};
double r7[]={-3.5173197701E+06,4.1316679161E+06,-3.3412651227E+06};
double pos[3];
ecef2pos(r1,pos);
assert(pos[2]<0.0);
ecef2pos(r2,pos);
assert(pos[0]==0&&pos[1]==0&&pos[2]>0.0);
ecef2pos(r3,pos);
assert(pos[0]==0&&fabs(pos[1]-PI/2)<1E-6&&pos[2]>0.0);
ecef2pos(r4,pos);
assert(fabs(pos[0]-PI/2)<1E-6&&pos[2]>0.0);
ecef2pos(r5,pos);
assert(fabs(pos[0]+PI/2)<1E-6&&pos[2]>0.0);
ecef2pos(r6,pos);
assert(fabs(pos[0]*R2D-3.1796021375E+01)<1E-7&&
fabs(pos[1]*R2D-1.3040799917E+02)<1E-7&&
fabs(pos[2]-6.8863206206E+01)<1E-4);
ecef2pos(r7,pos);
assert(fabs(pos[0]*R2D+3.1796021375E+01)<1E-7&&
fabs(pos[1]*R2D-1.3040799917E+02)<1E-7&&
fabs(pos[2]-6.8863206206E+01)<1E-4);
printf("%s utset1 : OK\n",__FILE__);
}
static void RtkCommon__ecef2pos(JNIEnv* env, jclass clazz, jdouble x,
jdouble y, jdouble z, jdoubleArray j_pos)
{
double r[3] = {x, y, z};
double pos[3];
ecef2pos(r, pos);
(*env)->SetDoubleArrayRegion(env, j_pos, 0, 3, pos);
}
//---------------------------------------------------------------------------
void __fastcall TMapAreaDialog::BtnCenterClick(TObject *Sender)
{
AnsiString s;
double rr[3],pos[3];
if (!Plot->GetCenterPos(rr)) return;
ecef2pos(rr,pos);
Lat->Text=s.sprintf("%.7f",pos[0]*R2D);
Lon->Text=s.sprintf("%.7f",pos[1]*R2D);
UpdateMap();
}
static void possend(hcnraw_t *hcnraw){
int i;
double r[3],e[3];
posstate_t posstate = { 0 };
char sys[] = {
0x01,0x04,0x20,0x20,0x01,0x04,0x20,0x01,0x04,
0x08,0x02,0x02,0x08,0x08
};
if(hcnraw->satpro.solsatnum <= 0) return;
r[0]=hcnraw->satpro.x;
r[1]=hcnraw->satpro.y;
r[2]=hcnraw->satpro.z;
ecef2pos(r,e);
posstate.lat = e[0]*R2D;
posstate.lon = e[1]*R2D;
posstate.hgt = e[2];
posstate.postype = hcnraw->satpro.pvtstatus;
posstate.difftime= hcnraw->satpro.age;
for(i = 0;i < 4;i++) posstate.stnid[i] = hcnraw->satpro.stnid[i];
posstate.gpssec = (float)time2gpst(hcnraw->time,&posstate.gpsweek);
posstate.pdop = hcnraw->satpro.pdop;
posstate.hdop = hcnraw->satpro.hdop;
posstate.vdop = hcnraw->satpro.vdop;
posstate.solsatnu= hcnraw->satpro.solsatnum;
for(i = 0;i < 5;i++) posstate.sateprn.satnu[i] = 0;
for(i = 0;i < hcnraw->satpro.satnum;i++){
if(hcnraw->satpro.satprosat[i].satstatus != 0) continue;
switch(sys[hcnraw->satpro.satprosat[i].sigtype]){
case SYS_GPS: posstate.sateprn.gps[posstate.sateprn.satnu[0]++]=
hcnraw->satpro.satprosat[i].svid;
break;
case SYS_GLO: posstate.sateprn.glo[posstate.sateprn.satnu[1]++]=
hcnraw->satpro.satprosat[i].svid;
break;
case SYS_GAL:posstate.sateprn.gali[posstate.sateprn.satnu[2]++]=
hcnraw->satpro.satprosat[i].svid;
break;
case SYS_CMP: posstate.sateprn.cmp[posstate.sateprn.satnu[3]++]=
hcnraw->satpro.satprosat[i].svid;
break;
case SYS_SBS: posstate.sateprn.sbas[posstate.sateprn.satnu[4]++]=
hcnraw->satpro.satprosat[i].svid;
break;
}
}
pthread_mutex_lock(&lock);
cmd_response_massage( sock,&msg_head[2],&posstate,sizeof(posstate_t));
pthread_mutex_unlock(&lock);
}
/* estimate ionosphere -------------------------------------------------------*/
static int est_iono(obs_t *obs, nav_t *nav, const pcv_t *pcv, double *rr,
double tint, FILE *fp)
{
sstat_t sstat[MAXSAT]={{{0}}};
ekf_t *ekf;
gtime_t time;
double pos[3],rs[MAXOBS*6],dts[MAXOBS*2],var[MAXOBS],e[3],azel[MAXOBS*2];
double *v,*H,*R,phw[MAXSAT]={0};
int i,j,n=0,info,nx=NX,nv=MAXSAT*2,svh[MAXOBS];
ekf=ekf_new(NX); v=mat(nv,1); H=mat(nx,nv); R=mat(nv,nv);
/* receiver position */
ecef2pos(rr,pos);
out_head(obs->data[0].time,pos,fp);
for (i=0;i<obs->n;i+=n) {
for (n=1;i+n<obs->n;n++) {
if (timediff(obs->data[i+n].time,obs->data[i].time)>1E-3) break;
}
time=obs->data[i].time;
/* satellite positions and clocks */
satposs(time,obs->data+i,n,nav,EPHOPT_BRDC,rs,dts,var,svh);
/* satellite azimuth/elevation angle */
for (j=0;j<n;j++) {
if (geodist(rs+j*6,rr,e)>0.0) satazel(pos,e,azel+j*2);
else azel[j*2]=azel[1+j*2]=0.0;
}
/* time update of parameters */
ud_state(obs->data+i,n,nav,pos,azel,ekf,sstat);
/* ionosphere residuals */
if ((nv=res_iono(obs->data+i,n,nav,rs,rr,pos,azel,pcv,ekf,phw,v,H,R))<=0) {
continue;
}
/* filter */
if ((info=filter(ekf->x,ekf->P,H,v,R,ekf->nx,nv))) {
fprintf(stderr,"filter error: info=%d\n",info);
break;
}
/* output ionopshere parameters */
if (tint<=0.0||fmod(time2gpst(time,NULL)+0.005,tint)<0.01) {
out_iono(obs->data[i].time,ekf,sstat,fp);
}
}
ekf_free(ekf); free(v); free(H); free(R);
return 1;
}
/* 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;
}
}
/* doppler residuals ---------------------------------------------------------*/
static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const double *rr, const double *x,
const double *azel, const int *vsat, double *v, double *H)
{
double lam,rate,pos[3],E[9],a[3],e[3],vs[3],cosel;
int i,j,nv=0;
trace(3,"resdop : n=%d\n",n);
ecef2pos(rr,pos); xyz2enu(pos,E);
fprintf(output, " Begin doppler residuals calculation\n");
for (i=0;i<n&&i<MAXOBS;i++) {
fprintf(output, " Satellite %i:\n", obs[i].sat);
lam=nav->lam[obs[i].sat-1][0];
if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) {
continue;
}
/* line-of-sight vector in ecef */
cosel=cos(azel[1+i*2]);
a[0]=sin(azel[i*2])*cosel;
a[1]=cos(azel[i*2])*cosel;
a[2]=sin(azel[1+i*2]);
matmul("TN",3,1,3,1.0,E,a,0.0,e);
/* satellite velocity relative to receiver in ecef */
for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j];
/* range rate with earth rotation correction */
rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]-
rs[3+i*6]*rr[1]-rs[ i*6]*x[1]);
fprintf(output, " rate = %f\n", rate);
/* doppler residual */
v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]);
fprintf(output, " Residual = %f", v[nv]);
/* design matrix */
for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0;
nv++;
}
fprintf(output, " End residuals calculation, total %i residuals\n", nv);
return nv;
}
// update time-information for solution plot --------------------------------
void __fastcall TPlot::UpdateTimeSol(void)
{
const char *unit[]={"m","m/s","m/s2"},*u;
const char *sol[]={"","FIX","FLOAT","SBAS","DGPS","Single","PPP"};
AnsiString msg,msgs[8],s;
sol_t *data;
double xyz[3],pos[3],r,az,el;
int sel=BtnSol1->Down||!BtnSol2->Down?0:1,ind=SolIndex[sel];
char tstr[64];
trace(3,"UpdateTimeSol\n");
if ((BtnSol1->Down||BtnSol2->Down||BtnSol12->Down)&&
(data=getsol(SolData+sel,ind))) {
if (!ConnectState) msg.sprintf("[%d]",sel+1); else msg="[R]";
TimeStr(data->time,2,1,tstr);
msg+=tstr;
msg+=" : ";
if (PLOT_SOLP<=PlotType&&PlotType<=PLOT_SOLA) {
PosToXyz(data->time,data->rr,data->type,xyz);
u=unit[PlotType-PLOT_SOLP];
msg+=s.sprintf("E=%7.4f%s N=%7.4f%s U=%7.4f%s Q=",
xyz[0],u,xyz[1],u,xyz[2],u);
}
else if (PlotType==PLOT_NSAT) {
msg+=s.sprintf("NS=%d AGE=%.1f RATIO=%.1f Q=",data->ns,data->age,
data->ratio);
}
else if (!data->type) {
ecef2pos(data->rr,pos);
msg+=LatLonStr(pos,9)+s.sprintf(" %9.4fm Q=",pos[2]);
}
else {
r=norm(data->rr,3);
az=norm(data->rr,2)<=1E-12?0.0:atan2(data->rr[0],data->rr[1])*R2D;
el=r<=1E-12?0.0:asin(data->rr[2]/r)*R2D;
msg+=s.sprintf("B=%.3fm D=%6.2f" CHARDEG " %5.2f" CHARDEG " Q=",
r,az<0.0?az+360.0:az,el);
}
if (1<=data->stat&&data->stat<=6) {
msgs[data->stat-1]=s.sprintf("%d:%s",data->stat,sol[data->stat]);
}
}
ShowMsg(msg);
ShowLegend(msgs);
}
//---------------------------------------------------------------------------
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;
}
//---------------------------------------------------------------------------
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;
}
//---------------------------------------------------------------------------
void __fastcall TOptDialog::BtnRefPosClick(TObject *Sender)
{
TEdit *edit[]={RefPos1,RefPos2,RefPos3};
double p[3],pos[3];
GetPos(RefPosTypeP->ItemIndex,edit,p);
ecef2pos(p,pos);
RefDialog->RovPos[0]=pos[0]*R2D;
RefDialog->RovPos[1]=pos[1]*R2D;
RefDialog->RovPos[2]=pos[2];
RefDialog->StaPosFile=StaPosFile->Text;
RefDialog->Left=Left+Width/2-RefDialog->Width/2;
RefDialog->Top=Top+Height/2-RefDialog->Height/2;
if (RefDialog->ShowModal()!=mrOk) return;
pos[0]=RefDialog->Pos[0]*D2R;
pos[1]=RefDialog->Pos[1]*D2R;
pos[2]=RefDialog->Pos[2];
pos2ecef(pos,p);
SetPos(RefPosTypeP->ItemIndex,edit,p);
}
/* pos2ecef() */
void utest2(void)
{
double lat,lon,h,pos[3],posi[3];
double r[3];
for (lat=-90.0;lat<=90.0;lat+=5.0) {
for (lon=-180.0;lon<180.0;lon+=5.0) {
for (h=-10.0;h<10000.0;h+=100.0) {
pos[0]=lat*D2R; pos[1]=lon*D2R; pos[2]=h;
pos2ecef(pos,r);
ecef2pos(r,posi);
assert(fabs(lat-posi[0]*R2D)<1E-7&&
(lat==-90.0||lat==90.0?1:fabs(lon-posi[1]*R2D)<1E-7)&&
fabs(h-posi[2])<1E-4);
}
}
}
printf("%s utset2 : OK\n",__FILE__);
}
/* 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;
}
//---------------------------------------------------------------------------
void __fastcall TPntDialog::BtnAddClick(TObject *Sender)
{
TGridRect r={0};
AnsiString s;
int i;
double rr[3],pos[3]={0};
for (i=0;i<PntList->RowCount;i++) {
if (PntList->Cells[3][i]=="") break;
}
if (i>=PntList->RowCount) return;
if (!Plot->GetCurrentPos(rr)) return;
if (norm(rr,3)<=0.0) return;
ecef2pos(rr,pos);
PntList->Cells[0][i]=s.sprintf("%.9f",pos[0]*R2D);
PntList->Cells[1][i]=s.sprintf("%.9f",pos[1]*R2D);
PntList->Cells[2][i]=s.sprintf("%.4f",pos[2]);
PntList->Cells[3][i]=s.sprintf("new point %d",i+1);
r.Top=r.Bottom=i;
r.Left=0; r.Right=3; PntList->Selection=r;
}
/* options to system options buffer ------------------------------------------*/
static void sysopts2buff(void)
{
double pos[3],*rr;
char id[32],*p;
int i,j,sat,*ps;
elmask_ =prcopt_.elmin *R2D;
elmaskar_ =prcopt_.elmaskar *R2D;
elmaskhold_=prcopt_.elmaskhold*R2D;
for (i=0;i<2;i++) {
ps=i==0?&prcopt_.rovpos:&prcopt_.refpos;
rr=i==0?prcopt_.ru:prcopt_.rb;
if (*ps==0) {
antpostype_[i]=0;
ecef2pos(rr,pos);
antpos_[i][0]=pos[0]*R2D;
antpos_[i][1]=pos[1]*R2D;
antpos_[i][2]=pos[2];
}
else antpostype_[i]=*ps+1;
}
/* excluded satellites */
exsats_[0]='\0';
for (sat=1,p=exsats_;sat<=MAXSAT&&p-exsats_<(int)sizeof(exsats_)-32;sat++) {
if (prcopt_.exsats[sat-1]) {
satno2id(sat,id);
p+=sprintf(p,"%s%s%s",p==exsats_?"":" ",
prcopt_.exsats[sat-1]==2?"+":"",id);
}
}
/* snrmask */
for (i=0;i<NFREQ;i++) {
snrmask_[i][0]='\0';
p=snrmask_[i];
for (j=0;j<9;j++) {
p+=sprintf(p,"%s%.0f",j>0?",":"",prcopt_.snrmask.mask[i][j]);
}
}
}
/* 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;
}
}
}
char get_solution(double &lat, double &lon, double &height)
{
rtksvrlock(&svr);
const sol_t *sol = &svr.solbuf[svr.nsol-1];
const double *rb = svr.rtk.rb;
double pos[3]={0},Qr[9],Qe[9]={0},dms1[3]={0},dms2[3]={0},bl[3]={0};
double enu[3]={0},pitch=0.0,yaw=0.0,len;
int i;
int status;
if (sol->time.time == 0 || !sol->stat) {
rtksvrunlock(&svr);
return -1; // no fix
}
if (1 <= sol->stat && sol->stat <= 2) status = 0; // fix
if (sol->stat == 3) status = 1; // sbas fix
else status = 2; // differential fix
if (norm(sol->rr,3) > 0.0 && norm(rb,3) > 0.0) {
for (i=0;i<3;i++) bl[i]=sol->rr[i]-rb[i];
}
len=norm(bl,3);
Qr[0]=sol->qr[0];
Qr[4]=sol->qr[1];
Qr[8]=sol->qr[2];
Qr[1]=Qr[3]=sol->qr[3];
Qr[5]=Qr[7]=sol->qr[4];
Qr[2]=Qr[6]=sol->qr[5];
if (norm(sol->rr,3) > 0.0) {
ecef2pos(sol->rr,pos);
covenu(pos,Qr,Qe);
lat = pos[0]*R2D;
lon = pos[1]*R2D;
if (solopt[0].height == 1) pos[2]-=geoidh(pos); /* geodetic */
height = pos[2];
}
svr.nsol=0;
rtksvrunlock(&svr);
/* 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];
}
}
}
