Skip to content
Snippets Groups Projects
Commit 465a7650 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

bug fixed

parent 4b8f5053
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 65 additions and 12 deletions
src/rcv/ublox.c
+ 65
− 12
View file @ 465a7650
...@@ -108,11 +108,11 @@ typedef enum { false, true } bool; ...@@ -108,11 +108,11 @@ typedef enum { false, true } bool;
/* get fields (little-endian) ------------------------------------------------*/ /* get fields (little-endian) ------------------------------------------------*/
#define U1(p) (*((unsigned char *)(p))) #define U1(p) (*((unsigned char *)(p)))
#define I1(p) (*((signed char *)(p))) #define I1(p) (*((signed char *)(p)))
static unsigned short U2(unsigned char *p) {unsigned short u; memcpy(&u,p,2); return u;} static unsigned short U2(unsigned char *p) {unsigned short u=0; memcpy(&u,p,2); return u;}
static unsigned long U4(unsigned char *p) {unsigned long u; memcpy(&u,p,4); return u;} static unsigned long U4(unsigned char *p) {unsigned long u=0; memcpy(&u,p,4); return u;}
static int I4(unsigned char *p) {signed long u; memcpy(&u,p,4); return u;} static int I4(unsigned char *p) {signed long u=0; memcpy(&u,p,4); return u;}
static float R4(unsigned char *p) {float r; memcpy(&r,p,4); return r;} static float R4(unsigned char *p) {float r=0; memcpy(&r,p,4); return r;}
static double R8(unsigned char *p) {double r; memcpy(&r,p,8); return r;} static double R8(unsigned char *p) {double r=0; memcpy(&r,p,8); return r;}
static double I8(unsigned char *p) {return I4(p+4)*4294967296.0+U4(p);} static double I8(unsigned char *p) {return I4(p+4)*4294967296.0+U4(p);}
...@@ -262,6 +262,7 @@ static int decode_rxmraw(raw_t *raw) ...@@ -262,6 +262,7 @@ static int decode_rxmraw(raw_t *raw)
char *q; char *q;
trace(4,"decode_rxmraw: len=%d\n",raw->len); trace(4,"decode_rxmraw: len=%d\n",raw->len);
//printf("decode_rxmraw: len=%d\n",raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype,"UBX RXM-RAW (%4d): nsat=%d",raw->len,U1(p+6)); sprintf(raw->msgtype,"UBX RXM-RAW (%4d): nsat=%d",raw->len,U1(p+6));
...@@ -339,6 +340,7 @@ static int decode_rxmrawx(raw_t *raw) ...@@ -339,6 +340,7 @@ static int decode_rxmrawx(raw_t *raw)
int week,nmeas,ver,gnss,svid,sigid,frqid,lockt,cn0,cpstd,prstd,tstat; int week,nmeas,ver,gnss,svid,sigid,frqid,lockt,cn0,cpstd,prstd,tstat;
trace(4,"decode_rxmrawx: len=%d\n",raw->len); trace(4,"decode_rxmrawx: len=%d\n",raw->len);
//printf("decode_rxmrawx: len=%d\n",raw->len);
if (raw->len<24) { if (raw->len<24) {
trace(2,"ubx rxmrawx length error: len=%d\n",raw->len); trace(2,"ubx rxmrawx length error: len=%d\n",raw->len);
...@@ -559,8 +561,9 @@ static int decode_rxmsfrb(raw_t *raw) ...@@ -559,8 +561,9 @@ static int decode_rxmsfrb(raw_t *raw)
unsigned int words[10]; unsigned int words[10];
int i,prn,sat,sys,id; int i,prn,sat,sys,id;
unsigned char *p=raw->buff+6; unsigned char *p=raw->buff+6;
trace(4,"decode_rxmsfrb: len=%d\n",raw->len); trace(4,"decode_rxmsfrb: len=%d\n",raw->len);
//printf("decode_rxmsfrb: len=%d\n",raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype,"UBX RXM-SFRB (%4d): prn=%2d",raw->len,U1(p+1)); sprintf(raw->msgtype,"UBX RXM-SFRB (%4d): prn=%2d",raw->len,U1(p+1));
...@@ -594,8 +597,9 @@ static int decode_navsol(raw_t *raw) ...@@ -594,8 +597,9 @@ static int decode_navsol(raw_t *raw)
{ {
int itow,ftow,week; int itow,ftow,week;
unsigned char *p=raw->buff+6; unsigned char *p=raw->buff+6;
trace(4,"decode_navsol: len=%d\n",raw->len); trace(4,"decode_navsol: len=%d\n",raw->len);
//printf("decode_navsol: len=%d\n",raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype,"UBX NAV-SOL (%4d):",raw->len); sprintf(raw->msgtype,"UBX NAV-SOL (%4d):",raw->len);
...@@ -613,8 +617,9 @@ static int decode_navtime(raw_t *raw) ...@@ -613,8 +617,9 @@ static int decode_navtime(raw_t *raw)
{ {
int itow,ftow,week; int itow,ftow,week;
unsigned char *p=raw->buff+6; unsigned char *p=raw->buff+6;
trace(4,"decode_navtime: len=%d\n",raw->len); trace(4,"decode_navtime: len=%d\n",raw->len);
//printf("decode_navtime: len=%d\n",raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype,"UBX NAV-TIME (%4d):",raw->len); sprintf(raw->msgtype,"UBX NAV-TIME (%4d):",raw->len);
...@@ -640,8 +645,9 @@ static int decode_trkmeas(raw_t *raw) ...@@ -640,8 +645,9 @@ static int decode_trkmeas(raw_t *raw)
values based on difference between TRK_MEAS values and RXM-RAWX values */ values based on difference between TRK_MEAS values and RXM-RAWX values */
const char P_adj_fw2[]={ 0, 0, 0, 0, 1, 3, 2, 0,-4,-3,-9,-8,-7,-4, 0}; /* fw 2.30 */ const char P_adj_fw2[]={ 0, 0, 0, 0, 1, 3, 2, 0,-4,-3,-9,-8,-7,-4, 0}; /* fw 2.30 */
const char P_adj_fw3[]={11,13,13,14,14,13,12,10, 8, 6, 5, 5, 5, 7, 0}; /* fw 3.01 */ const char P_adj_fw3[]={11,13,13,14,14,13,12,10, 8, 6, 5, 5, 5, 7, 0}; /* fw 3.01 */
trace(4,"decode_trkmeas: len=%d\n",raw->len); trace(4,"decode_trkmeas: len=%d\n",raw->len);
//printf("decode_trkmeas: len=%d\n",raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-MEAS (%4d):",raw->len); sprintf(raw->msgtype,"UBX TRK-MEAS (%4d):",raw->len);
...@@ -772,8 +778,9 @@ static int decode_trkd5(raw_t *raw) ...@@ -772,8 +778,9 @@ static int decode_trkd5(raw_t *raw)
double ts,tr=-1.0,t,tau,adr,dop,snr,utc_gpst; double ts,tr=-1.0,t,tau,adr,dop,snr,utc_gpst;
int i,j,n=0,type,off,len,sys,prn,sat,qi,frq,flag,week; int i,j,n=0,type,off,len,sys,prn,sat,qi,frq,flag,week;
unsigned char *p=raw->buff+6; unsigned char *p=raw->buff+6;
trace(4,"decode_trkd5: len=%d\n",raw->len); trace(4,"decode_trkd5: len=%d\n",raw->len);
//printf("decode_trkd5: len=%d\n",raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-D5 (%4d):",raw->len); sprintf(raw->msgtype,"UBX TRK-D5 (%4d):",raw->len);
...@@ -883,22 +890,45 @@ static int decode_nav(raw_t *raw, int sat, int off) ...@@ -883,22 +890,45 @@ static int decode_nav(raw_t *raw, int sat, int off)
unsigned int words[10]; unsigned int words[10];
int i,id; int i,id;
unsigned char *p=raw->buff+6+off; unsigned char *p=raw->buff+6+off;
if (raw->len<48+off) { if (raw->len<48+off) {
trace(2,"ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len); trace(2,"ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len);
//printf("ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len);
return -1; return -1;
} }
//printf("decode_nav: p = %hhx\n", *p);
//printf("decode_nav: p+1 = %hhx\n", *(p+1));
//printf("decode_nav: p+2 = %hhx\n", *(p+2));
//printf("decode_nav: p+3 = %hhx\n", *(p+3));
//printf("decode_nav: U4(p) = %lu\n",U4(p));
//printf("decode_nav: U4(p)>>24 = %lu\n",U4(p)>>24);
// printf("decode_nav: U4(p+4) = %lu\n",U4(p+4));
// printf("decode_nav: U4(p+4)>>24 = %lu\n",U4(p+4)>>24);
// printf("decode_nav: U4(p+8) = %lu\n",U4(p+8));
// printf("decode_nav: U4(p+8)>>24 = %lu\n",U4(p+8)>>24);
// printf("decode_nav: U4(p+12) = %lu\n",U4(p+12));
// printf("decode_nav: U4(p+12)>>24 = %lu\n",U4(p+12)>>24);
// printf("decode_nav: U4(p+16) = %lu\n",U4(p+16));
// printf("decode_nav: U4(p+16)>>24 = %lu\n",U4(p+16)>>24);
// printf("decode_nav: U4(p+20) = %lu\n",U4(p+20));
// printf("decode_nav: U4(p+20)>>24 = %lu\n",U4(p+20)>>24);
// printf("decode_nav: U4(p+24) = %lu\n",U4(p+24));
// printf("decode_nav: U4(p+24)>>24 = %lu\n",U4(p+24)>>24);
if ((U4(p)>>24)==PREAMB_CNAV) { if ((U4(p)>>24)==PREAMB_CNAV) {
trace(3,"ubx rawsfrbx cnav not supported sat=%d prn=%d\n",sat, trace(3,"ubx rawsfrbx cnav not supported sat=%d prn=%d\n",sat,
(U4(p)>>18)&0x3F); (U4(p)>>18)&0x3F);
//printf("ubx rawsfrbx cnav not supported sat=%d prn=%d\n",sat,(U4(p)>>18)&0x3F);
return 0; return 0;
} }
for (i=0;i<10;i++,p+=4) words[i]=U4(p)>>6; /* 24 bits without parity */ for (i=0;i<10;i++,p+=4) words[i]=U4(p)>>6; /* 24 bits without parity */
id=(words[1]>>2)&7; id=(words[1]>>2)&7;
//printf("decode_nav: sat=%2d id=%d len=%d\n",sat,id,raw->len);
if (id<1||5<id) { if (id<1||5<id) {
trace(2,"ubx rawsfrbx subfrm id error: sat=%2d id=%d len=%d\n",sat,id, trace(2,"ubx rawsfrbx subfrm id error: sat=%2d id=%d len=%d\n",sat,id,
raw->len); raw->len);
//printf("ubx rawsfrbx subfrm id error: sat=%2d id=%d len=%d\n",sat,id,raw->len);
return -1; return -1;
} }
for (i=0;i<10;i++) { for (i=0;i<10;i++) {
...@@ -912,12 +942,14 @@ static int decode_nav(raw_t *raw, int sat, int off) ...@@ -912,12 +942,14 @@ static int decode_nav(raw_t *raw, int sat, int off)
/* decode galileo navigation data --------------------------------------------*/ /* decode galileo navigation data --------------------------------------------*/
static int decode_enav(raw_t *raw, int sat, int off) static int decode_enav(raw_t *raw, int sat, int off)
{ {
//printf("decode_enav\n");
eph_t eph={0}; eph_t eph={0};
unsigned char *p=raw->buff+6+off,buff[32],crc_buff[26]={0}; unsigned char *p=raw->buff+6+off,buff[32],crc_buff[26]={0};
int i,j,k,part1,page1,part2,page2,type; int i,j,k,part1,page1,part2,page2,type;
if (raw->len<44+off) { if (raw->len<44+off) {
trace(2,"ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len); trace(2,"ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len);
//printf("ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len);
return -1; return -1;
} }
for (i=k=0;i<8;i++,p+=4) for (j=0;j<4;j++) { for (i=k=0;i<8;i++,p+=4) for (j=0;j<4;j++) {
...@@ -934,6 +966,7 @@ static int decode_enav(raw_t *raw, int sat, int off) ...@@ -934,6 +966,7 @@ static int decode_enav(raw_t *raw, int sat, int off)
/* test even-odd parts */ /* test even-odd parts */
if (part1!=0||part2!=1) { if (part1!=0||part2!=1) {
trace(2,"ubx rawsfrbx gal page even/odd error: sat=%2d\n",sat); trace(2,"ubx rawsfrbx gal page even/odd error: sat=%2d\n",sat);
//printf("ubx rawsfrbx gal page even/odd error: sat=%2d\n",sat);
return -1; return -1;
} }
/* test crc (4(pad) + 114 + 82 bits) */ /* test crc (4(pad) + 114 + 82 bits) */
...@@ -941,6 +974,7 @@ static int decode_enav(raw_t *raw, int sat, int off) ...@@ -941,6 +974,7 @@ static int decode_enav(raw_t *raw, int sat, int off)
for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(buff+16,i*8,8)); for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(buff+16,i*8,8));
if (rtk_crc24q(crc_buff,25)!=getbitu(buff+16,82,24)) { if (rtk_crc24q(crc_buff,25)!=getbitu(buff+16,82,24)) {
trace(2,"ubx rawsfrbx gal page crc error: sat=%2d\n",sat); trace(2,"ubx rawsfrbx gal page crc error: sat=%2d\n",sat);
//printf("ubx rawsfrbx gal page crc error: sat=%2d\n",sat);
return -1; return -1;
} }
type=getbitu(buff,2,6); /* word type */ type=getbitu(buff,2,6); /* word type */
...@@ -970,6 +1004,7 @@ static int decode_enav(raw_t *raw, int sat, int off) ...@@ -970,6 +1004,7 @@ static int decode_enav(raw_t *raw, int sat, int off)
/* test svid consistency */ /* test svid consistency */
if (eph.sat!=sat) { if (eph.sat!=sat) {
trace(2,"ubx rawsfrbx gal svid error: sat=%2d %2d\n",sat,eph.sat); trace(2,"ubx rawsfrbx gal svid error: sat=%2d %2d\n",sat,eph.sat);
//printf("ubx rawsfrbx gal svid error: sat=%2d %2d\n",sat,eph.sat);
return -1; return -1;
} }
if (!strstr(raw->opt,"-EPHALL")) { if (!strstr(raw->opt,"-EPHALL")) {
...@@ -985,6 +1020,7 @@ static int decode_enav(raw_t *raw, int sat, int off) ...@@ -985,6 +1020,7 @@ static int decode_enav(raw_t *raw, int sat, int off)
/* decode beidou navigation data ---------------------------------------------*/ /* decode beidou navigation data ---------------------------------------------*/
static int decode_cnav(raw_t *raw, int sat, int off) static int decode_cnav(raw_t *raw, int sat, int off)
{ {
//printf("decode_cnav\n");
eph_t eph={0}; eph_t eph={0};
unsigned int words[10]; unsigned int words[10];
int i,id,pgn,prn; int i,id,pgn,prn;
...@@ -992,6 +1028,7 @@ static int decode_cnav(raw_t *raw, int sat, int off) ...@@ -992,6 +1028,7 @@ static int decode_cnav(raw_t *raw, int sat, int off)
if (raw->len<48+off) { if (raw->len<48+off) {
trace(2,"ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len); trace(2,"ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len);
//printf("ubx rawsfrbx length error: sat=%d len=%d\n",sat,raw->len);
return -1; return -1;
} }
for (i=0;i<10;i++,p+=4) words[i]=U4(p)&0x3FFFFFFF; /* 30 bits */ for (i=0;i<10;i++,p+=4) words[i]=U4(p)&0x3FFFFFFF; /* 30 bits */
...@@ -1000,6 +1037,7 @@ static int decode_cnav(raw_t *raw, int sat, int off) ...@@ -1000,6 +1037,7 @@ static int decode_cnav(raw_t *raw, int sat, int off)
id=(words[0]>>12)&0x07; /* subframe id (3bit) */ id=(words[0]>>12)&0x07; /* subframe id (3bit) */
if (id<1||5<id) { if (id<1||5<id) {
trace(2,"ubx rawsfrbx subfrm id error: sat=%2d\n",sat); trace(2,"ubx rawsfrbx subfrm id error: sat=%2d\n",sat);
//printf("ubx rawsfrbx subfrm id error: sat=%2d\n",sat);
return -1; return -1;
} }
if (prn>5&&prn<59) { /* IGSO/MEO */ if (prn>5&&prn<59) { /* IGSO/MEO */
...@@ -1019,6 +1057,7 @@ static int decode_cnav(raw_t *raw, int sat, int off) ...@@ -1019,6 +1057,7 @@ static int decode_cnav(raw_t *raw, int sat, int off)
pgn=(words[1]>>14)&0x0F; /* page number (4bit) */ pgn=(words[1]>>14)&0x0F; /* page number (4bit) */
if (pgn<1||10<pgn) { if (pgn<1||10<pgn) {
trace(2,"ubx rawsfrbx page number error: sat=%2d\n",sat); trace(2,"ubx rawsfrbx page number error: sat=%2d\n",sat);
//printf("ubx rawsfrbx page number error: sat=%2d\n",sat);
return -1; return -1;
} }
for (i=0;i<10;i++) { for (i=0;i<10;i++) {
...@@ -1042,6 +1081,7 @@ static int decode_cnav(raw_t *raw, int sat, int off) ...@@ -1042,6 +1081,7 @@ static int decode_cnav(raw_t *raw, int sat, int off)
/* decode glonass navigation data --------------------------------------------*/ /* decode glonass navigation data --------------------------------------------*/
static int decode_gnav(raw_t *raw, int sat, int off, int frq) static int decode_gnav(raw_t *raw, int sat, int off, int frq)
{ {
//printf("decode_gnav\n");
geph_t geph={0}; geph_t geph={0};
int i,j,k,m,prn; int i,j,k,m,prn;
unsigned char *p=raw->buff+6+off,buff[64],*fid; unsigned char *p=raw->buff+6+off,buff[64],*fid;
...@@ -1050,6 +1090,7 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq) ...@@ -1050,6 +1090,7 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq)
if (raw->len<24+off) { if (raw->len<24+off) {
trace(2,"ubx rawsfrbx gnav length error: len=%d\n",raw->len); trace(2,"ubx rawsfrbx gnav length error: len=%d\n",raw->len);
//printf("ubx rawsfrbx gnav length error: len=%d\n",raw->len);
return -1; return -1;
} }
for (i=k=0;i<4;i++,p+=4) for (j=0;j<4;j++) { for (i=k=0;i<4;i++,p+=4) for (j=0;j<4;j++) {
...@@ -1058,11 +1099,13 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq) ...@@ -1058,11 +1099,13 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq)
/* test hamming of glonass string */ /* test hamming of glonass string */
if (!test_glostr(buff)) { if (!test_glostr(buff)) {
trace(2,"ubx rawsfrbx glo string hamming error: sat=%2d\n",sat); trace(2,"ubx rawsfrbx glo string hamming error: sat=%2d\n",sat);
//printf("ubx rawsfrbx glo string hamming error: sat=%2d\n",sat);
return -1; return -1;
} }
m=getbitu(buff,1,4); m=getbitu(buff,1,4);
if (m<1||15<m) { if (m<1||15<m) {
trace(2,"ubx rawsfrbx glo string no error: sat=%2d\n",sat); trace(2,"ubx rawsfrbx glo string no error: sat=%2d\n",sat);
//printf("ubx rawsfrbx glo string no error: sat=%2d\n",sat);
return -1; return -1;
} }
/* flush frame buffer if frame-id changed */ /* flush frame buffer if frame-id changed */
...@@ -1090,11 +1133,13 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq) ...@@ -1090,11 +1133,13 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq)
/* decode sbas navigation data -----------------------------------------------*/ /* decode sbas navigation data -----------------------------------------------*/
static int decode_snav(raw_t *raw, int sat, int off) static int decode_snav(raw_t *raw, int sat, int off)
{ {
//printf("decode_snav\n");
int i,j,k,prn,tow,week; int i,j,k,prn,tow,week;
unsigned char *p=raw->buff+6+off,buff[64]; unsigned char *p=raw->buff+6+off,buff[64];
if (raw->len<40+off) { if (raw->len<40+off) {
trace(2,"ubx rawsfrbx snav length error: len=%d\n",raw->len); trace(2,"ubx rawsfrbx snav length error: len=%d\n",raw->len);
//printf("ubx rawsfrbx snav length error: len=%d\n",raw->len);
return -1; return -1;
} }
tow=(int)time2gpst(timeadd(raw->time,-1.0),&week); tow=(int)time2gpst(timeadd(raw->time,-1.0),&week);
...@@ -1114,8 +1159,9 @@ static int decode_rxmsfrbx(raw_t *raw) ...@@ -1114,8 +1159,9 @@ static int decode_rxmsfrbx(raw_t *raw)
{ {
int prn,sat,sys; int prn,sat,sys;
unsigned char *p=raw->buff+6; unsigned char *p=raw->buff+6;
trace(4,"decode_rxmsfrbx: len=%d\n",raw->len); trace(4,"decode_rxmsfrbx: len=%d\n",raw->len);
//printf("decode_rxmsfrbx: len=%d\n",raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype,"UBX RXM-SFRBX (%4d): sys=%d prn=%3d",raw->len, sprintf(raw->msgtype,"UBX RXM-SFRBX (%4d): sys=%d prn=%3d",raw->len,
...@@ -1123,14 +1169,17 @@ static int decode_rxmsfrbx(raw_t *raw) ...@@ -1123,14 +1169,17 @@ static int decode_rxmsfrbx(raw_t *raw)
} }
if (!(sys=ubx_sys(U1(p)))) { if (!(sys=ubx_sys(U1(p)))) {
trace(2,"ubx rxmsfrbx sys id error: sys=%d\n",U1(p)); trace(2,"ubx rxmsfrbx sys id error: sys=%d\n",U1(p));
//printf("ubx rxmsfrbx sys id error: sys=%d\n",U1(p));
return -1; return -1;
} }
prn=U1(p+1)+(sys==SYS_QZS?192:0); prn=U1(p+1)+(sys==SYS_QZS?192:0);
if (!(sat=satno(sys,prn))) { if (!(sat=satno(sys,prn))) {
if (sys==SYS_GLO&&prn==255) { if (sys==SYS_GLO&&prn==255) {
//printf("decode_rxmsfrbx: GLO and prn 255\n",sys,prn);
return 0; /* suppress error for unknown glo satellite */ return 0; /* suppress error for unknown glo satellite */
} }
trace(2,"ubx rxmsfrbx sat number error: sys=%d prn=%d\n",sys,prn); trace(2,"ubx rxmsfrbx sat number error: sys=%d prn=%d\n",sys,prn);
//printf("ubx rxmsfrbx sat number error: sys=%d prn=%d\n",sys,prn);
return -1; return -1;
} }
switch (sys) { switch (sys) {
...@@ -1148,8 +1197,9 @@ static int decode_trksfrbx(raw_t *raw) ...@@ -1148,8 +1197,9 @@ static int decode_trksfrbx(raw_t *raw)
{ {
int prn,sat,sys; int prn,sat,sys;
unsigned char *p=raw->buff+6; unsigned char *p=raw->buff+6;
trace(4,"decode_trksfrbx: len=%d\n",raw->len); trace(4,"decode_trksfrbx: len=%d\n",raw->len);
//printf("decode_trksfrbx: len=%d\n",raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-SFRBX (%4d): sys=%d prn=%3d",raw->len, sprintf(raw->msgtype,"UBX TRK-SFRBX (%4d): sys=%d prn=%3d",raw->len,
...@@ -1186,6 +1236,7 @@ static int decode_timtm2(raw_t *raw) ...@@ -1186,6 +1236,7 @@ static int decode_timtm2(raw_t *raw)
double tr[6],tf[6]; double tr[6],tf[6];
trace(4, "decode_timtm2: len=%d\n", raw->len); trace(4, "decode_timtm2: len=%d\n", raw->len);
//printf("decode_timtm2: len=%d\n", raw->len);
if (raw->outtype) { if (raw->outtype) {
sprintf(raw->msgtype, "UBX TIM-TM2 (%4d)", raw->len); sprintf(raw->msgtype, "UBX TIM-TM2 (%4d)", raw->len);
...@@ -1235,6 +1286,7 @@ static int decode_ubx(raw_t *raw) ...@@ -1235,6 +1286,7 @@ static int decode_ubx(raw_t *raw)
/* checksum */ /* checksum */
if (!checksum(raw->buff,raw->len)) { if (!checksum(raw->buff,raw->len)) {
trace(2,"ubx checksum error: type=%04x len=%d\n",type,raw->len); trace(2,"ubx checksum error: type=%04x len=%d\n",type,raw->len);
//printf("ubx checksum error: type=%04x len=%d\n",type,raw->len);
return -1; return -1;
} }
switch (type) { switch (type) {
...@@ -1306,6 +1358,7 @@ extern int input_ubx(raw_t *raw, unsigned char data) ...@@ -1306,6 +1358,7 @@ extern int input_ubx(raw_t *raw, unsigned char data)
if (raw->nbyte==6) { if (raw->nbyte==6) {
if ((raw->len=U2(raw->buff+4)+8)>MAXRAWLEN) { if ((raw->len=U2(raw->buff+4)+8)>MAXRAWLEN) {
trace(2,"ubx length error: len=%d\n",raw->len); trace(2,"ubx length error: len=%d\n",raw->len);
//printf("ubx length error: len=%d\n",raw->len);
raw->nbyte=0; raw->nbyte=0;
return -1; return -1;
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment