/* decode gps/qzss almanac ---------------------------------------------------*/
static void decode_almanac(const unsigned char *buff, int sat, alm_t *alm)
{
gtime_t toa;
double deltai,sqrtA,tt;
int i=50,f0;
trace(4,"decode_almanac: sat=%2d\n",sat);
if (!alm||alm[sat-1].week==0) return;
alm[sat-1].sat =sat;
alm[sat-1].e =getbits(buff,i,16)*P2_21; i+=16;
alm[sat-1].toas=getbitu(buff,i, 8)*4096.0; i+= 8;
deltai =getbits(buff,i,16)*P2_19*SC2RAD; i+=16;
alm[sat-1].OMGd=getbits(buff,i,16)*P2_38*SC2RAD; i+=16;
alm[sat-1].svh =getbitu(buff,i, 8); i+= 8;
sqrtA =getbitu(buff,i,24)*P2_11; i+=24;
alm[sat-1].OMG0=getbits(buff,i,24)*P2_23*SC2RAD; i+=24;
alm[sat-1].omg =getbits(buff,i,24)*P2_23*SC2RAD; i+=24;
alm[sat-1].M0 =getbits(buff,i,24)*P2_23*SC2RAD; i+=24;
f0 =getbitu(buff,i, 8); i+= 8;
alm[sat-1].f1 =getbits(buff,i,11)*P2_38; i+=11;
alm[sat-1].f0 =getbits(buff,i, 3)*P2_17+f0*P2_20;
alm[sat-1].A =sqrtA*sqrtA;
alm[sat-1].i0 =0.3*SC2RAD+deltai;
toa=gpst2time(alm[sat-1].week,alm[sat-1].toas);
tt=timediff(toa,alm[sat-1].toa);
if (tt<302400.0) alm[sat-1].week--;
else if (tt>302400.0) alm[sat-1].week++;
alm[sat-1].toa=gpst2time(alm[sat-1].week,alm[sat-1].toas);
}
/* decode GPS/QZS navigation data (subframe 1) ---------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_subfrm1(const uint8_t *buff, sdreph_t *eph)
{
double toc;
int week;
eph->tow_gpst =getbitu( buff, 30,17)*6.0;
week =getbitu( buff, 60,10)+1024;
eph->eph.code =getbitu( buff, 70, 2);
eph->eph.sva =getbitu( buff, 72, 4);
eph->eph.svh =getbitu( buff, 76, 6);
eph->eph.iodc =getbitu2(buff, 82, 2,210, 8);
eph->eph.flag =getbitu( buff, 90, 1);
eph->eph.tgd[0]=getbits( buff,196, 8)*P2_31;
toc =getbitu( buff,218,16)*16.0;
eph->eph.f2 =getbits( buff,240, 8)*P2_55;
eph->eph.f1 =getbits( buff,248,16)*P2_43;
eph->eph.f0 =getbits( buff,270,22)*P2_31;
eph->eph.week=adjgpsweek(week);
eph->week_gpst=eph->eph.week;
eph->eph.ttr=gpst2time(eph->eph.week,eph->tow_gpst);
eph->eph.toc=gpst2time(eph->eph.week,toc);
/* subframe decode counter */
eph->cnt++;
}
/* decode gps/qzss navigation data subframe 1 --------------------------------*/
static int decode_subfrm1(const unsigned char *buff, eph_t *eph)
{
double tow,toc;
int i=48,week,iodc0,iodc1,tgd;
trace(4,"decode_subfrm1:\n");
trace(5,"decode_subfrm1: buff="); traceb(5,buff,30);
tow =getbitu(buff,24,17)*6.0; /* transmission time */
week =getbitu(buff,i,10); i+=10;
eph->code =getbitu(buff,i, 2); i+= 2;
eph->sva =getbitu(buff,i, 4); i+= 4; /* ura index */
eph->svh =getbitu(buff,i, 6); i+= 6;
iodc0 =getbitu(buff,i, 2); i+= 2;
eph->flag =getbitu(buff,i, 1); i+= 1+87;
tgd =getbits(buff,i, 8); i+= 8;
iodc1 =getbitu(buff,i, 8); i+= 8;
toc =getbitu(buff,i,16)*16.0; i+=16;
eph->f2 =getbits(buff,i, 8)*P2_55; i+= 8;
eph->f1 =getbits(buff,i,16)*P2_43; i+=16;
eph->f0 =getbits(buff,i,22)*P2_31;
eph->tgd[0]=tgd==-128?0.0:tgd*P2_31; /* ref [4] */
eph->iodc=(iodc0<<8)+iodc1;
eph->week=adjgpsweek(week); /* week of tow */
eph->ttr=gpst2time(eph->week,tow);
eph->toc=gpst2time(eph->week,toc);
return 1;
}
/* 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;
}
/* decode tof and toe field (ref [1] 5.7.2.2.1.1) ----------------------------*/
static int decode_lextof(const unsigned char *buff, int i, gtime_t *tof,
gtime_t *toe)
{
double tt,toes;
int tow,week;
char s1[64],s2[64];
trace(3,"decode_lextof:\n");
tow =getbitu(buff,i,20); i+=20;
week=getbitu(buff,i,13); i+=13;
toes=getbitu(buff,i,16)*15.0; i+=16;
*tof=gpst2time(week,tow);
*toe=gpst2time(week,toes);
trace(3,"decode_lextof: tow=%d week=%d toe=%d\n",tow,week,toes);
tt=timediff(*toe,*tof);
if (tt<-302400.0) *toe=timeadd(*toe, 604800.0);
else if (tt> 302400.0) *toe=timeadd(*toe,-604800.0);
time2str(*tof,s1,3);
time2str(*toe,s2,3);
trace(4,"decode_lextof: tof=%s toe=%s\n",s1,s2);
return i;
}
/* decode type 17: gps ephemerides -------------------------------------------*/
static int decode_type17(rtcm_t *rtcm)
{
eph_t eph={0};
double toc,sqrtA;
int i=48,week,prn,sat;
rtklib_trace(4,"decode_type17: len=%d\n",rtcm->len);
if (i+480<=rtcm->len*8) {
week =getbitu(rtcm->buff,i,10); i+=10;
eph.idot =getbits(rtcm->buff,i,14)*P2_43*SC2RAD; i+=14;
eph.iode =getbitu(rtcm->buff,i, 8); i+= 8;
toc =getbitu(rtcm->buff,i,16)*16.0; i+=16;
eph.f1 =getbits(rtcm->buff,i,16)*P2_43; i+=16;
eph.f2 =getbits(rtcm->buff,i, 8)*P2_55; i+= 8;
eph.crs =getbits(rtcm->buff,i,16)*P2_5; i+=16;
eph.deln =getbits(rtcm->buff,i,16)*P2_43*SC2RAD; i+=16;
eph.cuc =getbits(rtcm->buff,i,16)*P2_29; i+=16;
eph.e =getbitu(rtcm->buff,i,32)*P2_33; i+=32;
eph.cus =getbits(rtcm->buff,i,16); i+=16;
sqrtA =getbitu(rtcm->buff,i,32)*P2_19; i+=32;
eph.toes =getbitu(rtcm->buff,i,16); i+=16;
eph.OMG0 =getbits(rtcm->buff,i,32)*P2_31*SC2RAD; i+=32;
eph.cic =getbits(rtcm->buff,i,16)*P2_29; i+=16;
eph.i0 =getbits(rtcm->buff,i,32)*P2_31*SC2RAD; i+=32;
eph.cis =getbits(rtcm->buff,i,16)*P2_29; i+=16;
eph.omg =getbits(rtcm->buff,i,32)*P2_31*SC2RAD; i+=32;
eph.crc =getbits(rtcm->buff,i,16)*P2_5; i+=16;
eph.OMGd =getbits(rtcm->buff,i,24)*P2_43*SC2RAD; i+=24;
eph.M0 =getbits(rtcm->buff,i,32)*P2_31*SC2RAD; i+=32;
eph.iodc =getbitu(rtcm->buff,i,10); i+=10;
eph.f0 =getbits(rtcm->buff,i,22)*P2_31; i+=22;
prn =getbitu(rtcm->buff,i, 5); i+= 5+3;
eph.tgd[0]=getbits(rtcm->buff,i, 8)*P2_31; i+= 8;
eph.code =getbitu(rtcm->buff,i, 2); i+= 2;
eph.sva =getbitu(rtcm->buff,i, 4); i+= 4;
eph.svh =getbitu(rtcm->buff,i, 6); i+= 6;
eph.flag =getbitu(rtcm->buff,i, 1);
}
else {
rtklib_trace(2,"rtcm2 17 length error: len=%d\n",rtcm->len);
return -1;
}
if (prn==0) prn=32;
sat=satno(SYS_GPS,prn);
eph.sat=sat;
eph.week=adjgpsweek(week);
eph.toe=gpst2time(eph.week,eph.toes);
eph.toc=gpst2time(eph.week,toc);
eph.ttr=rtcm->time;
eph.A=sqrtA*sqrtA;
rtcm->nav.eph[sat-1]=eph;
rtcm->ephsat=sat;
return 2;
}
/* decode ephemeris ----------------------------------------------------------*/
static int decode_gpsephem(int sat, raw_t *raw)
{
eph_t eph={0};
unsigned char *puiTmp = (raw->buff)+2;
unsigned short week;
double toc;
trace(4,"decode_ephem: sat=%2d\n",sat);
eph.crs = R4(&puiTmp[ 2]);
eph.deln = R4(&puiTmp[ 6]) * 1e+3;
eph.M0 = R8(&puiTmp[ 10]);
eph.cuc = R4(&puiTmp[ 18]);
eph.e = R8(&puiTmp[ 22]);
eph.cus = R4(&puiTmp[ 30]);
eph.A = pow(R8(&puiTmp[ 34]), 2);
eph.toes = R8(&puiTmp[ 42]) * 1e-3;
eph.cic = R4(&puiTmp[ 50]);
eph.OMG0 = R8(&puiTmp[ 54]);
eph.cis = R4(&puiTmp[ 62]);
eph.i0 = R8(&puiTmp[ 66]);
eph.crc = R4(&puiTmp[ 74]);
eph.omg = R8(&puiTmp[ 78]);
eph.OMGd = R8(&puiTmp[ 86]) * 1e+3;
eph.idot = R8(&puiTmp[ 94]) * 1e+3;
eph.tgd[0] = R4(&puiTmp[102]) * 1e-3;
toc = R8(&puiTmp[106]) * 1e-3;
eph.f2 = R4(&puiTmp[114]) * 1e+3;
eph.f1 = R4(&puiTmp[118]);
eph.f0 = R4(&puiTmp[122]) * 1e-3;
eph.sva = uraindex(I2(&puiTmp[126]));
eph.iode = I2(&puiTmp[128]);
eph.iodc = I2(&puiTmp[130]);
eph.code = I2(&puiTmp[132]);
eph.flag = I2(&puiTmp[134]);
week = I2(&puiTmp[136]);
eph.fit = 0;
if (week>=4096) {
trace(2,"nvs gps ephemeris week error: sat=%2d week=%d\n",sat,week);
return -1;
}
eph.week=adjgpsweek(week);
eph.toe=gpst2time(eph.week,eph.toes);
eph.toc=gpst2time(eph.week,toc);
eph.ttr=raw->time;
if (!strstr(raw->opt,"-EPHALL")) {
if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */
}
eph.sat=sat;
raw->nav.eph[sat-1]=eph;
raw->ephsat=sat;
return 2;
}
/* decode type 26: ionospheric deley corrections -----------------------------*/
static int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
{
int i,j,block,delay,give,band=getbitu(msg->msg,14,4);
trace(4,"decode_sbstype26:\n");
if (band>MAXBAND||sbsion[band].iodi!=(int)getbitu(msg->msg,217,2)) return 0;
block=getbitu(msg->msg,18,4);
for (i=0;i<15;i++) {
if ((j=block*15+i)>=sbsion[band].nigp) continue;
give=getbitu(msg->msg,22+i*13+9,4);
delay=getbitu(msg->msg,22+i*13,9);
sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow);
sbsion[band].igp[j].delay=delay==0x1FF?0.0f:delay*0.125f;
sbsion[band].igp[j].give=give+1;
if (sbsion[band].igp[j].give>=16) {
sbsion[band].igp[j].give=0;
}
}
trace(5,"decode_sbstype26: band=%d block=%d\n",band,block);
return 1;
}
/* decode type 2-5,0: fast corrections ---------------------------------------*/
static int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i,j,iodf,type,udre;
double prc,dt;
gtime_t t0;
trace(4,"decode_sbstype2:\n");
if (sbssat->iodp!=(int)getbitu(msg->msg,16,2)) return 0;
type=getbitu(msg->msg, 8,6);
iodf=getbitu(msg->msg,14,2);
for (i=0;i<13;i++) {
if ((j=13*((type==0?2:type)-2)+i)>=sbssat->nsat) break;
udre=getbitu(msg->msg,174+4*i,4);
t0 =sbssat->sat[j].fcorr.t0;
prc=sbssat->sat[j].fcorr.prc;
sbssat->sat[j].fcorr.t0=gpst2time(msg->week,msg->tow);
sbssat->sat[j].fcorr.prc=getbits(msg->msg,18+i*12,12)*0.125f;
sbssat->sat[j].fcorr.udre=udre+1;
dt=timediff(sbssat->sat[j].fcorr.t0,t0);
if (t0.time==0||dt<=0.0||18.0<dt||sbssat->sat[j].fcorr.ai==0) {
sbssat->sat[j].fcorr.rrc=0.0;
sbssat->sat[j].fcorr.dt=0.0;
}
else {
sbssat->sat[j].fcorr.rrc=(sbssat->sat[j].fcorr.prc-prc)/dt;
sbssat->sat[j].fcorr.dt=dt;
}
sbssat->sat[j].fcorr.iodf=iodf;
}
trace(5,"decode_sbstype2: type=%d iodf=%d\n",type,iodf);
return 1;
}
/* decode type 12: madoca orbit and clock correction -------------------------*/
static int decode_lextype12(const lexmsg_t *msg, nav_t *nav, gtime_t *tof)
{
rtcm_t rtcm={0};
double tow;
unsigned char buff[1200];
int i=0,j,k,n,week;
trace(3,"decode_lextype12:\n");
tow =getbitu(buff,i,20); i+=20;
week=getbitu(buff,i,13); i+=13;
*tof=gpst2time(week,tow);
/* convert lex type 12 to rtcm ssr message */
while ((n=lex2rtcm(msg->msg,i,buff))) {
rtcm.time=*tof;
for (j=0;j<n+6;j++) {
/* input rtcm ssr message */
if (input_rtcm3(&rtcm,buff[j])!=10) continue;
/* update ssr corrections in nav data */
for (k=0;k<MAXSAT;k++) {
if (!rtcm.ssr[k].update) continue;
nav->ssr[k]=rtcm.ssr[k];
rtcm.ssr[k].update=0;
}
}
i+=n*8;
}
return 1;
}
/* decode ionosphere correction field (ref [1] 5.7.2.2.1.3) ------------------*/
static int decode_lexion(const unsigned char *buff, int i, gtime_t tof,
nav_t *nav)
{
lexion_t ion={{0}};
int tow,week;
trace(3,"decode_lexion: tof=%s\n",time_str(tof,0));
tow=getbitu(buff,i,20); i+=20;
if (tow==0xFFFFF) { /* correction not available */
return i+192;
}
week=getbitu(buff,i,13); i+=13;
ion.t0=gpst2time(week,tow);
ion.tspan =getbitu(buff,i, 8)*60.0; i+= 8; /* time span (s) */
ion.pos0[0] =getbits(buff,i,19)*1E-5; i+=19; /* latitude (rad) */
ion.pos0[1] =getbits(buff,i,20)*1E-5; i+=20; /* longitude (rad) */
ion.coef[0][0]=getbits(buff,i,22)*1E-3; i+=22;
ion.coef[1][0]=getbits(buff,i,22)*1E-2; i+=22;
ion.coef[2][0]=getbits(buff,i,22)*1E-2; i+=22;
ion.coef[0][1]=getbits(buff,i,22)*1E-2; i+=22;
ion.coef[1][1]=getbits(buff,i,22)*1E-2; i+=22;
ion.coef[2][1]=getbits(buff,i,22)*1E-1; i+=22;
nav->lexion=ion;
trace(4,"t0=%s tspan=%.0f pos0=%.1f %.1f coef=%.3f %.3f %.3f %.3f %.3f %.3f\n",
time_str(ion.t0,0),ion.tspan,ion.pos0[0]*R2D,ion.pos0[1]*R2D,
ion.coef[0][0],ion.coef[1][0],ion.coef[2][0],ion.coef[0][1],
ion.coef[1][1],ion.coef[2][1]);
return i;
}
/* read stec grid data body --------------------------------------------------*/
static void read_stecb(FILE *fp, nav_t *nav)
{
gtime_t time;
double tow,pos[2],iono,rate,rms;
char buff[256],id[64];
int week,sat,slip;
trace(3,"read_stecb:\n");
while (fgets(buff,sizeof(buff),fp)) {
/* read position */
if (sscanf(buff,"$SPOS %d %lf %lf %lf",&week,&tow,pos,pos+1)>=4) {
/* add stec grid */
if (!add_grid(nav,pos)) break;
}
/* read ionos */
if (sscanf(buff,"$STEC %d %lf %s %d %lf %lf %lf",&week,&tow,id,&slip,
&iono,&rate,&rms)>=6) {
if (nav->nn<=0||!(sat=satid2no(id))) continue;
time=gpst2time(week,tow);
/* add grid data */
if (!add_data(nav->stec+nav->nn-1,time,sat,slip,iono,rate,rms)) {
break;
}
}
}
}
/* decode gps navigation data subframe 5 -------------------------------------*/
static void decode_gps_subfrm5(const unsigned char *buff, alm_t *alm)
{
double toas;
int i,sat,week,svid=getbitu(buff,50,6);
if (1<=svid&&svid<=24) { /* page 1-24 */
/* decode almanac */
sat=getbitu(buff,50,6);
if (1<=sat&&sat<=32) decode_almanac(buff,sat,alm);
}
else if (svid==51) { /* page 25 */
if (alm) {
i=56;
toas=getbitu(buff,i,8)*4096; i+=8;
week=getbitu(buff,i,8); i+=8;
week=adjgpsweek(week);
/* decode sv health */
for (sat=1;sat<=24;sat++) {
alm[sat-1].svh=getbitu(buff,i,6); i+=6;
}
for (sat=1;sat<=32;sat++) {
alm[sat-1].toas=toas;
alm[sat-1].week=week;
alm[sat-1].toa=gpst2time(week,toas);
}
}
}
}
/* 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);
}
/* decode type 9: geo navigation message -------------------------------------*/
static int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
{
seph_t seph={0};
int i,sat,t;
trace(4,"decode_sbstype9:\n");
if (!(sat=satno(SYS_SBS,msg->prn))) {
trace(2,"invalid prn in sbas type 9: prn=%3d\n",msg->prn);
return 0;
}
t=(int)getbitu(msg->msg,22,13)*16-(int)msg->tow%86400;
if (t<=-43200) t+=86400;
else if (t> 43200) t-=86400;
seph.sat=sat;
seph.t0 =gpst2time(msg->week,msg->tow+t);
seph.tof=gpst2time(msg->week,msg->tow);
seph.sva=getbitu(msg->msg,35,4);
seph.svh=seph.sva==15?1:0; /* unhealthy if ura==15 */
seph.pos[0]=getbits(msg->msg, 39,30)*0.08;
seph.pos[1]=getbits(msg->msg, 69,30)*0.08;
seph.pos[2]=getbits(msg->msg, 99,25)*0.4;
seph.vel[0]=getbits(msg->msg,124,17)*0.000625;
seph.vel[1]=getbits(msg->msg,141,17)*0.000625;
seph.vel[2]=getbits(msg->msg,158,18)*0.004;
seph.acc[0]=getbits(msg->msg,176,10)*0.0000125;
seph.acc[1]=getbits(msg->msg,186,10)*0.0000125;
seph.acc[2]=getbits(msg->msg,196,10)*0.0000625;
seph.af0=getbits(msg->msg,206,12)*P2_31;
seph.af1=getbits(msg->msg,218, 8)*P2_39/2.0;
i=msg->prn-MINPRNSBS;
if (!nav->seph||fabs(timediff(nav->seph[i].t0,seph.t0))<1E-3) { /* not change */
return 0;
}
nav->seph[NSATSBS+i]=nav->seph[i]; /* previous */
nav->seph[i]=seph; /* current */
trace(5,"decode_sbstype9: prn=%d\n",msg->prn);
return 1;
}
/* decode solution status ----------------------------------------------------*/
static int decode_solstat(char *buff, solstat_t *stat)
{
static const solstat_t stat0={{0}};
double tow,az,el,resp,resc;
int n,week,sat,frq,code=0,vsat,snr,fix,slip,lock,outc,slipc,rejc;
char id[32]="",*p;
int prmc;
trace(4,"decode_solstat: buff=%s\n",buff);
if (strstr(buff,"$SAT")!=buff) return 0;
for (p=buff,prmc=1;*p;p++) {
if (*p==',') {
*p=' ';
++prmc;
}
}
if(18==prmc) {
n=sscanf(buff,"$SAT%d%lf%s%d%d%lf%lf%lf%lf%d%d%d%d%d%d%d%d",
&week,&tow,id,&frq,&code,&az,&el,&resp,&resc,&vsat,&snr,&fix,&slip,
&lock,&outc,&slipc,&rejc);
}
else {
n=sscanf(buff,"$SAT%d%lf%s%d%lf%lf%lf%lf%d%d%d%d%d%d%d%d",
&week,&tow,id,&frq,&az,&el,&resp,&resc,&vsat,&snr,&fix,&slip,
&lock,&outc,&slipc,&rejc);
}
if (n<15) {
trace(2,"invalid format of solution status: %s\n",buff);
return 0;
}
if ((sat=satid2no(id))<=0) {
trace(2,"invalid satellite in solution status: %s\n",id);
return 0;
}
*stat=stat0;
stat->time=gpst2time(week,tow);
stat->sat =(unsigned char)sat;
stat->frq =(unsigned char)frq;
stat->code =(unsigned char)code;
stat->az =(float)(az*D2R);
stat->el =(float)(el*D2R);
stat->resp =(float)resp;
stat->resc =(float)resc;
stat->flag =(unsigned char)((vsat<<5)+(slip<<3)+fix);
stat->snr =(unsigned char)(snr*4.0+0.5);
stat->lock =(unsigned short)lock;
stat->outc =(unsigned short)outc;
stat->slipc=(unsigned short)slipc;
stat->rejc =(unsigned short)rejc;
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;
}
/* 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;
}
/* 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__);
}
/* decode id#21 navigation data (ecef) ---------------------------------------*/
static int decode_ss2ecef(raw_t *raw)
{
unsigned char *p=raw->buff+4;
trace(4,"decode_ss2ecef: len=%d\n",raw->len);
if (raw->len!=85) {
trace(2,"ss2 id#21 length error: len=%d\n",raw->len);
return -1;
}
raw->time=gpst2time(U2(p+8),R8(p));
return 0;
}
/* decode skytraq measurement time info --------------------------------------*/
static int decode_stqtime(raw_t *raw)
{
unsigned char *p=raw->buff+4;
double tow;
int week;
trace(4,"decode_stqtime: len=%d\n",raw->len);
raw->iod=U1(p+1);
week =U2(p+2);
tow =U4(p+4)*0.001;
raw->time=gpst2time(week,tow);
return 0;
}
/* 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);
}
/* decode solution time ------------------------------------------------------*/
static char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time)
{
double v[MAXFIELD];
char *p,*q,s[64]=" ";
int n,len;
trace(4,"decode_soltime:\n");
if (!strcmp(opt->sep,"\\t")) strcpy(s,"\t");
else if (*opt->sep) strcpy(s,opt->sep);
len=(int)strlen(s);
/* yyyy/mm/dd hh:mm:ss or yyyy mm dd hh:mm:ss */
if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf",v,v+1,v+2,v+3,v+4,v+5)>=6) {
if (v[0]<100.0) {
v[0]+=v[0]<80.0?2000.0:1900.0;
}
*time=epoch2time(v);
if (opt->times==TIMES_UTC) {
*time=utc2gpst(*time);
}
else if (opt->times==TIMES_JST) {
*time=utc2gpst(timeadd(*time,-9*3600.0));
}
if (!(p=strchr(buff,':'))||!(p=strchr(p+1,':'))) return NULL;
for (p++;isdigit((int)*p)||*p=='.';) p++;
return p+len;
}
if (opt->posf==SOLF_GSIF) {
if (sscanf(buff,"%lf %lf %lf %lf:%lf:%lf",v,v+1,v+2,v+3,v+4,v+5)<6) {
return NULL;
}
*time=timeadd(epoch2time(v),-12.0*3600.0);
if (!(p=strchr(buff,':'))||!(p=strchr(p+1,':'))) return NULL;
for (p++;isdigit((int)*p)||*p=='.';) p++;
return p+len;
}
/* wwww ssss */
for (p=buff,n=0;n<2;p=q+len) {
if ((q=strstr(p,s))) *q='\0';
if (*p) v[n++]=atof(p);
if (!q) break;
}
if (n>=2&&0.0<=v[0]&&v[0]<=3000.0&&0.0<=v[1]&&v[1]<604800.0) {
*time=gpst2time((int)v[0],v[1]);
return p;
}
return NULL;
}
// 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);
}
/* decode qzss navigation data subframe 4/5 ----------------------------------*/
static void decode_qzs_subfrm45(const unsigned char *buff, alm_t *alm,
double *ion, double *utc, int *leaps)
{
int i,j,sat,toas,week,svid=getbitu(buff,50,6);
if (1<=svid&&svid<=5) { /* qzss almanac */
if (!(sat=satno(SYS_QZS,192+svid))) return;
decode_almanac(buff,sat,alm);
}
else if (svid==51) { /* qzss health */
if (alm) {
i=56;
toas=getbitu(buff,i,8)*4096; i+=8;
week=getbitu(buff,i,8); i+=8;
week=adjgpsweek(week);
for (j=0;j<5;j++) {
if (!(sat=satno(SYS_QZS,193+j))) continue;
alm[sat-1].toas=toas;
alm[sat-1].week=week;
alm[sat-1].toa=gpst2time(week,toas);
alm[sat-1].svh=getbitu(buff,i,6); i+=6;
}
}
}
else if (svid==56) { /* ion/utc parameters */
if (ion) {
i=56;
ion[0]=getbits(buff,i, 8)*P2_30; i+= 8;
ion[1]=getbits(buff,i, 8)*P2_27; i+= 8;
ion[2]=getbits(buff,i, 8)*P2_24; i+= 8;
ion[3]=getbits(buff,i, 8)*P2_24; i+= 8;
ion[4]=getbits(buff,i, 8)*pow(2,11); i+= 8;
ion[5]=getbits(buff,i, 8)*pow(2,14); i+= 8;
ion[6]=getbits(buff,i, 8)*pow(2,16); i+= 8;
ion[7]=getbits(buff,i, 8)*pow(2,16);
}
if (utc) {
i=120;
utc[1]=getbits(buff,i,24)*P2_50; i+=24;
utc[0]=getbits(buff,i,32)*P2_30; i+=32;
utc[2]=getbits(buff,i, 8)*pow(2,12); i+= 8;
utc[3]=getbitu(buff,i, 8);
}
}
}
/* decode binex mesaage 0x01-03: decoded sbas ephmemeris ---------------------*/
static int decode_bnx_01_03(raw_t *raw, unsigned char *buff, int len)
{
seph_t seph={0};
unsigned char *p=buff;
double tow,tod,tof;
int prn,week,iodn;
trace(4,"binex 0x01-03: len=%d\n",len);
if (len>=98) {
prn =U1(p); p+=1;
week =U2(p); p+=2;
tow =U4(p); p+=4;
seph.af0 =R8(p); p+=8;
tod =R4(p); p+=4;
tof =U4(p); p+=4;
seph.pos[0]=R8(p)*1E3; p+=8;
seph.vel[0]=R8(p)*1E3; p+=8;
seph.acc[0]=R8(p)*1E3; p+=8;
seph.pos[1]=R8(p)*1E3; p+=8;
seph.vel[1]=R8(p)*1E3; p+=8;
seph.acc[1]=R8(p)*1E3; p+=8;
seph.pos[2]=R8(p)*1E3; p+=8;
seph.vel[2]=R8(p)*1E3; p+=8;
seph.acc[2]=R8(p)*1E3; p+=8;
seph.svh =U1(p); p+=1;
seph.sva =U1(p); p+=1;
iodn =U1(p);
}
else {
trace(2,"binex 0x01-03 length error: len=%d\n",len);
return -1;
}
if (!(seph.sat=satno(SYS_SBS,prn))) {
trace(2,"binex 0x01-03 satellite error: prn=%d\n",prn);
return -1;
}
seph.t0=gpst2time(week,tow);
seph.tof=adjweek(seph.t0,tof);
if (!strstr(raw->opt,"-EPHALL")) {
if (fabs(timediff(seph.t0,raw->nav.seph[prn-MINPRNSBS].t0))<1.0&&
seph.sva==raw->nav.seph[prn-MINPRNSBS].sva) return 0; /* unchanged */
}
raw->nav.seph[prn-MINPRNSBS]=seph;
raw->ephsat=seph.sat;
return 2;
}
/* 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;
}
/* convert sbas message ------------------------------------------------------*/
static void convsbs(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n)
{
gtime_t ts1,te1;
int msg,prn,sat,sys;
trace(3,"convsbs :\n");
ts1=opt->ts; if (ts1.time!=0) ts1=timeadd(ts1,-MAXDTOE);
te1=opt->te; if (te1.time!=0) te1=timeadd(te1, MAXDTOE);
msg=sbsupdatecorr(&str->raw.sbsmsg,str->nav);
prn=str->raw.sbsmsg.prn;
if (MINPRNSBS<=prn&&prn<=MAXPRNSBS) sys=SYS_SBS;
else if (MINPRNQZS<=prn&&prn<=MAXPRNQZS) sys=SYS_QZS;
else {
trace(2,"sbas message satellite error: prn=%d\n",prn);
return;
}
if (!(sat=satno(sys,prn))||opt->exsats[sat-1]==1) return;
if (ofp[6]) { /* output sbas log */
if (screent(gpst2time(str->raw.sbsmsg.week,str->raw.sbsmsg.tow),opt->ts,
opt->te,0.0)) {
sbsoutmsg(ofp[6],&str->raw.sbsmsg); n[6]++;
}
}
if (!(opt->navsys&SYS_SBS)||msg!=9||
!screent(str->time,ts1,te1,0.0)) return;
if (ofp[1]&&opt->rnxver>2.99) {
/* output rinex nav */
outrnxhnavb(ofp[1],opt,str->nav->seph+prn-MINPRNSBS);
n[1]++;
}
if (ofp[3]&&opt->rnxver<=2.99) {
/* output rinex hnav */
outrnxhnavb(ofp[3],opt,str->nav->seph+prn-MINPRNSBS);
n[3]++;
}
}
/* decode type 14: gps time of week ------------------------------------------*/
static int decode_type14(rtcm_t *rtcm)
{
double zcnt;
int i=48,week,hour,leaps;
rtklib_trace(4,"decode_type14: len=%d\n",rtcm->len);
zcnt=getbitu(rtcm->buff,24,13);
if (i+24<=rtcm->len*8) {
week =getbitu(rtcm->buff,i,10); i+=10;
hour =getbitu(rtcm->buff,i, 8); i+= 8;
leaps=getbitu(rtcm->buff,i, 6);
}
else {
rtklib_trace(2,"rtcm2 14 length error: len=%d\n",rtcm->len);
return -1;
}
rtcm->time=gpst2time(week,hour*3600.0+zcnt*0.6);
rtcm->nav.leaps=leaps;
return 6;
}
/* decode id#67 sbas data ----------------------------------------------------*/
static int decode_ss2sbas(raw_t *raw)
{
gtime_t time;
int i,prn;
unsigned char *p=raw->buff+4;
trace(4,"decode_ss2sbas: len=%d\n",raw->len);
if (raw->len!=54) {
trace(2,"ss2 id#67 length error: len=%d\n",raw->len);
return -1;
}
prn=U4(p+12);
if (prn<MINPRNSBS||MAXPRNSBS<prn) return 0;
raw->sbsmsg.week=U4(p);
raw->sbsmsg.tow=(int)R8(p+4);
time=gpst2time(raw->sbsmsg.week,raw->sbsmsg.tow);
raw->sbsmsg.prn=prn;
for (i=0;i<29;i++) raw->sbsmsg.msg[i]=p[16+i];
return 3;
}
