/* decode type 1: prn masks --------------------------------------------------*/
static int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i,n,sat;
trace(4,"decode_sbstype1:\n");
for (i=1,n=0;i<=210&&n<MAXSAT;i++) {
if (getbitu(msg->msg,13+i,1)) {
if (i<= 37) sat=satno(SYS_GPS,i); /* 0- 37: gps */
else if (i<= 61) sat=satno(SYS_GLO,i-37); /* 38- 61: glonass */
else if (i<=119) sat=0; /* 62-119: future gnss */
else if (i<=138) sat=satno(SYS_SBS,i); /* 120-138: geo/waas */
else if (i<=182) sat=0; /* 139-182: reserved */
else if (i<=192) sat=satno(SYS_SBS,i+10); /* 183-192: qzss ref [2] */
else if (i<=202) sat=satno(SYS_QZS,i); /* 193-202: qzss ref [2] */
else sat=0; /* 203- : reserved */
sbssat->sat[n++].sat=sat;
}
}
sbssat->iodp=getbitu(msg->msg,224,2);
sbssat->nsat=n;
trace(5,"decode_sbstype1: nprn=%d iodp=%d\n",n,sbssat->iodp);
return 1;
}
/* decode ephemeris and sv clock field (ref [1] 5.7.2.2.1.2) -----------------*/
static int decode_lexeph(const unsigned char *buff, int i, gtime_t toe,
nav_t *nav)
{
lexeph_t eph={{0}};
gtime_t tof;
unsigned char health;
int j,prn,sat;
trace(3,"decode_lexeph: toe=%s\n",time_str(toe,0));
prn =getbitu(buff,i, 8); i+= 8;
eph.ura =getbitu(buff,i, 4); i+= 4;
eph.pos [0]=getbits_33(buff,i)*P2_6; i+=33;
eph.pos [1]=getbits_33(buff,i)*P2_6; i+=33;
eph.pos [2]=getbits_33(buff,i)*P2_6; i+=33;
eph.vel [0]=getbits(buff,i,28)*P2_15; i+=28;
eph.vel [1]=getbits(buff,i,28)*P2_15; i+=28;
eph.vel [2]=getbits(buff,i,28)*P2_15; i+=28;
eph.acc [0]=getbits(buff,i,24)*P2_24; i+=24;
eph.acc [1]=getbits(buff,i,24)*P2_24; i+=24;
eph.acc [2]=getbits(buff,i,24)*P2_24; i+=24;
eph.jerk[0]=getbits(buff,i,20)*P2_32; i+=20;
eph.jerk[1]=getbits(buff,i,20)*P2_32; i+=20;
eph.jerk[2]=getbits(buff,i,20)*P2_32; i+=20;
eph.af0 =getbits(buff,i,26)*P2_35; i+=26;
eph.af1 =getbits(buff,i,20)*P2_48; i+=20;
eph.tgd =getbits(buff,i,13)*P2_35; i+=13;
for (j=0;j<7;j++) {
eph.isc[j]=getbits(buff,i,13)*P2_35; i+=13;
}
if (prn==255) return i; /* no satellite */
if ( 1<=prn&&prn<= 32) sat=satno(SYS_GPS,prn);
else if (193<=prn&&prn<=195) sat=satno(SYS_QZS,prn);
else {
trace(2,"lex ephemeris prn error prn=%d\n",prn);
return i;
}
eph.toe=toe;
eph.sat=sat;
tof =nav->lexeph[sat-1].tof;
health=nav->lexeph[sat-1].health;
nav->lexeph[sat-1]=eph;
nav->lexeph[sat-1].tof =tof;
nav->lexeph[sat-1].health=health;
trace(4,"sat=%2d toe=%s pos=%.3f %.3f %.3f vel=%.5f %.5f %.5f\n",
sat,time_str(toe,0),eph.pos[0],eph.pos[1],eph.pos[2],
eph.vel[0],eph.vel[1],eph.vel[2]);
trace(4,"clk=%11.3f %8.5f tgd=%7.3f\n",eph.af0*1E9,eph.af1*1E9,
eph.tgd*1E9);
trace(4,"isc=%6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f\n",
eph.isc[0]*1E9,eph.isc[1]*1E9,eph.isc[2]*1E9,eph.isc[3]*1E9,
eph.isc[4]*1E9,eph.isc[5]*1E9,eph.isc[6]*1E9);
return i;
}
/* 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 NVS epehemerides in clear ------------------------------------------*/
static int decode_xf7eph(raw_t *raw)
{
int prn,sat,sys;
unsigned char *p=raw->buff;
trace(4,"decode_xf7eph: len=%d\n",raw->len);
if ((raw->len)<93) {
trace(2,"nvs xf7eph length error: len=%d\n",raw->len);
return -1;
}
sys = (U1(p+2)==1)?SYS_GPS:((U1(p+2)==2)?SYS_GLO:SYS_NONE);
prn = U1(p+3);
if (!(sat=satno(sys==1?SYS_GPS:SYS_GLO,prn))) {
trace(2,"nvs xf7eph satellite number error: prn=%d\n",prn);
return -1;
}
if (sys==SYS_GPS) {
return decode_gpsephem(sat,raw);
}
else if (sys==SYS_GLO) {
return decode_gloephem(sat,raw);
}
return 0;
}
/* decode skytraq raw channel mesurement -------------------------------------*/
static int decode_stqraw(raw_t *raw)
{
int i,j,iod,prn,sys,sat,n=0,nsat;
unsigned char *p=raw->buff+4,ind;
trace(4,"decode_stqraw: len=%d\n",raw->len);
iod=U1(p+1);
if (iod!=raw->iod) {
trace(2,"stq raw iod error: iod=%d %d\n",iod,raw->iod);
return -1;
}
nsat=U1(p+2);
if (raw->len<8+23*nsat) {
trace(2,"stq raw length error: len=%d nsat=%d\n",raw->len,nsat);
return -1;
}
for (i=0,p+=3; i<nsat&&i<MAXOBS; i++,p+=23) {
ind =U1(p+22);
prn =U1(p);
raw->obs.data[n].SNR[0]=(unsigned char)(U1(p+1)*4.0+0.5);
raw->obs.data[n].P[0] =(ind&0x1)?R8r(p+ 2):0.0;
raw->obs.data[n].L[0] =(ind&0x4)?R8r(p+10):0.0;
raw->obs.data[n].D[0] =(ind&0x2)?R4r(p+18):0.0f;
raw->obs.data[n].LLI[0]=(ind&0x8)?1:0; /* slip */
raw->obs.data[n].code[0]=CODE_L1C;
/* receiver dependent options */
if (strstr(raw->opt,"-invcp")) {
raw->obs.data[n].L[0]=-raw->obs.data[n].L[0];
}
if (MINPRNGPS<=prn&&prn<=MAXPRNGPS) {
sys=SYS_GPS;
}
else if (MINPRNGLO<=prn-64&&prn-64<=MAXPRNGLO) {
sys=SYS_GLO;
prn-=64;
}
else {
trace(2,"stq raw satellite number error: prn=%d\n",prn);
continue;
}
if (!(sat=satno(sys,prn))) {
trace(2,"stq raw satellite number error: sys=%d prn=%d\n",sys,prn);
continue;
}
raw->obs.data[n].time=raw->time;
raw->obs.data[n].sat =sat;
for (j=1; j<NFREQ; j++) {
raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
raw->obs.data[n].D[j]=0.0;
raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
raw->obs.data[n].code[j]=CODE_NONE;
}
n++;
}
raw->obs.n=n;
return n>0?1:0;
}
/* decode skytraq glonass string buffer --------------------------------------*/
static int decode_stqglostr(raw_t *raw)
{
int i,prn,sat,strno;
unsigned char *p=raw->buff+4;
trace(4,"decode_stqglostr: len=%d\n",raw->len);
if (raw->len<19) {
trace(2,"stq glo string length error: len=%d\n",raw->len);
return -1;
}
prn=U1(p+1)-64;
if (!(sat=satno(SYS_GLO,prn))) {
trace(2,"stq glo string satellite number error: prn=%d\n",prn);
return -1;
}
strno=U1(p+2);
if (strno<1||4<strno) {
trace(2,"stq glo string number error: prn=%d strno=%d\n",prn,strno);
return -1;
}
for (i=0; i<9; i++) {
raw->subfrm[sat-1][strno*9-1-i]=p[3+i];
}
if (strno<5) return 0;
return 0;
}
/* read sp3 header -----------------------------------------------------------*/
static int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
double *bfact, char *tsys)
{
int i,j,k=0,ns=0,sys,prn;
char buff[1024];
trace(3,"readsp3h:\n");
for (i=0;i<22;i++) {
if (!fgets(buff,sizeof(buff),fp)) break;
if (i==0) {
*type=buff[2];
if (str2time(buff,3,28,time)) return 0;
}
else if (2<=i&&i<=6) {
if (i==2) {
ns=(int)str2num(buff,4,2);
}
for (j=0;j<17&&k<ns;j++) {
sys=code2sys(buff[9+3*j]);
prn=(int)str2num(buff,10+3*j,2);
if (k<MAXSAT) sats[k++]=satno(sys,prn);
}
}
else if (i==12) {
strncpy(tsys,buff+9,3); tsys[3]='\0';
}
else if (i==14) {
bfact[0]=str2num(buff, 3,10);
bfact[1]=str2num(buff,14,12);
}
}
return ns;
}
/* decode ublox rxm-sfrb: subframe buffer ------------------------------------*/
static int decode_rxmsfrb(raw_t *raw)
{
unsigned int words[10];
int i,prn,sat,sys,id;
unsigned char *p=raw->buff+6;
trace(4,"decode_rxmsfrb: len=%d\n",raw->len);
if (raw->len<42) {
trace(2,"ubx rxmsfrb length error: len=%d\n",raw->len);
return -1;
}
prn=U1(p+1);
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"ubx rxmsfrb satellite number error: prn=%d\n",prn);
return -1;
}
sys=satsys(sat,&prn);
if (sys==SYS_GPS) {
id=save_subfrm(sat,raw);
if (id==3) return decode_ephem(sat,raw);
if (id==4) return decode_alm1 (sat,raw);
if (id==5) return decode_alm2 (sat,raw);
return 0;
}
else if (sys==SYS_SBS) {
for (i=0,p+=2;i<10;i++,p+=4) words[i]=U4(p);
return sbsdecodemsg(raw->time,prn,words,&raw->sbsmsg)?3:0;
}
return 0;
}
/* decode type 1/9: differential gps correction/partial correction set -------*/
static int decode_type1(rtcm_t *rtcm)
{
int i=48,fact,udre,prn,sat,iod;
double prc,rrc;
rtklib_trace(4,"decode_type1: len=%d\n",rtcm->len);
while (i+40<=rtcm->len*8) {
fact=getbitu(rtcm->buff,i, 1); i+= 1;
udre=getbitu(rtcm->buff,i, 2); i+= 2;
prn =getbitu(rtcm->buff,i, 5); i+= 5;
prc =getbits(rtcm->buff,i,16); i+=16;
rrc =getbits(rtcm->buff,i, 8); i+= 8;
iod =getbits(rtcm->buff,i, 8); i+= 8;
if (prn==0) prn=32;
if (prc==0x80000000||rrc==0xFFFF8000) {
rtklib_trace(2,"rtcm2 1 prc/rrc indicates satellite problem: prn=%d\n",prn);
continue;
}
if (rtcm->dgps) {
sat=satno(SYS_GPS,prn);
rtcm->dgps[sat-1].t0=rtcm->time;
rtcm->dgps[sat-1].prc=prc*(fact?0.32:0.02);
rtcm->dgps[sat-1].rrc=rrc*(fact?0.032:0.002);
rtcm->dgps[sat-1].iod=iod;
rtcm->dgps[sat-1].udre=udre;
}
}
return 7;
}
/* decode skytraq subframe buffer --------------------------------------------*/
static int decode_stqsfrb(raw_t *raw)
{
int prn,sat,sys,id;
unsigned char *p=raw->buff+4;
trace(4,"decode_stqsfrb: len=%d\n",raw->len);
if (raw->len<40) {
trace(2,"stq subframe length error: len=%d\n",raw->len);
return -1;
}
prn=U1(p+1);
if (prn>MAXPRNGPS) prn+=MINPRNSBS-38;
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"stq subframe satellite number error: prn=%d\n",prn);
return -1;
}
sys=satsys(sat,&prn);
if (sys==SYS_GPS) {
id=save_subfrm(sat,raw);
if (id==3) return decode_ephem(sat,raw);
if (id==4) return decode_alm1 (sat,raw);
if (id==5) return decode_alm2 (sat,raw);
return 0;
}
return 0;
}
/* 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 binex mesaage 0x01-02: decoded glonass ephmemeris ------------------*/
static int decode_bnx_01_02(raw_t *raw, unsigned char *buff, int len)
{
geph_t geph={0};
unsigned char *p=buff;
double tod,tof,tau_gps;
int prn,day,leap;
trace(4,"binex 0x01-02: len=%d\n",len);
if (len>=119) {
prn =U1(p)+1; p+=1;
day =U2(p); p+=2;
tod =U4(p); p+=4;
geph.taun =-R8(p); p+=8;
geph.gamn =R8(p); p+=8;
tof =U4(p); p+=4;
geph.pos[0]=R8(p)*1E3; p+=8;
geph.vel[0]=R8(p)*1E3; p+=8;
geph.acc[0]=R8(p)*1E3; p+=8;
geph.pos[1]=R8(p)*1E3; p+=8;
geph.vel[1]=R8(p)*1E3; p+=8;
geph.acc[1]=R8(p)*1E3; p+=8;
geph.pos[2]=R8(p)*1E3; p+=8;
geph.vel[2]=R8(p)*1E3; p+=8;
geph.acc[2]=R8(p)*1E3; p+=8;
geph.svh =U1(p)&0x1; p+=1;
geph.frq =I1(p); p+=1;
geph.age =U1(p); p+=1;
leap =U1(p); p+=1;
tau_gps =R8(p); p+=8;
geph.dtaun =R8(p);
}
else {
trace(2,"binex 0x01-02: length error len=%d\n",len);
return -1;
}
if (!(geph.sat=satno(SYS_GLO,prn))) {
trace(2,"binex 0x01-02: satellite error prn=%d\n",prn);
return -1;
}
if (raw->time.time==0) return 0;
geph.toe=utc2gpst(adjday(raw->time,tod-10800.0));
geph.tof=utc2gpst(adjday(raw->time,tof-10800.0));
geph.iode=(int)(fmod(tod+10800.0,86400.0)/900.0+0.5);
if (!strstr(raw->opt,"-EPHALL")) {
if (fabs(timediff(geph.toe,raw->nav.geph[prn-MINPRNGLO].toe))<1.0&&
geph.svh==raw->nav.geph[prn-MINPRNGLO].svh) return 0; /* unchanged */
}
raw->nav.geph[prn-1]=geph;
raw->ephsat=geph.sat;
return 2;
}
/* decode type 18: rtk uncorrected carrier-phase -----------------------------*/
static int decode_type18(rtcm_t *rtcm)
{
gtime_t time;
double usec,cp,tt;
int i=48,index,freq,sync=1,code,sys,prn,sat,loss;
rtklib_trace(4,"decode_type18: len=%d\n",rtcm->len);
if (i+24<=rtcm->len*8) {
freq=getbitu(rtcm->buff,i, 2); i+= 2+2;
usec=getbitu(rtcm->buff,i,20); i+=20;
}
else {
rtklib_trace(2,"rtcm2 18 length error: len=%d\n",rtcm->len);
return -1;
}
if (freq&0x1) {
rtklib_trace(2,"rtcm2 18 not supported frequency: freq=%d\n",freq);
return -1;
}
freq>>=1;
while (i+48<=rtcm->len*8&&rtcm->obs.n<MAXOBS) {
sync=getbitu(rtcm->buff,i, 1); i+= 1;
code=getbitu(rtcm->buff,i, 1); i+= 1;
sys =getbitu(rtcm->buff,i, 1); i+= 1;
prn =getbitu(rtcm->buff,i, 5); i+= 5+3;
loss=getbitu(rtcm->buff,i, 5); i+= 5;
cp =getbits(rtcm->buff,i,32); i+=32;
if (prn==0) prn=32;
if (!(sat=satno(sys?SYS_GLO:SYS_GPS,prn))) {
rtklib_trace(2,"rtcm2 18 satellite number error: sys=%d prn=%d\n",sys,prn);
continue;
}
time=timeadd(rtcm->time,usec*1E-6);
if (sys) time=utc2gpst(time); /* convert glonass time to gpst */
tt=timediff(rtcm->obs.data[0].time,time);
if (rtcm->obsflag||fabs(tt)>1E-9) {
rtcm->obs.n=rtcm->obsflag=0;
}
if ((index=obsindex(&rtcm->obs,time,sat))>=0) {
rtcm->obs.data[index].L[freq]=-cp/256.0;
rtcm->obs.data[index].LLI[freq]=rtcm->loss[sat-1][freq]!=loss;
rtcm->obs.data[index].code[freq]=
!freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C);
rtcm->loss[sat-1][freq]=loss;
}
}
rtcm->obsflag=!sync;
return sync?0:1;
}
/* decode signal health field (ref [1] 5.7.2.2.1.1) --------------------------*/
static int decode_lexhealth(const unsigned char *buff, int i, gtime_t tof,
nav_t *nav)
{
int j,sat;
unsigned char health;
trace(3,"decode_lexhealth: tof=%s\n",time_str(tof,0));
for (j=0;j<35;j++) {
health=getbitu(buff,i,5); i+= 5;
if (j<3) sat=satno(SYS_QZS,j+193);
else sat=satno(SYS_GPS,j-2);
if (!sat) continue;
nav->lexeph[sat-1].tof=tof;
nav->lexeph[sat-1].health=health;
trace(4,"sat=%2d health=%d\n",sat,health);
}
return i;
}
/* decode GLONASS navigation data (string 4) -----------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_g1s4(const uint8_t *buff, sdreph_t *eph)
{
eph->geph.taun =getbits_glo(buff, 5,22)*P2_30;
eph->geph.dtaun =getbits_glo(buff,27, 5)*P2_30;
eph->geph.age =getbitu(buff,32, 5);
eph->geph.sva =getbitu(buff,52, 4);
eph->nt =getbitu(buff,59,11);
eph->prn =getbitu(buff,70, 5);
eph->geph.sat =satno(SYS_GLO,eph->prn);
/* ephemeris counter */
eph->cnt++;
}
/* decode raw obs data -------------------------------------------------------*/
static int decode_gw10raw(raw_t *raw)
{
double tow,tows,toff,pr,cp;
int i,j,n,prn,flg,sat,snr;
unsigned char *p=raw->buff+2;
trace(4,"decode_gw10raw: len=%d\n",raw->len);
tow=R8(p);
tows=floor(tow*1000.0+0.5)/1000.0; /* round by 10ms */
toff=CLIGHT*(tows-tow); /* time tag offset (m) */
if (!adjweek(raw,tows)) {
trace(2,"decode_gw10raw: no gps week infomation\n");
return 0;
}
for (i=n=0,p+=8;i<16&&n<MAXOBS;i++,p+=23) {
if (U1(p+1)!=1) continue;
prn=U1(p);
if (!(sat=satno(prn<=MAXPRNGPS?SYS_GPS:SYS_SBS,prn))) {
trace(2,"gw10raw satellite number error: prn=%d\n",prn);
continue;
}
pr =R8(p+ 2)-toff;
snr=U2(p+16);
cp =-(int)(U4(p+18))/256.0-toff/lam_carr[0];
flg=U1(p+22);
if (flg&0x3) {
trace(2,"gw10raw raw data invalid: prn=%d\n",prn);
continue;
}
raw->obs.data[n].time=raw->time;
raw->obs.data[n].sat =sat;
raw->obs.data[n].P[0]=pr;
raw->obs.data[n].L[0]=(flg&0x80)?0.0:((flg&0x40)?cp-0.5:cp);
raw->obs.data[n].D[0]=0.0;
raw->obs.data[n].SNR[0]=(unsigned char)(snr*4.0+0.5);
raw->obs.data[n].LLI[0]=(flg&0x80)?1:0;
raw->obs.data[n].code[0]=CODE_L1C;
for (j=1;j<NFREQ;j++) {
raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
raw->obs.data[n].D[j]=0.0;
raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
raw->obs.data[n].code[j]=CODE_NONE;
}
n++;
}
raw->obs.n=n;
return 1;
}
/* 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;
}
/* 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 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 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;
}
/* update glonass frequency channel number in raw data struct ----------------*/
static void updatefcn(rtksvr_t *svr)
{
int i,j,sat,frq;
for (i=0;i<MAXPRNGLO;i++) {
sat=satno(SYS_GLO,i+1);
for (j=0,frq=-999;j<3;j++) {
if (svr->raw[j].nav.geph[i].sat!=sat) continue;
frq=svr->raw[j].nav.geph[i].frq;
}
if (frq<-7||frq>6) continue;
for (j=0;j<3;j++) {
if (svr->raw[j].nav.geph[i].sat==sat) continue;
svr->raw[j].nav.geph[i].sat=sat;
svr->raw[j].nav.geph[i].frq=frq;
}
}
}
/* decode skytraq subframe buffer --------------------------------------------*/
static int decode_stqsfrb(raw_t *raw)
{
int prn,sat,id;
unsigned char *p=raw->buff+4;
trace(4,"decode_stqsfrb: len=%d\n",raw->len);
if (raw->len<40) {
trace(2,"stq subframe length error: len=%d\n",raw->len);
return -1;
}
prn=U1(p+1);
if (!(sat=satno(SYS_GPS,prn))) {
trace(2,"stq subframe satellite number error: prn=%d\n",prn);
return -1;
}
id=save_subfrm(sat,raw);
if (id==3) return decode_ephem(sat,raw);
if (id==4) return decode_alm1 (sat,raw);
if (id==5) return decode_alm2 (sat,raw);
return 0;
}
/* decode raw ephemereris ----------------------------------------------------*/
static int decode_gw10reph(raw_t *raw)
{
eph_t eph={0};
double tow;
int i,week,prn,sat;
unsigned char *p=raw->buff+2,buff[90];
trace(4,"decode_gw10reph: len=%d\n",raw->len);
prn=U1(p);
if (!(sat=satno(SYS_GPS,prn))) {
trace(2,"gw10 raw ephemeris satellite number error: prn=%d\n",prn);
return -1;
}
for (i=0;i<90;i++) {
buff[i]=*(p+1+i);
}
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,"gw10 raw ephemeris navigation frame error: prn=%d\n",prn);
return -1;
}
/* set time if no time avaliable */
if (raw->time.time==0) {
tow=getbitu(buff,24,17)*6.0;
week=getbitu(buff,48,10)+OFFWEEK;
raw->time=timeadd(gpst2time(week,tow),24.0);
}
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 gps message --------------------------------------------------------*/
static int decode_gw10gps(raw_t *raw)
{
eph_t eph={0};
double tow,ion[8]={0},utc[4]={0};
unsigned int buff=0;
int i,prn,sat,id,leaps;
unsigned char *p=raw->buff+2,subfrm[30];
trace(4,"decode_gw10gps: len=%d\n",raw->len);
tow=U4(p)/1000.0; p+=4;
prn=U1(p); p+=1;
if (!(sat=satno(SYS_GPS,prn))) {
trace(2,"gw10 gps satellite number error: tow=%.1f prn=%d\n",tow,prn);
return -1;
}
for (i=0;i<10;i++) {
buff=(buff<<30)|U4(p); p+=4;
/* check parity of word */
if (!check_parity(buff,subfrm+i*3)) {
trace(2,"gw10 gps frame parity error: tow=%.1f prn=%2d word=%2d\n",
tow,prn,i+1);
return -1;
}
}
id=getbitu(subfrm,43,3); /* subframe id */
if (id<1||5<id) {
trace(2,"gw10 gps frame id error: tow=%.1f prn=%2d id=%d\n",tow,prn,id);
return -1;
}
for (i=0;i<30;i++) raw->subfrm[sat-1][i+(id-1)*30]=subfrm[i];
if (id==3) { /* decode ephemeris */
if (decode_frame(raw->subfrm[sat-1] ,&eph,NULL,NULL,NULL,NULL)!=1||
decode_frame(raw->subfrm[sat-1]+30,&eph,NULL,NULL,NULL,NULL)!=2||
decode_frame(raw->subfrm[sat-1]+60,&eph,NULL,NULL,NULL,NULL)!=3) {
return 0;
}
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;
}
else if (id==4) { /* decode ion-utc parameters */
if (decode_frame(subfrm,NULL,NULL,ion,utc,&leaps)!=4) {
return 0;
}
if (norm(ion,8)>0.0&&norm(utc,4)>0.0&&leaps!=0) {
for (i=0;i<8;i++) raw->nav.ion_gps[i]=ion[i];
for (i=0;i<4;i++) raw->nav.utc_gps[i]=utc[i];
raw->nav.leaps=leaps;
return 9;
}
}
return 0;
}
/* decode binex mesaage 0x01-06: decoded qzss ephmemeris ---------------------*/
static int decode_bnx_01_06(raw_t *raw, unsigned char *buff, int len)
{
eph_t eph={0};
unsigned char *p=buff;
double tow,ura,sqrtA;
int prn,flag;
trace(4,"binex 0x01-06: len=%d\n",len);
if (len>=127) {
prn =U1(p); p+=1;
eph.week =U2(p); p+=2;
tow =I4(p); p+=4;
eph.toes =I4(p); p+=4;
eph.tgd[0]=R4(p); p+=4;
eph.iodc =I4(p); p+=4;
eph.f2 =R4(p); p+=4;
eph.f1 =R4(p); p+=4;
eph.f0 =R4(p); p+=4;
eph.iode =I4(p); p+=4;
eph.deln =R4(p)*SC2RAD; p+=4;
eph.M0 =R8(p); p+=8;
eph.e =R8(p); p+=8;
sqrtA =R8(p); p+=8;
eph.cic =R4(p); p+=4;
eph.crc =R4(p); p+=4;
eph.cis =R4(p); p+=4;
eph.crs =R4(p); p+=4;
eph.cuc =R4(p); p+=4;
eph.cus =R4(p); p+=4;
eph.OMG0 =R8(p); p+=8;
eph.omg =R8(p); p+=8;
eph.i0 =R8(p); p+=8;
eph.OMGd =R4(p)*SC2RAD; p+=4;
eph.idot =R4(p)*SC2RAD; p+=4;
ura =R4(p)*0.1; p+=4;
eph.svh =U2(p); p+=2;
flag =U2(p);
}
else {
trace(2,"binex 0x01-06: length error len=%d\n",len);
return -1;
}
if (!(eph.sat=satno(SYS_QZS,prn))) {
trace(2,"binex 0x01-06: satellite error prn=%d\n",prn);
return 0;
}
eph.A=sqrtA*sqrtA;
eph.toe=gpst2time(eph.week,eph.toes);
eph.toc=gpst2time(eph.week,eph.toes);
eph.ttr=adjweek(eph.toe,tow);
eph.fit=(flag&0x01)?0.0:2.0; /* 0:2hr,1:>2hr */
eph.sva=uraindex(ura);
eph.code=2; /* codes on L2 channel */
if (!strstr(raw->opt,"-EPHALL")) {
if (raw->nav.eph[eph.sat-1].iode==eph.iode&&
raw->nav.eph[eph.sat-1].iodc==eph.iodc) return 0; /* unchanged */
}
raw->nav.eph[eph.sat-1]=eph;
raw->ephsat=eph.sat;
return 2;
}
/* decode binex mesaage 0x01-05: decoded beidou-2/compass ephmemeris ---------*/
static int decode_bnx_01_05(raw_t *raw, unsigned char *buff, int len)
{
eph_t eph={0};
unsigned char *p=buff;
double tow,toc,sqrtA;
int prn,flag1,flag2;
trace(4,"binex 0x01-05: len=%d\n",len);
if (len>=117) {
prn =U1(p); p+=1;
eph.week =U2(p); p+=2;
tow =I4(p); p+=4;
toc =I4(p); p+=4;
eph.toes =I4(p); p+=4;
eph.f2 =R4(p); p+=4;
eph.f1 =R4(p); p+=4;
eph.f0 =R4(p); p+=4;
eph.deln =R4(p)*SC2RAD; p+=4;
eph.M0 =R8(p); p+=8;
eph.e =R8(p); p+=8;
sqrtA =R8(p); p+=8;
eph.cic =R4(p); p+=4;
eph.crc =R4(p); p+=4;
eph.cis =R4(p); p+=4;
eph.crs =R4(p); p+=4;
eph.cuc =R4(p); p+=4;
eph.cus =R4(p); p+=4;
eph.OMG0 =R8(p); p+=8;
eph.omg =R8(p); p+=8;
eph.i0 =R8(p); p+=8;
eph.OMGd =R4(p)*SC2RAD; p+=4;
eph.idot =R4(p)*SC2RAD; p+=4;
flag1 =U2(p); p+=2;
flag2 =U4(p);
}
else {
trace(2,"binex 0x01-05: length error len=%d\n",len);
return -1;
}
if (!(eph.sat=satno(SYS_CMP,prn))) {
trace(2,"binex 0x01-05: satellite error prn=%d\n",prn);
return 0;
}
eph.A=sqrtA*sqrtA;
eph.toe=gpst2time(eph.week+1356,eph.toes+14.0); /* bdt -> gpst */
eph.toc=gpst2time(eph.week+1356,eph.toes+14.0); /* bdt -> gpst */
eph.ttr=adjweek(eph.toe,tow+14.0); /* bdt -> gpst */
eph.iodc=(flag1>>1)&0x1F;
eph.iode=(flag1>>6)&0x1F;
eph.svh=flag1&0x01;
eph.sva=flag2&0x0F; /* ura index */
eph.tgd[0]=bds_tgd(flag2>> 4); /* TGD1 (s) */
eph.tgd[1]=bds_tgd(flag2>>14); /* TGD2 (s) */
eph.flag=(flag1>>11)&0x07; /* nav type (0:unknown,1:IGSO/MEO,2:GEO) */
eph.code=(flag2>>25)&0x7F;
/* message source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)*/
if (!strstr(raw->opt,"-EPHALL")) {
if (raw->nav.eph[eph.sat-1].iode==eph.iode&&
raw->nav.eph[eph.sat-1].iodc==eph.iodc) return 0; /* unchanged */
}
raw->nav.eph[eph.sat-1]=eph;
raw->ephsat=eph.sat;
return 2;
}
/* decode binex mesaage 0x01-04: decoded galileo ephmemeris ------------------*/
static int decode_bnx_01_04(raw_t *raw, unsigned char *buff, int len)
{
eph_t eph={0};
unsigned char *p=buff;
double tow,ura,sqrtA;
int prn;
trace(4,"binex 0x01-04: len=%d\n",len);
if (len>=127) {
prn =U1(p)+1; p+=1;
eph.week =U2(p); p+=2;
tow =I4(p); p+=4;
eph.toes =I4(p); p+=4;
eph.tgd[0]=R4(p); p+=4; /* BGD E5a/E1 */
eph.tgd[1]=R4(p); p+=4; /* BGD E5b/E1 */
eph.iode =I4(p); p+=4; /* IODnav */
eph.f2 =R4(p); p+=4;
eph.f1 =R4(p); p+=4;
eph.f0 =R4(p); p+=4;
eph.deln =R4(p)*SC2RAD; p+=4;
eph.M0 =R8(p); p+=8;
eph.e =R8(p); p+=8;
sqrtA =R8(p); p+=8;
eph.cic =R4(p); p+=4;
eph.crc =R4(p); p+=4;
eph.cis =R4(p); p+=4;
eph.crs =R4(p); p+=4;
eph.cuc =R4(p); p+=4;
eph.cus =R4(p); p+=4;
eph.OMG0 =R8(p); p+=8;
eph.omg =R8(p); p+=8;
eph.i0 =R8(p); p+=8;
eph.OMGd =R4(p)*SC2RAD; p+=4;
eph.idot =R4(p)*SC2RAD; p+=4;
ura =R4(p)*0.1; p+=4;
eph.svh =U2(p); p+=2;
eph.code =U2(p); /* data source */
}
else {
trace(2,"binex 0x01-04: length error len=%d\n",len);
return -1;
}
if (!(eph.sat=satno(SYS_GAL,prn))) {
trace(2,"binex 0x01-04: satellite error prn=%d\n",prn);
return -1;
}
eph.A=sqrtA*sqrtA;
eph.iode=eph.iodc;
eph.toe=gpst2time(eph.week,eph.toes);
eph.toc=gpst2time(eph.week,eph.toes);
eph.ttr=adjweek(eph.toe,tow);
eph.sva=uraindex(ura);
if (!strstr(raw->opt,"-EPHALL")) {
if (raw->nav.eph[eph.sat-1].iode==eph.iode&&
raw->nav.eph[eph.sat-1].iodc==eph.iodc) return 0; /* unchanged */
}
raw->nav.eph[eph.sat-1]=eph;
raw->ephsat=eph.sat;
return 2;
}
/* decode binex mesaage 0x7f-05: trimble netr8 -------------------------------*/
static int decode_bnx_7f_05(raw_t *raw, unsigned char *buff, int len)
{
obsd_t data={{0}};
double clkoff=0.0,toff[16]={0};
char *msg;
unsigned char *p=buff;
unsigned int flag;
int i,nsat,nobs,prn,sys,sat,clkrst=0,rsys=0,nsys=0,tsys[16]={0};
trace(4,"decode_bnx_7f_05\n");
raw->obs.n=0;
flag=U1(p++);
nsat=(int)(flag&0x3F)+1;
if (flag&0x80) { /* rxclkoff */
clkrst=getbitu(p,0, 2);
clkoff=getbits(p,2,22)*1E-9; p+=3;
}
if (flag&0x40) { /* systime */
nsys=getbitu(p,0,4);
rsys=getbitu(p,4,4); p++;
for (i=0;i<nsys;i++) {
toff[i]=getbits(p,0,24)*1E-9;
tsys[i]=getbitu(p,28,4); p+=4;
}
}
for (i=0;i<nsat;i++) {
prn =U1(p++);
nobs=getbitu(p,1,3);
sys =getbitu(p,4,4); p++;
trace(5,"binex 0x7F-05 PRN=%3d SYS=%d NOBS=%d\n",prn,sys,nobs);
switch (sys) {
case 0: sat=satno(SYS_GPS,prn); break;
case 1: sat=satno(SYS_GLO,prn); break;
case 2: sat=satno(SYS_SBS,prn); break;
case 3: sat=satno(SYS_GAL,prn); break;
case 4: sat=satno(SYS_CMP,prn); break;
case 5: sat=satno(SYS_QZS,prn); break;
default: sat=0; break;
}
/* decode binex mesaage 0x7F-05 obs data */
p=decode_bnx_7f_05_obs(raw,p,sat,nobs,&data);
if ((int)(p-buff)>len) {
trace(2,"binex 0x7F-05 length error: nsat=%2d len=%d\n",nsat,len);
return -1;
}
/* save obs data to obs buffer */
if (data.sat&&raw->obs.n<MAXOBS) {
raw->obs.data[raw->obs.n++]=data;
}
}
if (raw->outtype) {
msg=raw->msgtype+strlen(raw->msgtype);
sprintf(msg," nsat=%2d",nsat);
}
return raw->obs.n>0?1:0;
}
/* decode id#23 measurement block --------------------------------------------*/
static int decode_ss2meas(raw_t *raw)
{
const double freqif=1.405396825E6,tslew=1.75E-7;
double tow,slew,code,icp,d;
int i,j,n,prn,sat,nobs;
unsigned char *p=raw->buff+4;
unsigned int sc;
trace(4,"decode_ss2meas: len=%d\n",raw->len);
nobs=U1(p+2);
if (17+nobs*11!=raw->len) {
trace(2,"ss2 id#23 message length error: len=%d\n",raw->len);
return -1;
}
tow=floor(R8(p+3)*1000.0+0.5)/1000.0; /* rounded by 1ms */
if (!adjweek(raw,tow)) {
trace(2,"ss2 id#23 message time adjustment error\n");
return -1;
}
/* time slew defined as uchar (ref [1]) but minus value appears in some f/w */
slew=*(char *)(p)*tslew;
raw->icpc+=4.5803-freqif*slew-FREQ1*(slew-1E-6); /* phase correction */
for (i=n=0,p+=11;i<nobs&&n<MAXOBS;i++,p+=11) {
prn=(p[0]&0x1F)+1;
if (!(sat=satno(p[0]&0x20?SYS_SBS:SYS_GPS,prn))) {
trace(2,"ss2 id#23 satellite number error: prn=%d\n",prn);
continue;
}
raw->obs.data[n].time=raw->time;
raw->obs.data[n].sat=sat;
code=(tow-floor(tow))-(double)(U4(p+2))/2095104000.0;
raw->obs.data[n].P[0]=CLIGHT*(code+(code<0.0?1.0:0.0));
icp=(double)(U4(p+6)>>2)/1024.0+raw->off[sat-1]; /* unwrap */
if (fabs(icp-raw->icpp[sat-1])>524288.0) {
d=icp>raw->icpp[sat-1]?-1048576.0:1048576.0;
raw->off[sat-1]+=d; icp+=d;
}
raw->icpp[sat-1]=icp;
raw->obs.data[n].L[0]=icp+raw->icpc;
raw->obs.data[n].D[0]=0.0;
raw->obs.data[n].SNR[0]=(unsigned char)(floor(U1(p+1)+0.5));
sc=U1(p+10);
raw->obs.data[n].LLI[0]=(int)((unsigned char)sc-(unsigned char)raw->lockt[sat-1][0])>0;
raw->obs.data[n].LLI[0]|=U1(p+6)&1?2:0;
raw->obs.data[n].code[0]=CODE_L1C;
raw->lockt[sat-1][0]=sc;
for (j=1;j<NFREQ;j++) {
raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
raw->obs.data[n].D[j]=0.0;
raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
raw->obs.data[n].code[j]=CODE_NONE;
}
n++;
}
raw->obs.n=n;
return 1;
}
/* initialize sdr channel struct -----------------------------------------------
* set value to sdr channel struct
* args : int chno I channel number (1,2,...)
* int sys I system type (SYS_***)
* int prn I PRN number
* int ctype I code type (CTYPE_***)
* int dtype I data type (DTYPEI or DTYPEIQ)
* int ftype I front end type (FTYPE1 or FTYPE2)
* double f_cf I center (carrier) frequency (Hz)
* double f_sf I sampling frequency (Hz)
* double f_if I intermidiate frequency (Hz)
* sdrch_t *sdr I/0 sdr channel struct
* return : int 0:okay -1:error
*-----------------------------------------------------------------------------*/
extern int initsdrch(int chno, int sys, int prn, int ctype, int dtype,
int ftype, double f_cf, double f_sf, double f_if,
sdrch_t *sdr)
{
int i;
short *rcode;
sdr->no=chno;
sdr->sys=sys;
sdr->prn=prn;
sdr->sat=satno(sys,prn);
sdr->ctype=ctype;
sdr->dtype=dtype;
sdr->ftype=ftype;
sdr->f_sf=f_sf;
sdr->f_if=f_if;
sdr->ti=1/f_sf;
/* code generation */
if (!(sdr->code=gencode(prn,ctype,&sdr->clen,&sdr->crate))) {
SDRPRINTF("error: gencode\n"); return -1;
}
sdr->ci=sdr->ti*sdr->crate;
sdr->ctime=sdr->clen/sdr->crate;
sdr->nsamp=(int)(f_sf*sdr->ctime);
sdr->nsampchip=(int)(sdr->nsamp/sdr->clen);
satno2id(sdr->sat,sdr->satstr);
/* set carrier frequency */
if (ctype==CTYPE_G1) {
sprintf(sdr->satstr,"R%d",prn); /* frequency number instead of PRN */
sdr->f_cf=FREQ1_GLO+DFRQ1_GLO*prn; /* FDMA */
sdr->foffset=DFRQ1_GLO*prn; /* FDMA */
} else if (sdrini.fend==FEND_FRTLSDR) {
sdr->foffset=f_cf*sdrini.rtlsdrppmerr*1e-6;
} else {
sdr->f_cf=f_cf; /* carrier frequency */
sdr->foffset=0.0; /* frequency offset */
}
/* for BeiDou B1I */
if (ctype==CTYPE_B1I) sdr->nsampchip*=2; /* for BOC code */
/* acqisition struct */
initacqstruct(sys,ctype,prn,&sdr->acq);
sdr->acq.nfft=2*sdr->nsamp;//calcfftnum(2*sdr->nsamp,0);
/* memory allocation */
if (!(sdr->acq.freq=(double*)malloc(sizeof(double)*sdr->acq.nfreq))) {
SDRPRINTF("error: initsdrch memory alocation\n"); return -1;
}
/* doppler search frequency */
for (i=0;i<sdr->acq.nfreq;i++)
sdr->acq.freq[i]=sdr->f_if+((i-(sdr->acq.nfreq-1)/2)*sdr->acq.step)
+sdr->foffset;
/* tracking struct */
if (inittrkstruct(sdr->sat,ctype,sdr->ctime,&sdr->trk)<0) return -1;
/* navigation struct */
if (initnavstruct(sys,ctype,prn,&sdr->nav)<0) {
return -1;
}
/* memory allocation */
if (!(rcode=(short *)sdrmalloc(sizeof(short)*sdr->acq.nfft)) ||
!(sdr->xcode=cpxmalloc(sdr->acq.nfft))) {
SDRPRINTF("error: initsdrch memory alocation\n"); return -1;
}
/* other code generation */
for (i=0;i<sdr->acq.nfft;i++) rcode[i]=0; /* zero padding */
rescode(sdr->code,sdr->clen,0,0,sdr->ci,sdr->nsamp,rcode); /* resampling */
cpxcpx(rcode,NULL,1.0,sdr->acq.nfft,sdr->xcode); /* FFT for acquisition */
cpxfft(NULL,sdr->xcode,sdr->acq.nfft);
sdrfree(rcode);
return 0;
}
