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 BeiDou D2 ephemeris --------------------------------------------------
* decode BeiDou D2 ephemeris (GEO satellites) (ref [3] 5.3)
* args : unsigned char *buff I beidou D2 subframe 1 page bits
* buff[ 0- 37]: page 1 (300 bits)
* buff[ 38- 75]: page 2
* ...
* buff[342-379]: page 10
* eph_t *eph IO ephemeris structure
* return : status (1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int decode_bds_d2(const unsigned char *buff, eph_t *eph)
{
double toc_bds,sqrtA;
unsigned int f1p4,cucp5,ep6,cicp7,i0p8,OMGdp9,omgp10;
unsigned int sow1,sow3,sow4,sow5,sow6,sow7,sow8,sow9,sow10;
int i,f1p3,cucp4,ep5,cicp6,i0p7,OMGdp8,omgp9;
int pgn1,pgn3,pgn4,pgn5,pgn6,pgn7,pgn8,pgn9,pgn10;
trace(3,"decode_bds_d2:\n");
i=8*38*0; /* page 1 */
pgn1 =getbitu (buff,i+ 42, 4);
sow1 =getbitu2(buff,i+ 18, 8,i+ 30,12);
eph->svh =getbitu (buff,i+ 46, 1); /* SatH1 */
eph->iodc =getbitu (buff,i+ 47, 5); /* AODC */
eph->sva =getbitu (buff,i+ 60, 4);
eph->week =getbitu (buff,i+ 64,13); /* week in BDT */
toc_bds =getbitu2(buff,i+ 77, 5,i+ 90,12)*8.0;
eph->tgd[0]=getbits (buff,i+102,10)*0.1*1E-9;
eph->tgd[1]=getbits (buff,i+120,10)*0.1*1E-9;
i=8*38*2; /* page 3 */
pgn3 =getbitu (buff,i+ 42, 4);
sow3 =getbitu2(buff,i+ 18, 8,i+ 30,12);
eph->f0 =getbits2(buff,i+100,12,i+120,12)*P2_33;
f1p3 =getbits (buff,i+132,4);
i=8*38*3; /* page 4 */
pgn4 =getbitu (buff,i+ 42, 4);
sow4 =getbitu2(buff,i+ 18, 8,i+ 30,12);
f1p4 =getbitu2(buff,i+ 46, 6,i+ 60,12);
eph->f2 =getbits2(buff,i+ 72,10,i+ 90, 1)*P2_66;
eph->iode =getbitu (buff,i+ 91, 5); /* AODE */
eph->deln =getbits (buff,i+ 96,16)*P2_43*SC2RAD;
cucp4 =getbits (buff,i+120,14);
i=8*38*4; /* page 5 */
pgn5 =getbitu (buff,i+ 42, 4);
sow5 =getbitu2(buff,i+ 18, 8,i+ 30,12);
cucp5 =getbitu (buff,i+ 46, 4);
eph->M0 =getbits3(buff,i+ 50, 2,i+ 60,22,i+ 90, 8)*P2_31*SC2RAD;
eph->cus =getbits2(buff,i+ 98,14,i+120, 4)*P2_31;
ep5 =getbits (buff,i+124,10);
i=8*38*5; /* page 6 */
pgn6 =getbitu (buff,i+ 42, 4);
sow6 =getbitu2(buff,i+ 18, 8,i+ 30,12);
ep6 =getbitu2(buff,i+ 46, 6,i+ 60,16);
sqrtA =getbitu3(buff,i+ 76, 6,i+ 90,22,i+120,4)*P2_19;
cicp6 =getbits (buff,i+124,10);
eph->A =sqrtA*sqrtA;
i=8*38*6; /* page 7 */
pgn7 =getbitu (buff,i+ 42, 4);
sow7 =getbitu2(buff,i+ 18, 8,i+ 30,12);
cicp7 =getbitu2(buff,i+ 46, 6,i+ 60, 2);
eph->cis =getbits (buff,i+ 62,18)*P2_31;
eph->toes =getbitu2(buff,i+ 80, 2,i+ 90,15)*8.0;
i0p7 =getbits2(buff,i+105, 7,i+120,14);
i=8*38*7; /* page 8 */
pgn8 =getbitu (buff,i+ 42, 4);
sow8 =getbitu2(buff,i+ 18, 8,i+ 30,12);
i0p8 =getbitu2(buff,i+ 46, 6,i+ 60, 5);
eph->crc =getbits2(buff,i+ 65,17,i+ 90, 1)*P2_6;
eph->crs =getbits (buff,i+ 91,18)*P2_6;
OMGdp8 =getbits2(buff,i+109, 3,i+120,16);
i=8*38*8; /* page 9 */
pgn9 =getbitu (buff,i+ 42, 4);
sow9 =getbitu2(buff,i+ 18, 8,i+ 30,12);
OMGdp9 =getbitu (buff,i+ 46, 5);
eph->OMG0 =getbits3(buff,i+ 51, 1,i+ 60,22,i+ 90, 9)*P2_31*SC2RAD;
omgp9 =getbits2(buff,i+ 99,13,i+120,14);
i=8*38*9; /* page 10 */
pgn10 =getbitu (buff,i+ 42, 4);
sow10 =getbitu2(buff,i+ 18, 8,i+ 30,12);
omgp10 =getbitu (buff,i+ 46, 5);
eph->idot =getbits2(buff,i+ 51, 1,i+ 60,13)*P2_43*SC2RAD;
/* check consistency of page numbers, sows and toe/toc */
if (pgn1!=1||pgn3!=3||pgn4!=4||pgn5!=5||pgn6!=6||pgn7!=7||pgn8!=8||pgn9!=9||
pgn10!=10) {
trace(3,"decode_bds_d2 error: pgn=%d %d %d %d %d %d %d %d %d\n",
pgn1,pgn3,pgn4,pgn5,pgn6,pgn7,pgn8,pgn9,pgn10);
return 0;
}
if (sow3!=sow1+6||sow4!=sow3+3||sow5!=sow4+3||sow6!=sow5+3||
sow7!=sow6+3||sow8!=sow7+3||sow9!=sow8+3||sow10!=sow9+3) {
trace(3,"decode_bds_d2 error: sow=%d %d %d %d %d %d %d %d %d\n",
sow1,sow3,sow4,sow5,sow6,sow7,sow8,sow9,sow10);
return 0;
}
if (toc_bds!=eph->toes) {
trace(3,"decode_bds_d2 error: toe=%.0f toc=%.0f\n",eph->toes,toc_bds);
return 0;
}
eph->f1 =merge_two_s(f1p3 ,f1p4 ,18)*P2_50;
eph->cuc =merge_two_s(cucp4 ,cucp5 , 4)*P2_31;
eph->e =merge_two_s(ep5 ,ep6 ,22)*P2_33;
eph->cic =merge_two_s(cicp6 ,cicp7 , 8)*P2_31;
eph->i0 =merge_two_s(i0p7 ,i0p8 ,11)*P2_31*SC2RAD;
eph->OMGd=merge_two_s(OMGdp8,OMGdp9, 5)*P2_43*SC2RAD;
eph->omg =merge_two_s(omgp9 ,omgp10, 5)*P2_31*SC2RAD;
eph->ttr=bdt2gpst(bdt2time(eph->week,sow1)); /* bdt -> gpst */
if (eph->toes>sow1+302400.0) eph->week++;
else if (eph->toes<sow1-302400.0) eph->week--;
eph->toe=bdt2gpst(bdt2time(eph->week,eph->toes)); /* bdt -> gpst */
eph->toc=bdt2gpst(bdt2time(eph->week,toc_bds)); /* bdt -> gpst */
return 1;
}
