/* 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);
}
static int getbits2(const unsigned char *buff, int p1, int l1, int p2, int l2)
{
if (getbitu(buff,p1,1))
return (int)((getbits(buff,p1,l1)<<l2)+getbitu(buff,p2,l2));
else
return (int)getbitu2(buff,p1,l1,p2,l2);
}
/* decode rtcm ver.2 message -------------------------------------------------*/
extern int decode_rtcm2(rtcm_t *rtcm)
{
double zcnt;
int staid,seqno,stah,ret=0,type=getbitu(rtcm->buff,8,6);
rtklib_trace(3,"decode_rtcm2: type=%2d len=%3d\n",type,rtcm->len);
if ((zcnt=getbitu(rtcm->buff,24,13)*0.6)>=3600.0) {
rtklib_trace(2,"rtcm2 modified z-count error: zcnt=%.1f\n",zcnt);
return -1;
}
adjhour(rtcm,zcnt);
staid=getbitu(rtcm->buff,14,10);
seqno=getbitu(rtcm->buff,37, 3);
stah =getbitu(rtcm->buff,45, 3);
if (seqno-rtcm->seqno!=1&&seqno-rtcm->seqno!=-7) {
rtklib_trace(2,"rtcm2 message outage: seqno=%d->%d\n",rtcm->seqno,seqno);
}
rtcm->seqno=seqno;
rtcm->stah =stah;
if (rtcm->outtype) {
sprintf(rtcm->msgtype,"RTCM %2d (%4d) zcnt=%7.1f staid=%3d seqno=%d",
type,rtcm->len,zcnt,staid,seqno);
}
if (type==3||type==22||type==23||type==24) {
if (rtcm->staid!=0&&staid!=rtcm->staid) {
rtklib_trace(2,"rtcm2 station id changed: %d->%d\n",rtcm->staid,staid);
}
rtcm->staid=staid;
}
if (rtcm->staid!=0&&staid!=rtcm->staid) {
rtklib_trace(2,"rtcm2 station id invalid: %d %d\n",staid,rtcm->staid);
return -1;
}
switch (type) {
case 1: ret=decode_type1 (rtcm); break;
case 3: ret=decode_type3 (rtcm); break;
case 9: ret=decode_type1 (rtcm); break;
case 14: ret=decode_type14(rtcm); break;
case 16: ret=decode_type16(rtcm); break;
case 17: ret=decode_type17(rtcm); break;
case 18: ret=decode_type18(rtcm); break;
case 19: ret=decode_type19(rtcm); break;
case 22: ret=decode_type22(rtcm); break;
case 23: ret=decode_type23(rtcm); break; /* not supported */
case 24: ret=decode_type24(rtcm); break; /* not supported */
case 31: ret=decode_type31(rtcm); break; /* not supported */
case 32: ret=decode_type32(rtcm); break; /* not supported */
case 34: ret=decode_type34(rtcm); break; /* not supported */
case 36: ret=decode_type36(rtcm); break; /* not supported */
case 37: ret=decode_type37(rtcm); break; /* not supported */
case 59: ret=decode_type59(rtcm); break; /* not supported */
}
if (ret>=0) {
if (1<=type&&type<=99) rtcm->nmsg2[type]++; else rtcm->nmsg2[0]++;
}
return ret;
}
static int getbits3(const unsigned char *buff, int p1, int l1, int p2, int l2,
int p3, int l3)
{
if (getbitu(buff,p1,1))
return (int)((getbits(buff,p1,l1)<<(l2+l3))+
(getbitu(buff,p2,l2)<<l3)+getbitu(buff,p3,l3));
else
return (int)getbitu3(buff,p1,l1,p2,l2,p3,l3);
}
extern int32_t getbits3(const uint8_t *buff, int p1, int l1, int p2, int l2,
int p3, int l3)
{
if (getbitu(buff,p1,1))
return (int32_t)((getbits(buff,p1,l1)<<(l2+l3))+
(getbitu(buff,p2,l2)<<l3)+getbitu(buff,p3,l3));
else
return (int32_t)getbitu3(buff,p1,l1,p2,l2,p3,l3);
}
/* 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 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 GLONASS navigation data (string 1) -----------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_g1s1(const uint8_t *buff, sdreph_t *eph)
{
eph->tk[0] =getbitu(buff, 9, 5)-3; /* 3 hour bias in UTC-GPST */
eph->tk[1] =getbitu(buff,14, 6);
eph->tk[2] =getbitu(buff,20, 1)*30;
eph->geph.vel[0]=getbits_glo(buff,21,24)*P2_20*1000; /* km to m */
eph->geph.acc[0]=getbits_glo(buff,45, 5)*P2_30*1000; /* km/s to m/s */
eph->geph.pos[0]=getbits_glo(buff,50,27)*P2_11*1000; /* km/s^2 to m/s^2 */
/* ephemeris counter */
eph->cnt++;
}
/* 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 gps navigation data subframe 4 -------------------------------------*/
static void decode_gps_subfrm4(const unsigned char *buff, alm_t *alm,
double *ion, double *utc, int *leaps)
{
int i,sat,svid=getbitu(buff,50,6);
if (25<=svid&&svid<=32) { /* page 2,3,4,5,7,8,9,10 */
/* decode almanac */
sat=getbitu(buff,50,6);
if (1<=sat&&sat<=32) decode_almanac(buff,sat,alm);
}
else if (svid==63) { /* page 25 */
/* decode as and sv config */
i=56;
for (sat=1;sat<=32;sat++) {
if (alm) alm[sat-1].svconf=getbitu(buff,i,4); i+=4;
}
/* decode sv health */
i=186;
for (sat=25;sat<=32;sat++) {
if (alm) alm[sat-1].svh =getbitu(buff,i,6); i+=6;
}
}
else if (svid==56) { /* page 18 */
/* decode 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);
}
if (leaps) {
i=192;
*leaps=getbits(buff,i,8);
}
}
}
/* 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 half long term correction ------------------------------------------*/
static int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{
trace(4,"decode_longcorrh:\n");
if (getbitu(msg->msg,p,1)==0) { /* vel code=0 */
if (sbssat->iodp==(int)getbitu(msg->msg,p+103,2)) {
return decode_longcorr0(msg,p+ 1,sbssat)&&
decode_longcorr0(msg,p+52,sbssat);
}
}
else if (sbssat->iodp==(int)getbitu(msg->msg,p+104,2)) {
return decode_longcorr1(msg,p+1,sbssat);
}
return 0;
}
/* decode type 7: fast correction degradation factor -------------------------*/
static int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i;
trace(4,"decode_sbstype7\n");
if (sbssat->iodp!=(int)getbitu(msg->msg,18,2)) return 0;
sbssat->tlat=getbitu(msg->msg,14,4);
for (i=0;i<sbssat->nsat&&i<MAXSAT;i++) {
sbssat->sat[i].fcorr.ai=getbitu(msg->msg,22+i*4,4);
}
return 1;
}
/* convert lex binary file to lex message log ----------------------------------
* convert lex binary file to lex message log
* args : int type I output type (0:all)
* int format I lex binary format (0:no-headr,1:with-header)
* char *infile I input file
* char *outfile I output file
* return : status (1:ok,0:no correction)
* notes : see ref [1] 5.7.2.1
*-----------------------------------------------------------------------------*/
extern int lexconvbin(int type, int format, const char *infile,
const char *outfile)
{
FILE *ifp,*ofp;
lexmsg_t msg;
unsigned int preamb;
unsigned char buff[LEXHEADLEN+LEXFRMLEN/8];
int i,j,n=0;
size_t len=(format?LEXHEADLEN:0)+LEXFRMLEN/8;
trace(3,"lexconvbin:type=%d infile=%s outfile=%s\n",type,infile,outfile);
if (!(ifp=fopen(infile,"rb"))) {
trace(1,"lexconvbin infile open error: %s\n",infile);
return 0;
}
if (!(ofp=fopen(outfile,"w"))) {
trace(1,"lexconvbin outfile open error: %s\n",outfile);
fclose(ifp);
return 0;
}
while (fread(buff,1,len,ifp)==len) {
i=format?LEXHEADLEN*8:0;
preamb =getbitu(buff,i,32); i+=32;
msg.prn =getbitu(buff,i, 8); i+= 8;
msg.type =getbitu(buff,i, 8); i+= 8;
msg.alert=getbitu(buff,i, 1); i+= 1;
if (preamb!=LEXFRMPREAMB) {
trace(1,"lex frame preamble error: preamb=%08X\n",preamb);
continue;
}
for (j=0;j<212;j++) {
msg.msg[j]=(unsigned char)getbitu(buff,i,8); i+=8;
}
msg.msg[211]&=0xFE;
fprintf(stderr,"frame=%5d prn=%d type=%d alert=%d\r",++n,msg.prn,
msg.type,msg.alert);
if (type==0||type==msg.type) {
lexoutmsg(ofp,&msg);
}
}
fclose(ifp);
fclose(ofp);
fprintf(stderr,"\n");
return 1;
}
/* check Galileo E1B CRC -------------------------------------------------------
* compute and check CRC of Galileo E1B page data
* args : uint8_t *data1 I E1B page part 1 (15 bytes (120 bits))
* uint8_t *data2 I E1B page part 2 (15 bytes (120 bits))
* return : int 1:okay 0: wrong parity
*-----------------------------------------------------------------------------*/
extern int checkcrc_e1b(uint8_t *data1, uint8_t *data2)
{
uint8_t crcbins[25]={0};
int i,j,crcbits[196],crc,crcmsg;
/* page part 1 */
for (i=0;i<15;i++) {
for (j=0;j<8;j++) {
if (8*i+j==114) break;
crcbits[8*i+j]=-2*(((data1[i]<<j)&0x80)>>7)+1;
}
}
/* page part 2 */
for (i=0;i<11;i++) {
for (j=0;j<8;j++) {
if (8*i+j==82) break;
crcbits[114+8*i+j]=-2*(((data2[i]<<j)&0x80)>>7)+1;
}
}
bits2byte(crcbits,196,25,1,crcbins); /* right alignment for crc */
crc=crc24q(crcbins,25); /* compute crc24 */
crcmsg=getbitu(data2,82,24); /* crc in message */
/* crc matching */
if (crc==crcmsg) return 0;
else return -1;
}
/* decode type 22: extended reference station parameter ----------------------*/
static int decode_type22(rtcm_t *rtcm)
{
double del[2][3]={{0}},hgt=0.0;
int i=48,j,noh;
rtklib_trace(4,"decode_type22: len=%d\n",rtcm->len);
if (i+24<=rtcm->len*8) {
del[0][0]=getbits(rtcm->buff,i,8)/25600.0; i+=8;
del[0][1]=getbits(rtcm->buff,i,8)/25600.0; i+=8;
del[0][2]=getbits(rtcm->buff,i,8)/25600.0; i+=8;
}
else {
rtklib_trace(2,"rtcm2 22 length error: len=%d\n",rtcm->len);
return -1;
}
if (i+24<=rtcm->len*8) {
i+=5; noh=getbits(rtcm->buff,i,1); i+=1;
hgt=noh?0.0:getbitu(rtcm->buff,i,18)/25600.0;
i+=18;
}
if (i+24<=rtcm->len*8) {
del[1][0]=getbits(rtcm->buff,i,8)/1600.0; i+=8;
del[1][1]=getbits(rtcm->buff,i,8)/1600.0; i+=8;
del[1][2]=getbits(rtcm->buff,i,8)/1600.0;
}
rtcm->sta.deltype=1; /* xyz */
for (j=0;j<3;j++) rtcm->sta.del[j]=del[0][j];
rtcm->sta.hgt=hgt;
return 5;
}
/* update sbas corrections -----------------------------------------------------
* update sbas correction parameters in navigation data with a sbas message
* args : sbsmg_t *msg I sbas message
* nav_t *nav IO navigation data
* return : message type (-1: error or not supported type)
* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t)
* seph[prn-MINPRNSBS+1] : sat prn current epehmeris
* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris
*-----------------------------------------------------------------------------*/
extern int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
{
int type=getbitu(msg->msg,8,6),stat=-1;
trace(3,"sbsupdatecorr: type=%d\n",type);
if (msg->week==0) return -1;
switch (type) {
case 0: stat=decode_sbstype2 (msg,&nav->sbssat); break;
case 1: stat=decode_sbstype1 (msg,&nav->sbssat); break;
case 2:
case 3:
case 4:
case 5: stat=decode_sbstype2 (msg,&nav->sbssat); break;
case 6: stat=decode_sbstype6 (msg,&nav->sbssat); break;
case 7: stat=decode_sbstype7 (msg,&nav->sbssat); break;
case 9: stat=decode_sbstype9 (msg,nav); break;
case 18: stat=decode_sbstype18(msg,nav ->sbsion); break;
case 24: stat=decode_sbstype24(msg,&nav->sbssat); break;
case 25: stat=decode_sbstype25(msg,&nav->sbssat); break;
case 26: stat=decode_sbstype26(msg,nav ->sbsion); break;
case 63: break; /* null message */
/*default: trace(2,"unsupported sbas message: type=%d\n",type); break;*/
}
return stat?type:-1;
}
/* decode navigation data (Galileo E1B page) -----------------------------------
*
* args : uint8_t *buff1 I navigation data bits (even page)
* uint8_t *buff2 I navigation data bits (odd page)
* sdreph_t *eph I/O sdr ephemeris structure
* return : int word type
*-----------------------------------------------------------------------------*/
extern int decode_page_e1b(const uint8_t *buff1, const uint8_t *buff2,
sdrnav_t *nav)
{
int id;
/* buff is 240 bits (30 bytes) of composed two page parts */
/* see Galileo SISICD Table 39, pp. 34 */
uint8_t buff[30];
memcpy(buff,buff1,15); memcpy(&buff[15],buff2,15);
id=getbitu(buff,2,6); /* word type */
Ephemeris[nav->sat].PageN((id >= 7)? 999:id);
switch (id) {
case 0: decode_word0(buff,nav); break;
case 1: decode_word1(buff,nav); break;
case 2: decode_word2(buff,nav); break;
case 3: decode_word3(buff,nav); break;
case 4: decode_word4(buff,nav); break;
case 5: decode_word5(buff,nav); break;
case 6: decode_word6(buff,nav); break;
case 7: case 8: case 9: break; // almanac
case 10: decode_word10(buff,nav); break;
case 63: break; // dummy page (we've actually seen these)
default:
printf("%s UNKNOWN W%d\n", PRN(nav->sat), id);
break;
}
return id;
}
/* 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 GLONASS navigation data (string 2) -----------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_g1s2(const uint8_t *buff, sdreph_t *eph)
{
int oldiode=eph->geph.iode;
eph->geph.svh =getbitu(buff, 5, 1);
eph->geph.iode =getbitu(buff, 9, 7)*15;
eph->geph.vel[1]=getbits_glo(buff,21,24)*P2_20*1000; /* km to m */
eph->geph.acc[1]=getbits_glo(buff,45, 5)*P2_30*1000; /* km/s to m/s */
eph->geph.pos[1]=getbits_glo(buff,50,27)*P2_11*1000; /* km/s^2 to m/s^2 */
/* ephemeris update flag */
if (oldiode-eph->geph.iode!=0) eph->update=ON;
/* ephemeris counter */
eph->cnt++;
}
/* decode GLONASS navigation data (string 5) -----------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_g1s5(const uint8_t *buff, sdreph_t *eph)
{
eph->n4 =getbitu(buff,49,5);
/* ephemeris counter */
eph->cnt++;
}
/* decode type 6: integrity info ---------------------------------------------*/
static int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i,iodf[4],udre;
trace(4,"decode_sbstype6:\n");
for (i=0;i<4;i++) {
iodf[i]=getbitu(msg->msg,14+i*2,2);
}
for (i=0;i<sbssat->nsat&&i<MAXSAT;i++) {
if (sbssat->sat[i].fcorr.iodf!=iodf[i/13]) continue;
udre=getbitu(msg->msg,22+i*4,4);
sbssat->sat[i].fcorr.udre=udre+1;
}
trace(5,"decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]);
return 1;
}
/* 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 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;
}
/** Check RTCM frame header and CRC valid.
*
* \param buff A pointer to the RTCM message buffer to check.
* \return If valid then return length of the data message in range 0-1023.
* Returns a negative number if the frame is invalid:
* - `-1` : Preamble not valid
* - `-2` : CRC mismatch
*/
s16 rtcm3_check_frame(u8 *buff)
{
/* Check preamble byte. */
u8 preamble = getbitu(buff, 0, 8);
if (preamble != RTCM3_PREAMBLE)
return -1;
s16 len = getbitu(buff, 14, 10);
/* Calculate CRC of frame header and data message. */
u32 crc_calc = crc24q(buff, len + 3, 0);
/* Check CRC in frame. */
u32 crc_frame = getbitu(buff, (len+3)*8, 24);
if (crc_calc != crc_frame)
return -2;
return len;
}
/* decode GPS/QZS navigation data (subframe 3) ---------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : none
*-----------------------------------------------------------------------------*/
void decode_subfrm3(const uint8_t *buff, sdreph_t *eph)
{
int oldiode=eph->eph.iode;
eph->tow_gpst=getbitu( buff, 30,17)*6.0;
eph->eph.cic =getbits( buff, 60,16)*P2_29;
eph->eph.OMG0=getbits2(buff, 76, 8, 90,24)*P2_31*SC2RAD;
eph->eph.cis =getbits( buff,120,16)*P2_29;
eph->eph.i0 =getbits2(buff,136, 8,150,24)*P2_31*SC2RAD;
eph->eph.crc =getbits( buff,180,16)*P2_5;
eph->eph.omg =getbits2(buff,196, 8,210,24)*P2_31*SC2RAD;
eph->eph.OMGd=getbits( buff,240,24)*P2_43*SC2RAD;
eph->eph.iode=getbitu( buff,270, 8);
eph->eph.idot=getbits( buff,278,14)*P2_43*SC2RAD;
/* ephemeris update flag */
if (oldiode-eph->eph.iode!=0) eph->update=ON;
/* subframe counter */
eph->cnt++;
}
/* decode half long term correction (vel code=0) -----------------------------*/
static int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{
int i,n=getbitu(msg->msg,p,6);
trace(4,"decode_longcorr0:\n");
if (n==0||n>MAXSAT) return 0;
sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8);
for (i=0;i<3;i++) {
sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+9*i,9)*0.125;
sbssat->sat[n-1].lcorr.dvel[i]=0.0;
}
sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+41,10)*P2_31;
sbssat->sat[n-1].lcorr.daf1=0.0;
sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow);
trace(5,"decode_longcorr0:sat=%2d\n",sbssat->sat[n-1].sat);
return 1;
}
/* 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;
}
END_TEST
START_TEST(test_getbitu)
{
u8 test_data[] = {
0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF
};
u32 ret;
ret = getbitu(test_data, 0, 8);
fail_unless(ret == 0x01,
"Test case 1 expected 0x01, got 0x%02X", ret);
ret = getbitu(test_data, 4, 8);
fail_unless(ret == 0x12,
"test case 2 expected 0x12, got 0x%02X", ret);
ret = getbitu(test_data, 28, 16);
fail_unless(ret == 0x789A,
"test case 3 expected 0x789A, got 0x%04X", ret);
ret = getbitu(test_data, 12, 32);
fail_unless(ret == 0x3456789A,
"test case 4 expected 0x3456789A, got 0x%08X", ret);
ret = getbitu(test_data, 10, 3);
fail_unless(ret == 0x4,
"test case 5 expected 0x4, got 0x%01X", ret);
ret = getbitu(test_data, 10, 13);
fail_unless(ret == 0x11A2,
"test case 6 expected 0x11A2, got 0x%04X", ret);
}
/* decode navigation data (GPS/QZS L1CA subframe) ------------------------------
*
* args : uint8_t *buff I navigation data bits
* sdreph_t *eph I/O sdr ephemeris structure
* return : int subframe ID (1-5)
*-----------------------------------------------------------------------------*/
int decode_frame_l1ca(const uint8_t *buff, sdreph_t *eph)
{
int id=getbitu(buff,49,3); /* subframe ID */
switch (id) {
case 1: decode_subfrm1(buff,eph); break;
case 2: decode_subfrm2(buff,eph); break;
case 3: decode_subfrm3(buff,eph); break;
case 4: decode_subfrm4(buff,eph); break;
case 5: decode_subfrm5(buff,eph); break;
}
return id;
}