/* 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 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);
}
}
}
}
/* 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 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 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 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 navigation data subframe 5 -----------------------------------------*/
static int decode_subfrm5(const unsigned char *buff, alm_t *alm)
{
double toas;
int i,sat,week,svid=getbitu(buff,50,6);
trace(4,"decode_subfrm5: svid=%d\n",svid);
trace(5,"decode_subfrm5: buff=");
traceb(5,buff,30);
if (1<=svid&&svid<=24) { /* page 1-24 */
/* decode almanac */
decode_almanac(buff,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);
}
}
}
return 5;
}
/* decode NVS xf5-raw: raw measurement data ----------------------------------*/
static int decode_xf5raw(raw_t *raw)
{
gtime_t time;
double tadj=0.0,toff=0.0,tn;
int dTowInt;
double dTowUTC, dTowGPS, dTowFrac, L1, P1, D1;
double gpsutcTimescale;
unsigned char rcvTimeScaleCorr, sys, carrNo;
int i,j,prn,sat,n=0,nsat,week;
unsigned char *p=raw->buff+2;
char *q,tstr[32],flag;
trace(4,"decode_xf5raw: len=%d\n",raw->len);
/* time tag adjustment option (-TADJ) */
if ((q=strstr(raw->opt,"-tadj"))) {
sscanf(q,"-TADJ=%lf",&tadj);
}
dTowUTC =R8(p);
week = U2(p+8);
gpsutcTimescale = R8(p+10);
/* glonassutcTimescale = R8(p+18); */
rcvTimeScaleCorr = I1(p+26);
/* check gps week range */
if (week>=4096) {
trace(2,"nvs xf5raw obs week error: week=%d\n",week);
return -1;
}
week=adjgpsweek(week);
if ((raw->len - 31)%30) {
/* Message length is not correct: there could be an error in the stream */
trace(2,"nvs xf5raw len=%d seems not be correct\n",raw->len);
return -1;
}
nsat = (raw->len - 31)/30;
dTowGPS = dTowUTC + gpsutcTimescale;
/* Tweak pseudoranges to allow Rinex to represent the NVS time of measure */
dTowInt = 10.0*floor((dTowGPS/10.0)+0.5);
dTowFrac = dTowGPS - (double) dTowInt;
time=gpst2time(week, dTowInt*0.001);
/* time tag adjustment */
if (tadj>0.0) {
tn=time2gpst(time,&week)/tadj;
toff=(tn-floor(tn+0.5))*tadj;
time=timeadd(time,-toff);
}
/* check time tag jump */
if (raw->time.time&&fabs(timediff(time,raw->time))>86400.0) {
time2str(time,tstr,3);
trace(2,"nvs xf5raw time tag jump error: time=%s\n",tstr);
return 0;
}
if (fabs(timediff(time,raw->time))<=1e-3) {
time2str(time,tstr,3);
trace(2,"nvs xf5raw time tag duplicated: time=%s\n",tstr);
return 0;
}
for (i=0,p+=27;(i<nsat) && (n<MAXOBS); i++,p+=30) {
raw->obs.data[n].time = time;
sys = (U1(p)==1)?SYS_GLO:((U1(p)==2)?SYS_GPS:((U1(p)==4)?SYS_SBS:SYS_NONE));
prn = U1(p+1);
if (sys == SYS_SBS) prn += 120; /* Correct this */
if (!(sat=satno(sys,prn))) {
trace(2,"nvs xf5raw satellite number error: sys=%d prn=%d\n",sys,prn);
continue;
}
carrNo = I1(p+2);
L1 = R8(p+ 4);
P1 = R8(p+12);
D1 = R8(p+20);
/* check range error */
if (L1<-1E10||L1>1E10||P1<-1E10||P1>1E10||D1<-1E5||D1>1E5) {
trace(2,"nvs xf5raw obs range error: sat=%2d L1=%12.5e P1=%12.5e D1=%12.5e\n",
sat,L1,P1,D1);
continue;
}
raw->obs.data[n].SNR[0]=(unsigned char)(I1(p+3)*4.0+0.5);
if (sys==SYS_GLO) {
raw->obs.data[n].L[0] = L1 - toff*(FREQ1_GLO+DFRQ1_GLO*carrNo);
} else {
raw->obs.data[n].L[0] = L1 - toff*FREQ1;
}
raw->obs.data[n].P[0] = (P1-dTowFrac)*CLIGHT*0.001 - toff*CLIGHT; /* in ms, needs to be converted */
raw->obs.data[n].D[0] = (float)D1;
/* set LLI if meas flag 4 (carrier phase present) off -> on */
flag=U1(p+28);
raw->obs.data[n].LLI[0]=(flag&0x08)&&!(raw->halfc[sat-1][0]&0x08)?1:0;
raw->halfc[sat-1][0]=flag;
#if 0
if (raw->obs.data[n].SNR[0] > 160) {
time2str(time,tstr,3);
trace(2,"%s, obs.data[%d]: SNR=%.3f LLI=0x%02x\n", tstr,
n, (raw->obs.data[n].SNR[0])/4.0, U1(p+28) );
}
#endif
raw->obs.data[n].code[0] = CODE_L1C;
raw->obs.data[n].sat = sat;
for (j=1;j<NFREQ+NEXOBS;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->time=time;
raw->obs.n=n;
return 1;
}
