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; }