/* 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 navigation data subframe 2 -----------------------------------------*/
static int decode_subfrm2(const unsigned char *buff, eph_t *eph)
{
double sqrtA;
int i=48;
trace(4,"decode_subfrm2:\n");
trace(5,"decode_subfrm2: buff=");
traceb(5,buff,30);
eph->iode=getbitu(buff,i, 8);
i+= 8;
eph->crs =getbits(buff,i,16)*P2_5;
i+=16;
eph->deln=getbits(buff,i,16)*P2_43*SC2RAD;
i+=16;
eph->M0 =getbits(buff,i,32)*P2_31*SC2RAD;
i+=32;
eph->cuc =getbits(buff,i,16)*P2_29;
i+=16;
eph->e =getbitu(buff,i,32)*P2_33;
i+=32;
eph->cus =getbits(buff,i,16)*P2_29;
i+=16;
sqrtA =getbitu(buff,i,32)*P2_19;
i+=32;
eph->toes=getbitu(buff,i,16)*16.0;
i+=16;
eph->fit =getbitu(buff,i, 1);
eph->A=sqrtA*sqrtA;
return 2;
}
/* 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 5 --------------------------------*/
static int decode_subfrm5(const unsigned char *buff, alm_t *alm, double *ion,
double *utc, int *leaps)
{
int dataid=getbitu(buff,48,2);
trace(4,"decode_subfrm5: dataid=%d\n",dataid);
trace(5,"decode_subfrm5: buff="); traceb(5,buff,30);
if (dataid==1) { /* gps */
decode_gps_subfrm5(buff,alm);
}
else if (dataid==3) { /* qzss */
decode_qzs_subfrm45(buff,alm,ion,utc,leaps);
}
return 5;
}
/* 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 navigation data subframe 4 -----------------------------------------*/
static int decode_subfrm4(const unsigned char *buff, alm_t *alm, double *ion,
double *utc, int *leaps)
{
int i,sat,svid=getbitu(buff,50,6);
trace(4,"decode_subfrm4: svid=%d\n",svid);
trace(5,"decode_subfrm4: buff=");
traceb(5,buff,30);
if (25<=svid&&svid<=32) { /* page 2,3,4,5,7,8,9,10 */
/* decode almanac */
decode_almanac(buff,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);
}
}
return 4;
}
/* generate ublox binary message -----------------------------------------------
* generate ublox binary message from message string
* args : char *msg IO message string
* "CFG-PRT portid res0 res1 mode baudrate inmask outmask flags"
* "CFG-USB vendid prodid res1 res2 power flags vstr pstr serino"
* "CFG-MSG msgid rate0 rate1 rate2 rate3"
* "CFG-NMEA filter version numsv flags"
* "CFG-RATE meas nav time"
* "CFG-CFG clear_mask save_mask load_mask"
* "CFG-TP interval length status time_ref res adelay rdelay udelay"
* "CFG-NAV2 ..."
* "CFG-DAT maja flat dx dy dz rotx roty rotz scale"
* "CFG-INF protocolid res0 res1 mask0 mask1 mask2 mask3"
* "CFG-RST navbbr reset res"
* "CFG-RXM gpsmode lpmode"
* "CFG-ANT flags pins"
* "CFG-FXN flags treacq tacq treacqoff tacqoff ton toff res basetow"
* "CFG-SBAS mode usage maxsbas res scanmode"
* "CFG-LIC key0 key1 key2 key3 key4 key5"
* "CFG-TM intid rate flags"
* "CFG-TM2 ch res0 res1 rate flags"
* "CFG-TMODE tmode posx posy posz posvar svinmindur svinvarlimit"
* "CFG-EKF ..."
* unsigned char *buff O binary message
* return : length of binary message (0: error)
* note : see reference [1] for details.
*-----------------------------------------------------------------------------*/
extern int gen_ubx(const char *msg, unsigned char *buff)
{
const char *cmd[]={
"PRT","USB","MSG","NMEA","RATE","CFG","TP","NAV2","DAT","INF",
"RST","RXM","ANT","FXN","SBAS","LIC","TM","TM2","TMODE","EKF",""
};
const unsigned char id[]={
0x00,0x1B,0x01,0x17,0x08,0x09,0x07,0x1A,0x06,0x02,
0x04,0x11,0x13,0x0E,0x16,0x80,0x10,0x19,0x1D,0x12
};
const int prm[][32]={
{FU1,FU1,FU2,FU4,FU4,FU2,FU2,FU2,FU2}, /* PRT */
{FU2,FU2,FU2,FU2,FU2,FU2,FS32,FS32,FS32}, /* USB */
{FU1,FU1,FU1,FU1,FU1,FU1}, /* MSG */
{FU1,FU1,FU1,FU1}, /* NMEA */
{FU2,FU2,FU2}, /* RATE */
{FU4,FU4,FU4}, /* CFG */
{FU4,FU4,FI1,FU1,FU2,FI2,FI2,FI4}, /* TP */
{FU1,FU1,FU2,FU1,FU1,FU1,FU1,FI4,FU1,FU1,FU1,FU1,FU1,FU1,FU2,FU2,FU2,FU2,
FU2,FU1,FU1,FU2,FU4,FU4}, /* NAV2 */
{FR8,FR8,FR4,FR4,FR4,FR4,FR4,FR4,FR4}, /* DAT */
{FU1,FU1,FU2,FU1,FU1,FU1,FU1}, /* INF */
{FU2,FU1,FU1}, /* RST */
{FU1,FU1}, /* RXM */
{FU2,FU2}, /* ANT */
{FU4,FU4,FU4,FU4,FU4,FU4,FU4,FU4}, /* FXN */
{FU1,FU1,FU1,FU1,FU4}, /* SBAS */
{FU2,FU2,FU2,FU2,FU2,FU2}, /* LIC */
{FU4,FU4,FU4}, /* TM */
{FU1,FU1,FU2,FU4,FU4}, /* TM2 */
{FU4,FI4,FI4,FI4,FU4,FU4,FU4}, /* TMODE */
{FU1,FU1,FU1,FU1,FU4,FU2,FU2,FU1,FU1,FU2} /* EKF */
};
unsigned char *q=buff;
char mbuff[1024],*args[32],*p;
int i,j,n,narg=0;
trace(4,"gen_ubxf: msg=%s\n",msg);
strcpy(mbuff,msg);
for (p=strtok(mbuff," ");p&&narg<32;p=strtok(NULL," ")) {
args[narg++]=p;
}
if (narg<1||strncmp(args[0],"CFG-",4)) return 0;
for (i=0;*cmd[i];i++) {
if (!strcmp(args[0]+4,cmd[i])) break;
}
if (!*cmd[i]) return 0;
*q++=UBXSYNC1;
*q++=UBXSYNC2;
*q++=UBXCFG;
*q++=id[i];
q+=2;
for (j=1;prm[i][j-1]>0;j++) {
switch (prm[i][j-1]) {
case FU1 : U1(q)=j<narg?(unsigned char )atoi(args[j]):0; q+=1; break;
case FU2 : U2(q)=j<narg?(unsigned short)atoi(args[j]):0; q+=2; break;
case FU4 : U4(q)=j<narg?(unsigned int )atoi(args[j]):0; q+=4; break;
case FI1 : I1(q)=j<narg?(char )atoi(args[j]):0; q+=1; break;
case FI2 : I2(q)=j<narg?(short )atoi(args[j]):0; q+=2; break;
case FI4 : I4(q)=j<narg?(int )atoi(args[j]):0; q+=4; break;
case FR4 : R4(q)=j<narg?(float )atof(args[j]):0; q+=4; break;
case FR8 : setR8(q,j<narg?(double)atof(args[j]):0); q+=8; break;
case FS32: sprintf((char *)q,"%-32.32s",j<narg?args[j]:""); q+=32; break;
}
}
n=(int)(q-buff)+2;
U2(buff+4)=(unsigned short)(n-8);
setcs(buff,n);
trace(5,"gen_ubxf: buff=\n"); traceb(5,buff,n);
return n;
}
