diff --git a/src/rcv/ublox.c b/src/rcv/ublox.c
index 345c66ce80000c97cf5ddfdb2cbaff468c669297..1fe6734180985e4680a79ee3685ad5b1f9f6188c 100644
--- a/src/rcv/ublox.c
+++ b/src/rcv/ublox.c
@@ -108,11 +108,11 @@ typedef enum { false, true } bool;
/* get fields (little-endian) ------------------------------------------------*/
#define U1(p) (*((unsigned 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 long U4(unsigned char *p) {unsigned long u; memcpy(&u,p,4); return u;}
-static int I4(unsigned char *p) {signed long u; memcpy(&u,p,4); return u;}
-static float R4(unsigned char *p) {float r; memcpy(&r,p,4); return r;}
-static double R8(unsigned char *p) {double r; memcpy(&r,p,8); return r;}
+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=0; 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=0; memcpy(&r,p,4); 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);}
@@ -262,6 +262,7 @@ static int decode_rxmraw(raw_t *raw)
char *q;
trace(4,"decode_rxmraw: len=%d\n",raw->len);
+ //printf("decode_rxmraw: len=%d\n",raw->len);
if (raw->outtype) {
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)
int week,nmeas,ver,gnss,svid,sigid,frqid,lockt,cn0,cpstd,prstd,tstat;
trace(4,"decode_rxmrawx: len=%d\n",raw->len);
+ //printf("decode_rxmrawx: len=%d\n",raw->len);
if (raw->len<24) {
trace(2,"ubx rxmrawx length error: len=%d\n",raw->len);
@@ -559,8 +561,9 @@ static int decode_rxmsfrb(raw_t *raw)
unsigned int words[10];
int i,prn,sat,sys,id;
unsigned char *p=raw->buff+6;
-
+
trace(4,"decode_rxmsfrb: len=%d\n",raw->len);
+ //printf("decode_rxmsfrb: len=%d\n",raw->len);
if (raw->outtype) {
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)
{
int itow,ftow,week;
unsigned char *p=raw->buff+6;
-
+
trace(4,"decode_navsol: len=%d\n",raw->len);
+ //printf("decode_navsol: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX NAV-SOL (%4d):",raw->len);
@@ -613,8 +617,9 @@ static int decode_navtime(raw_t *raw)
{
int itow,ftow,week;
unsigned char *p=raw->buff+6;
-
+
trace(4,"decode_navtime: len=%d\n",raw->len);
+ //printf("decode_navtime: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX NAV-TIME (%4d):",raw->len);
@@ -640,8 +645,9 @@ static int decode_trkmeas(raw_t *raw)
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_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);
+ //printf("decode_trkmeas: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-MEAS (%4d):",raw->len);
@@ -772,8 +778,9 @@ static int decode_trkd5(raw_t *raw)
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;
unsigned char *p=raw->buff+6;
-
+
trace(4,"decode_trkd5: len=%d\n",raw->len);
+ //printf("decode_trkd5: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-D5 (%4d):",raw->len);
@@ -883,22 +890,45 @@ static int decode_nav(raw_t *raw, int sat, int off)
unsigned int words[10];
int i,id;
unsigned char *p=raw->buff+6+off;
+
if (raw->len<48+off) {
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;
}
+ //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) {
trace(3,"ubx rawsfrbx cnav not supported sat=%d prn=%d\n",sat,
(U4(p)>>18)&0x3F);
+ //printf("ubx rawsfrbx cnav not supported sat=%d prn=%d\n",sat,(U4(p)>>18)&0x3F);
return 0;
}
for (i=0;i<10;i++,p+=4) words[i]=U4(p)>>6; /* 24 bits without parity */
id=(words[1]>>2)&7;
+ //printf("decode_nav: sat=%2d id=%d len=%d\n",sat,id,raw->len);
if (id<1||5<id) {
trace(2,"ubx rawsfrbx subfrm id error: sat=%2d id=%d len=%d\n",sat,id,
raw->len);
+ //printf("ubx rawsfrbx subfrm id error: sat=%2d id=%d len=%d\n",sat,id,raw->len);
return -1;
}
for (i=0;i<10;i++) {
@@ -912,12 +942,14 @@ static int decode_nav(raw_t *raw, int sat, int off)
/* decode galileo navigation data --------------------------------------------*/
static int decode_enav(raw_t *raw, int sat, int off)
{
+ //printf("decode_enav\n");
eph_t eph={0};
unsigned char *p=raw->buff+6+off,buff[32],crc_buff[26]={0};
int i,j,k,part1,page1,part2,page2,type;
if (raw->len<44+off) {
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;
}
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)
/* test even-odd parts */
if (part1!=0||part2!=1) {
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;
}
/* test crc (4(pad) + 114 + 82 bits) */
@@ -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));
if (rtk_crc24q(crc_buff,25)!=getbitu(buff+16,82,24)) {
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;
}
type=getbitu(buff,2,6); /* word type */
@@ -970,6 +1004,7 @@ static int decode_enav(raw_t *raw, int sat, int off)
/* test svid consistency */
if (eph.sat!=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;
}
if (!strstr(raw->opt,"-EPHALL")) {
@@ -985,6 +1020,7 @@ static int decode_enav(raw_t *raw, int sat, int off)
/* decode beidou navigation data ---------------------------------------------*/
static int decode_cnav(raw_t *raw, int sat, int off)
{
+ //printf("decode_cnav\n");
eph_t eph={0};
unsigned int words[10];
int i,id,pgn,prn;
@@ -992,6 +1028,7 @@ static int decode_cnav(raw_t *raw, int sat, int off)
if (raw->len<48+off) {
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;
}
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)
id=(words[0]>>12)&0x07; /* subframe id (3bit) */
if (id<1||5<id) {
trace(2,"ubx rawsfrbx subfrm id error: sat=%2d\n",sat);
+ //printf("ubx rawsfrbx subfrm id error: sat=%2d\n",sat);
return -1;
}
if (prn>5&&prn<59) { /* IGSO/MEO */
@@ -1019,6 +1057,7 @@ static int decode_cnav(raw_t *raw, int sat, int off)
pgn=(words[1]>>14)&0x0F; /* page number (4bit) */
if (pgn<1||10<pgn) {
trace(2,"ubx rawsfrbx page number error: sat=%2d\n",sat);
+ //printf("ubx rawsfrbx page number error: sat=%2d\n",sat);
return -1;
}
for (i=0;i<10;i++) {
@@ -1042,6 +1081,7 @@ static int decode_cnav(raw_t *raw, int sat, int off)
/* decode glonass navigation data --------------------------------------------*/
static int decode_gnav(raw_t *raw, int sat, int off, int frq)
{
+ //printf("decode_gnav\n");
geph_t geph={0};
int i,j,k,m,prn;
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)
if (raw->len<24+off) {
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;
}
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)
/* test hamming of glonass string */
if (!test_glostr(buff)) {
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;
}
m=getbitu(buff,1,4);
if (m<1||15<m) {
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;
}
/* flush frame buffer if frame-id changed */
@@ -1090,11 +1133,13 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq)
/* decode sbas navigation data -----------------------------------------------*/
static int decode_snav(raw_t *raw, int sat, int off)
{
+ //printf("decode_snav\n");
int i,j,k,prn,tow,week;
unsigned char *p=raw->buff+6+off,buff[64];
if (raw->len<40+off) {
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;
}
tow=(int)time2gpst(timeadd(raw->time,-1.0),&week);
@@ -1114,8 +1159,9 @@ static int decode_rxmsfrbx(raw_t *raw)
{
int prn,sat,sys;
unsigned char *p=raw->buff+6;
-
+
trace(4,"decode_rxmsfrbx: len=%d\n",raw->len);
+ //printf("decode_rxmsfrbx: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX RXM-SFRBX (%4d): sys=%d prn=%3d",raw->len,
@@ -1123,14 +1169,17 @@ static int decode_rxmsfrbx(raw_t *raw)
}
if (!(sys=ubx_sys(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;
}
prn=U1(p+1)+(sys==SYS_QZS?192:0);
if (!(sat=satno(sys,prn))) {
if (sys==SYS_GLO&&prn==255) {
+ //printf("decode_rxmsfrbx: GLO and prn 255\n",sys,prn);
return 0; /* suppress error for unknown glo satellite */
}
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;
}
switch (sys) {
@@ -1148,8 +1197,9 @@ static int decode_trksfrbx(raw_t *raw)
{
int prn,sat,sys;
unsigned char *p=raw->buff+6;
-
+
trace(4,"decode_trksfrbx: len=%d\n",raw->len);
+ //printf("decode_trksfrbx: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-SFRBX (%4d): sys=%d prn=%3d",raw->len,
@@ -1186,6 +1236,7 @@ static int decode_timtm2(raw_t *raw)
double tr[6],tf[6];
trace(4, "decode_timtm2: len=%d\n", raw->len);
+ //printf("decode_timtm2: len=%d\n", raw->len);
if (raw->outtype) {
sprintf(raw->msgtype, "UBX TIM-TM2 (%4d)", raw->len);
@@ -1235,6 +1286,7 @@ static int decode_ubx(raw_t *raw)
/* checksum */
if (!checksum(raw->buff,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;
}
switch (type) {
@@ -1306,6 +1358,7 @@ extern int input_ubx(raw_t *raw, unsigned char data)
if (raw->nbyte==6) {
if ((raw->len=U2(raw->buff+4)+8)>MAXRAWLEN) {
trace(2,"ubx length error: len=%d\n",raw->len);
+ //printf("ubx length error: len=%d\n",raw->len);
raw->nbyte=0;
return -1;
}