/* decode gps/qzss navigation data subframe 3 --------------------------------*/
static int decode_subfrm3(const unsigned char *buff, eph_t *eph)
{
double tow,toc;
int i=48,iode;
trace(4,"decode_subfrm3:\n");
trace(5,"decode_subfrm3: buff="); traceb(5,buff,30);
eph->cic =getbits(buff,i,16)*P2_29; i+=16;
eph->OMG0=getbits(buff,i,32)*P2_31*SC2RAD; i+=32;
eph->cis =getbits(buff,i,16)*P2_29; i+=16;
eph->i0 =getbits(buff,i,32)*P2_31*SC2RAD; i+=32;
eph->crc =getbits(buff,i,16)*P2_5; i+=16;
eph->omg =getbits(buff,i,32)*P2_31*SC2RAD; i+=32;
eph->OMGd=getbits(buff,i,24)*P2_43*SC2RAD; i+=24;
iode =getbitu(buff,i, 8); i+= 8;
eph->idot=getbits(buff,i,14)*P2_43*SC2RAD;
/* check iode and iodc consistency */
if (iode!=eph->iode||iode!=(eph->iodc&0xFF)) return 0;
/* adjustment for week handover */
tow=time2gpst(eph->ttr,&eph->week);
toc=time2gpst(eph->toc,NULL);
if (eph->toes<tow-302400.0) {eph->week++; tow-=604800.0;}
else if (eph->toes>tow+302400.0) {eph->week--; tow+=604800.0;}
eph->toe=gpst2time(eph->week,eph->toes);
eph->toc=gpst2time(eph->week,toc);
eph->ttr=gpst2time(eph->week,tow);
return 3;
}
/* output header -------------------------------------------------------------*/
static void outheader(FILE *fp, char **file, int n, const prcopt_t *popt,
const solopt_t *sopt)
{
const char *s1[]={"GPST","UTC","JST"};
gtime_t ts,te;
double t1,t2;
int i,j,w1,w2;
char s2[32],s3[32];
trace(3,"outheader: n=%d\n",n);
if (sopt->posf==SOLF_NMEA) return;
if (sopt->outhead) {
if (!*sopt->prog) {
fprintf(fp,"%s program : RTKLIB ver.%s\n",COMMENTH,VER_RTKLIB);
}
else {
fprintf(fp,"%s program : %s\n",COMMENTH,sopt->prog);
}
for (i=0;i<n;i++) {
fprintf(fp,"%s inp file : %s\n",COMMENTH,file[i]);
}
for (i=0;i<obss.n;i++) if (obss.data[i].rcv==1) break;
for (j=obss.n-1;j>=0;j--) if (obss.data[j].rcv==1) break;
if (j<i) {fprintf(fp,"\n%s no rover obs data\n",COMMENTH); return;}
ts=obss.data[i].time;
te=obss.data[j].time;
t1=time2gpst(ts,&w1);
t2=time2gpst(te,&w2);
if (sopt->times>=1) ts=gpst2utc(ts);
if (sopt->times>=1) te=gpst2utc(te);
if (sopt->times==2) ts=timeadd(ts,9*3600.0);
if (sopt->times==2) te=timeadd(te,9*3600.0);
time2str(ts,s2,1);
time2str(te,s3,1);
fprintf(fp,"%s obs start : %s %s (week%04d %8.1fs)\n",COMMENTH,s2,s1[sopt->times],w1,t1);
fprintf(fp,"%s obs end : %s %s (week%04d %8.1fs)\n",COMMENTH,s3,s1[sopt->times],w2,t2);
}
if (sopt->outopt) {
outprcopt(fp,popt);
}
if (PMODE_DGPS<=popt->mode&&popt->mode<=PMODE_FIXED&&popt->mode!=PMODE_MOVEB) {
fprintf(fp,"%s ref pos :",COMMENTH);
outrpos(fp,popt->rb,sopt);
fprintf(fp,"\n");
}
if (sopt->outhead||sopt->outopt) fprintf(fp,"%s\n",COMMENTH);
outsolhead(fp,sopt);
}
/* gpst2time(), time2gpst() */
void utest4(void)
{
gtime_t t;
double ep[6];
int w,week;
double time,tt;
t=gpst2time(0,0.0); time2epoch(t,ep);
assert(ep[0]==1980&&ep[1]==1&&ep[2]==6&&ep[3]==0&&ep[4]==0&&ep[5]==0.0);
t=gpst2time(1400,86400.0); time2epoch(t,ep);
assert(ep[0]==2006&&ep[1]==11&&ep[2]==6&&ep[3]==0&&ep[4]==0&&ep[5]==0.0);
t=gpst2time(1400,86400.0*7-1.0); time2epoch(t,ep);
assert(ep[0]==2006&&ep[1]==11&&ep[2]==11&&ep[3]==23&&ep[4]==59&&ep[5]==59.0);
t=gpst2time(1400,86400.0*7); time2epoch(t,ep);
assert(ep[0]==2006&&ep[1]==11&&ep[2]==12&&ep[3]==0&&ep[4]==0&&ep[5]==0.0);
t=gpst2time(1401,0.0); time2epoch(t,ep);
assert(ep[0]==2006&&ep[1]==11&&ep[2]==12&&ep[3]==0&&ep[4]==0&&ep[5]==0.0);
for (w=1000;w<1100;w++) {
for (time=0.0;time<86400.0*7;time+=3600.0) {
t=gpst2time(w,time); tt=time2gpst(t,&week);
assert(tt==time&&week==w);
}
}
printf("%s utset4 : OK\n",__FILE__);
}
// generate time-string -----------------------------------------------------
void __fastcall TPlot::TimeStr(gtime_t time, int n, int tsys, char *str)
{
struct tm *t;
char tstr[64],*label="";
double tow;
int week;
if (TimeLabel==0) { // www/ssss
tow=time2gpst(time,&week);
sprintf(tstr,"%4d/%*.*fs",week,(n>0?6:5)+n,n,tow);
}
else if (TimeLabel==1) { // gpst
time2str(time,tstr,n);
label=" GPST";
}
else if (TimeLabel==2) { // utc
time2str(gpst2utc(time),tstr,n);
label=" UTC";
}
else { // lt
time=gpst2utc(time);
if (!(t=localtime(&time.time))) strcpy(tstr,"2000/01/01 00:00:00.0");
else sprintf(tstr,"%04d/%02d/%02d %02d:%02d:%02d.%0*d",t->tm_year+1900,
t->tm_mon+1,t->tm_mday,t->tm_hour,t->tm_min,t->tm_sec,
n,(int)(time.sec*pow(10.0,n)));
label=" LT";
}
sprintf(str,"%s%s",tstr,label);
}
/* decode Galileo navigation data (I/NAV word 1) -------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_word1(const uint8_t *buff, sdrnav_t *nav)
{
sdreph_t *eph = &nav->sdreph;
int oldiodc=eph->eph.iodc;
double sqrtA;
eph->eph.iodc =getbitu( buff,OFFSET1+ 6,10);
eph->eph.toes =getbitu( buff,OFFSET1+16,14)*60; /* sec of week in GST */
eph->eph.M0 =getbits( buff,OFFSET1+30,32)*P2_31*SC2RAD;
eph->eph.e =getbitu( buff,OFFSET1+62,32)*P2_33;
sqrtA =getbitu2(buff,OFFSET1+94,18,OFFSET2+ 0,14)*P2_19;
eph->eph.A =sqrtA*sqrtA;
eph->eph.iode =eph->eph.iodc;
eph->eph.code =513; /* bit0,bit9 (1000000001) */
/* compute time of ephemeris */
if (eph->week_gst!=0) {
eph->eph.toe=gst2time(eph->week_gst,eph->eph.toes);
Ephemeris[nav->sat].Page1(eph->eph.iodc, eph->eph.M0, eph->eph.e, sqrtA, time2gpst(eph->eph.toe, NULL));
} else {
Ephemeris[nav->sat].Page1(eph->eph.iodc, eph->eph.M0, eph->eph.e, sqrtA);
}
/* ephemeris update flag */
if (oldiodc-eph->eph.iodc!=0) eph->update=ON;
/* ephemeris counter */
eph->cnt++;
}
/* decode waas messages ------------------------------------------------------*/
static int decode_gw10sbs(raw_t *raw)
{
double tow;
int i,prn;
unsigned char *p=raw->buff+2;
trace(4,"decode_gw10sbs : len=%d\n",raw->len);
tow=U4(p)/1000.0;
prn=U1(p+4);
if (prn<MINPRNSBS||MAXPRNSBS<prn) {
trace(2,"gw10 sbs satellite number error: prn=%d\n",prn);
return -1;
}
raw->sbsmsg.prn=prn;
raw->sbsmsg.tow=(int)tow;
tow=time2gpst(raw->time,&raw->sbsmsg.week);
if (raw->sbsmsg.tow<tow-302400.0) raw->sbsmsg.week++;
else if (raw->sbsmsg.tow>tow+302400.0) raw->sbsmsg.week--;
for (i=0;i<29;i++) {
raw->sbsmsg.msg[i]=*(p+5+i);
}
raw->sbsmsg.msg[28]&=0xC0;
return 3;
}
// 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 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]);
}
}
}
/* adjust weekly rollover of gps time ----------------------------------------*/
static gtime_t adjweek(gtime_t time, double tow)
{
double tow_p;
int week;
tow_p=time2gpst(time,&week);
if (tow<tow_p-302400.0) tow+=604800.0;
else if (tow>tow_p+302400.0) tow-=604800.0;
return gpst2time(week,tow);
}
/* output ionosphere header --------------------------------------------------*/
static void out_head(gtime_t time, const double *pos, FILE *fp)
{
double tow;
int week;
tow=time2gpst(time,&week);
fprintf(fp,"%s %4d %5.0f %7.3f %8.3f\n",SPOS_RID,week,tow,pos[0]*R2D,
pos[1]*R2D);
}
/* decode Galileo navigation data (I/NAV word 5) -------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_word5(const uint8_t *buff, sdrnav_t *nav)
{
sdreph_t *eph = &nav->sdreph;
unsigned int e1bdvs,e1bhs,e5bdvs,e5bhs;
double tow_gst;
eph->eph.tgd[0]=getbits(buff,OFFSET1+47,10)*P2_32; /* BGD E5a/E1 */
eph->eph.tgd[1]=getbits(buff,OFFSET1+57,10)*P2_32; /* BGD E5b/E1 */
//printf("decode_word5 %s tgd: %e %e\n", PRN(nav->sat), eph->eph.tgd[0], eph->eph.tgd[1]);
e5bhs =getbitu(buff,OFFSET1+67, 2); /* E5B signal health status */
e1bhs =getbitu(buff,OFFSET1+69, 2); /* E1B signal health status */
e5bdvs =getbitu(buff,OFFSET1+71, 1); /* E5B data validity status */
e1bdvs =getbitu(buff,OFFSET1+72, 1); /* E1B data validity status */
eph->week_gst =getbitu(buff,OFFSET1+73,12); /* week in GST */
eph->eph.week =eph->week_gst+1024; /* GST week to Galileo week */
tow_gst =getbitu(buff,OFFSET1+85,20)+2.0;
/* compute GPS time of week */
eph->tow_gpst =time2gpst(gst2time(eph->week_gst,tow_gst),&eph->week_gpst);
nav->tow_updated = 1;
eph->eph.ttr =gst2time(eph->week_gst,tow_gst);
/* compute time of clock and ephemeris */
double t_oc = 0, t_oe = 0;
if (eph->toc_gst!=0) {
eph->eph.toc=gst2time(eph->week_gst,eph->toc_gst);
t_oc = time2gpst(eph->eph.toc, NULL);
}
if (eph->eph.toes!=0) {
eph->eph.toe=gst2time(eph->week_gst,eph->eph.toes);
t_oe = time2gpst(eph->eph.toe, NULL);
}
Ephemeris[nav->sat].Page5(eph->tow_gpst, eph->week_gpst, eph->eph.tgd[1], t_oc, t_oe);
/* health status */
eph->eph.svh=(e5bhs<<7)+(e5bdvs<<6)+(e1bhs<<1)+(e1bdvs);
/* ephemeris counter */
eph->cnt++;
}
// read observation data ----------------------------------------------------
void __fastcall TPlot::ReadObs(TStrings *files)
{
obs_t obs={0};
nav_t nav={0};
sta_t sta={0};
AnsiString s;
char file[1024];
int i,nobs;
trace(3,"ReadObs\n");
if (files->Count<=0) return;
ReadWaitStart();
ShowLegend(NULL);
if ((nobs=ReadObsRnx(files,&obs,&nav,&sta))<=0) {
ReadWaitEnd();
return;
}
ClearObs();
Obs=obs;
Nav=nav;
Sta=sta;
SimObs=0;
UpdateObs(nobs);
UpdateMp();
if (ObsFiles!=files) {
ObsFiles->Assign(files);
}
NavFiles->Clear();
strcpy(file,U2A(files->Strings[0]).c_str());
Caption=s.sprintf("%s%s",file,files->Count>1?"...":"");
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();
}
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);
}
/* adjust week ---------------------------------------------------------------*/
static int adjweek(raw_t *raw, double sec)
{
double tow;
int week;
if (raw->time.time==0) return 0;
tow=time2gpst(raw->time,&week);
if (sec<tow-302400.0) sec+=604800.0;
else if (sec>tow+302400.0) sec-=604800.0;
raw->time=gpst2time(week,sec);
return 1;
}
/* adjust weekly rollover of gps time ----------------------------------------*/
static int adjweek(raw_t *raw, double tow)
{
double tow_p;
int week;
if (raw->time.time==0) return 0;
tow_p=time2gpst(raw->time,&week);
if (tow<tow_p-302400.0) tow+=604800.0;
else if (tow>tow_p+302400.0) tow-=604800.0;
raw->time=gpst2time(week,tow);
return 1;
}
/* 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;
}
// save snr and mp -------------------------------------------------------------
void __fastcall TPlot::SaveSnrMp(AnsiString file)
{
FILE *fp;
AnsiString ObsTypeText=ObsType2->Text;
gtime_t time;
double tow;
char sat[32],mp[32],tstr[64],*tlabel,*code=ObsTypeText.c_str()+1;
int i,j,k,week;
trace(3,"SaveSnrMp: 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)");
sprintf(mp,"%s MP(m)",ObsTypeText.c_str());
fprintf(fp,"%% %-*s %6s %8s %8s %9s %10s\n",TimeLabel==0?13:19,tlabel,"SAT",
"AZ(deg)","EL(deg)","SNR(dBHz)",mp);
for (i=0;i<MAXSAT;i++) {
if (SatMask[i]||!SatSel[i]) continue;
satno2id(i+1,sat);
for (j=0;j<Obs.n;j++) {
if (Obs.data[j].sat!=i+1) continue;
for (k=0;k<NFREQ+NEXOBS;k++) {
if (strstr(code2obs(Obs.data[j].code[k],NULL),code)) break;
}
if (k>=NFREQ+NEXOBS) continue;
time=Obs.data[j].time;
if (TimeLabel==0) {
tow=time2gpst(time,&week);
sprintf(tstr,"%4d %9.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 %6s %8.1f %8.1f %9.2f %10.4f\n",tstr,sat,Az[j]*R2D,
El[j]*R2D,Obs.data[j].SNR[k]*0.25,!Mp[k]?0.0:Mp[k][j]);
}
}
fclose(fp);
}
/* merge ephmeris data ---------------------------------------------------------
* merge separated ephmeris data to ephemeris struct
* args : sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void merge_g1(sdreph_t *eph)
{
double ep[6];
eph->geph.tof=glot2time(eph->nt,eph->n4,eph->tk[0],eph->tk[1],eph->tk[2]);
/* compute current tow (tk is time of word 1) */
eph->tow_gpst=time2gpst(eph->geph.tof,&eph->eph.week)+eph->s1cnt*2.0;
eph->week_gpst=eph->eph.week;
ep[3]=0;ep[4]=eph->geph.iode-60*3;ep[5]=0; /* 3 hour bias in UTC-GPST */
eph->geph.toe=utc2gpst(epoch2time(ep)); /* time of ephemeris */
}
// 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);
}
/* adjust hourly rollover of rtcm 2 time -------------------------------------*/
static void adjhour(rtcm_t *rtcm, double zcnt)
{
double tow,hour,sec;
int week;
/* if no time, get cpu time */
if (rtcm->time.time==0) rtcm->time=utc2gpst(timeget());
tow=time2gpst(rtcm->time,&week);
hour=floor(tow/3600.0);
sec=tow-hour*3600.0;
if (zcnt<sec-1800.0) zcnt+=3600.0;
else if (zcnt>sec+1800.0) zcnt-=3600.0;
rtcm->time=gpst2time(week,hour*3600+zcnt);
}
/* output ionosphere parameters ----------------------------------------------*/
static int out_iono(gtime_t time, const double *x, const double *P, int nx,
FILE *fp)
{
double tow;
char id[64];
int i,week;
tow=time2gpst(time,&week);
for (i=0;i<MAXSAT;i++) {
sat2id(i+1,id);
fprintf(fp,"$ION,%d,%.3f,%d,%s,%.1f,%.4f,%4f\n",week,tow,0,id,
ssat[i].azel[1]*R2D,x[II(i+1)],0);
}
}
/* decode Galileo navigation data (I/NAV word 6) -------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_word6(const uint8_t *buff, sdrnav_t *nav)
{
sdreph_t *eph = &nav->sdreph;
double tow_gst;
tow_gst =getbitu2(buff,OFFSET1+105, 7,OFFSET2+ 0,13)+2.0;
/* compute GPS time of week */
if (eph->week_gst!=0) {
eph->tow_gpst=
time2gpst(gst2time(eph->week_gst,tow_gst),&eph->week_gpst);
nav->tow_updated = 1;
eph->eph.ttr=gst2time(eph->week_gst,tow_gst);
Ephemeris[nav->sat].Page6(eph->tow_gpst, eph->week_gpst);
}
}
/* rinex converter -------------------------------------------------------------
* convert receiver log file to rinex obs/nav, sbas log files
* args : int format I receiver raw format (STRFMT_???)
* rnxopt_t *opt IO rinex options (see below)
* char *file I rtcm, receiver raw or rinex file
* (wild-cards (*) are expanded)
* char **ofile IO output files
* ofile[0] rinex obs file ("": no output)
* ofile[1] rinex nav file ("": no output)
* ofile[2] rinex gnav file ("": no output)
* ofile[3] rinex hnav file ("": no output)
* ofile[4] rinex qnav file ("": no output)
* ofile[5] rinex lnav file ("": no output)
* ofile[6] sbas/lex log file("": no output)
* return : status (1:ok,0:error,-1:abort)
* notes : the following members of opt are replaced by information in last
* converted rinex: opt->tstart, opt->tend, opt->obstype, opt->nobs
* keywords in ofile[] are replaced by first obs date/time and station
* id (%r)
* the order of wild-card expanded files must be in-order by time
*-----------------------------------------------------------------------------*/
extern int convrnx(int format, rnxopt_t *opt, const char *file, char **ofile)
{
gtime_t t0={0};
rnxopt_t opt_=*opt;
double tu,ts;
int i,week,stat=1;
trace(3,"convrnx: format=%d file=%s ofile=%s %s %s %s %s %s %s\n",
format,file,ofile[0],ofile[1],ofile[2],ofile[3],ofile[4],ofile[5],
ofile[6]);
showmsg("");
if (opt->ts.time==0||opt->te.time==0||opt->tunit<=0.0) {
/* single-session */
opt_.tstart=opt_.tend=t0;
stat=convrnx_s(0,format,&opt_,file,ofile);
}
else if (timediff(opt->ts,opt->te)<=0.0) {
/* multiple-session */
tu=opt->tunit<86400.0?opt->tunit:86400.0;
ts=tu*(int)floor(time2gpst(opt->ts,&week)/tu);
for (i=0;;i++) { /* for each session */
opt_.ts=gpst2time(week,ts+i*tu);
opt_.te=timeadd(opt_.ts,tu-DTTOL-0.001);
opt_.trtcm=timeadd(opt->trtcm,timediff(opt_.ts,opt->ts));
if (timediff(opt_.ts,opt->te)>0.0) break;
if (timediff(opt_.ts,opt->ts)<0.0) opt_.ts=opt->ts;
if (timediff(opt_.te,opt->te)>0.0) opt_.te=opt->te;
opt_.tstart=opt_.tend=t0;
if ((stat=convrnx_s(i+1,format,&opt_,file,ofile))<0) break;
}
}
else {
showmsg("no period");
return 0;
}
/* output start/end time */
opt->tstart=opt_.tstart; opt->tend=opt_.tend;
return stat;
}
/* decode id#22 ephemeris data ------------------------------------------------*/
static int decode_ss2eph(raw_t *raw)
{
eph_t eph={0};
unsigned int tow;
int i,j,prn,sat;
unsigned char *p=raw->buff+4,buff[90]={0};
trace(4,"decode_ss2eph: len=%d\n",raw->len);
if (raw->len!=79) {
trace(2,"ss2 id#22 length error: len=%d\n",raw->len);
return -1;
}
prn=(U4(p)&0x1F)+1;
if (!(sat=satno(SYS_GPS,prn))) {
trace(2,"ss2 id#22 satellite number error: prn=%d\n",prn);
return -1;
}
if (raw->time.time==0) {
trace(2,"ss2 id#22 week number unknown error\n");
return -1;
}
tow=(unsigned int)(time2gpst(raw->time,NULL)/6.0);
for (i=0;i<3;i++) {
buff[30*i+3]=(unsigned char)(tow>>9); /* add tow + subframe id */
buff[30*i+4]=(unsigned char)(tow>>1);
buff[30*i+5]=(unsigned char)(((tow&1)<<7)+((i+1)<<2));
for (j=0;j<24;j++) buff[30*i+6+j]=p[1+24*i+j];
}
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,"ss2 id#22 subframe error: prn=%d\n",prn);
return -1;
}
if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */
eph.sat=sat;
eph.ttr=raw->time;
raw->nav.eph[sat-1]=eph;
raw->ephsat=sat;
return 2;
}
/* output ionosphere parameters ----------------------------------------------*/
static void out_iono(gtime_t time, const ekf_t *ekf, const sstat_t *sstat,
FILE *fp)
{
double tow;
char id[64];
int sat,week;
tow=time2gpst(time,&week);
for (sat=1;sat<=MAXSAT;sat++) {
if (sstat[sat-1].time.time==0||
timediff(time,sstat[sat-1].time)>MAXGAP_IONO) continue;
satno2id(sat,id);
fprintf(fp,"%s %4d %6.0f %-3s %d %8.4f %9.6f %7.4f %6.1f %5.1f %7.3f %11.3f\n",
STEC_RID,week,tow,id,sstat[sat-1].slip,
ekf->x[II(sat)],ekf->x[II(sat)+1],
sqrt(ekf->P[II(sat)*(ekf->nx+1)]),sstat[sat-1].azel[0]*R2D,
sstat[sat-1].azel[1]*R2D, sstat[sat-1].PG,sstat[sat-1].LG);
}
}
//---------------------------------------------------------------------------
void __fastcall TTimeDialog::FormShow(TObject *Sender)
{
gtime_t utc;
double tow,doy;
int week;
char msg[1024],s1[64],s2[64],*p=msg;
utc=gpst2utc(Time);
time2str(Time,s1,0);
time2str(utc,s2,0);
tow=time2gpst(Time,&week);
doy=time2doy(Time);
p+=sprintf(p,"%s GPST\n",s1);
p+=sprintf(p,"%s UTC\n\n",s2);
p+=sprintf(p,"GPS Week: %d\n",week);
p+=sprintf(p,"GPS Time: %.0f s\n",tow);
p+=sprintf(p,"Day of Year: %03d\n",(int)floor(doy));
p+=sprintf(p,"Day of Week: %d\n",(int)floor(tow/86400.0));
p+=sprintf(p,"Time of Day: %.0f s\n",fmod(tow,86400.0));
sprintf(p,"Leap Seconds: %.0f s\n",timediff(Time,utc));
Message->Caption=msg;
}
/* decode Galileo navigation data (I/NAV word 0) -------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_word0(const uint8_t *buff, sdrnav_t *nav)
{
sdreph_t *eph = &nav->sdreph;
double tow_gst;
/* see Galileo SISICD Table 49, pp. 39, time field */
if (getbitu(buff,OFFSET1+6,2) != 2) {
printf("E1B word0 time field != 2\n");
return;
}
eph->week_gst =getbitu( buff,OFFSET1+ 96,12); /* week in GST */
eph->eph.week =eph->week_gst+1024; /* GST week to Galileo week */
tow_gst =getbitu2(buff,OFFSET1+108, 4,OFFSET2+ 0,16)+2.0;
/* compute GPS time of week */
eph->tow_gpst =time2gpst(gst2time(eph->week_gst,tow_gst),&eph->week_gpst);
nav->tow_updated = 1;
eph->eph.ttr =gst2time(eph->week_gst,tow_gst);
Ephemeris[nav->sat].Page0(eph->tow_gpst, eph->week_gpst);
}
/* decode sbas message ---------------------------------------------------------
* decode sbas message frame words and check crc
* args : gtime_t time I reception time
* int prn I sbas satellite prn number
* unsigned int *word I message frame words (24bit x 10)
* sbsmsg_t *sbsmsg O sbas message
* return : status (1:ok,0:crc error)
*-----------------------------------------------------------------------------*/
extern int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words,
sbsmsg_t *sbsmsg)
{
int i,j;
unsigned char f[29];
double tow;
trace(5,"sbsdecodemsg: prn=%d\n",prn);
if (time.time==0) return 0;
tow=time2gpst(time,&sbsmsg->week);
sbsmsg->tow=(int)(tow+DTTOL);
sbsmsg->prn=prn;
for (i=0;i<7;i++) for (j=0;j<4;j++) {
sbsmsg->msg[i*4+j]=(unsigned char)(words[i]>>((3-j)*8));
}
sbsmsg->msg[28]=(unsigned char)(words[7]>>18)&0xC0;
for (i=28;i>0;i--) f[i]=(sbsmsg->msg[i]>>6)+(sbsmsg->msg[i-1]<<2);
f[0]=sbsmsg->msg[0]>>6;
return crc24q(f,29)==(words[7]&0xFFFFFF); /* check crc */
}
/* 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;
}
static jdouble GTime_get_gps_tow(JNIEnv* env, jobject thiz)
{
return time2gpst(get_gtime_t(env, thiz), NULL);
}
