diff --git a/src/ephemeris.c b/src/ephemeris.c
index 75a6d2ac8d6e66b8fb74ca38c8370ca209974150..7a6605fdc00cb2d3bc1517df54b0e2cb5e1628b9 100644
--- a/src/ephemeris.c
+++ b/src/ephemeris.c
@@ -412,7 +412,17 @@ static eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
default: tmax=MAXDTOE+1.0; break;
}
tmin=tmax+1.0;
-
+
+ //DEBUG
+ /*if (iode != -1)
+ {
+ printf("- available iodes: ");
+ for (i=0;i<nav->n;i++)
+ if (nav->eph[i].sat==sat)
+ printf("%3d ",nav->eph[i].iode);
+ }//*/
+
+
for (i=0;i<nav->n;i++) {
if (nav->eph[i].sat!=sat) continue;
if (iode>=0&&nav->eph[i].iode!=iode) continue;
@@ -552,25 +562,44 @@ static int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
{
const sbssatp_t *sbs;
int i;
-
- trace(4,"satpos_sbas: time=%s sat=%2d\n",time_str(time,3),sat);
+
+ trace(4,"satpos_sbas: time=%s sat=%3d\n",time_str(time,3),sat);
+ //DEBUG
+ /*if (satsys(sat,NULL)!=SYS_GAL && satsys(sat,NULL)!=SYS_SBS)
+ printf("satpos_sbas: sat=%3d ",sat);//*/
/* search sbas satellite correciton */
for (i=0;i<nav->sbssat.nsat;i++) {
sbs=nav->sbssat.sat+i;
if (sbs->sat==sat) break;
}
- if (i>=nav->sbssat.nsat) {
- trace(2,"no sbas correction for orbit: %s sat=%2d\n",time_str(time,0),sat);
- ephpos(time,teph,sat,nav,-1,rs,dts,var,svh);
- *svh=-2;// JV: error code for detecting available ephemeris without sbas correction
+ if (i>=nav->sbssat.nsat || sbs->lcorr.iode < 0) {
+ trace(2,"no sbas correction for orbit: %s sat=%3d\n",time_str(time,0),sat);
+ //DEBUG
+ /*if (satsys(sat,NULL)!=SYS_GAL && satsys(sat,NULL)!=SYS_SBS)
+ printf("no sbas correction (i>=nav->sbssat.nsat || sbs->lcorr.iode < 0)\n");//*/
+ if (ephpos(time,teph,sat,nav,-1,rs,dts,var,svh))
+ *svh=-2;// JV: error code for detecting available ephemeris without sbas correction
+ else
+ {
+ *svh=-1;
+ // printf("satpos_sbas: no broadcast eph for sat %2d\n",sat);
+ }
return 0;
}
/* satellite postion and clock by broadcast ephemeris */
- if (!ephpos(time,teph,sat,nav,sbs->lcorr.iode,rs,dts,var,svh)) return 0;
+ //printf("lcorr.iode=%3d ",sbs->lcorr.iode);
+ if (!ephpos(time,teph,sat,nav,sbs->lcorr.iode,rs,dts,var,svh)){
+ //printf("not found\n");
+ return 0;
+ }
/* sbas satellite correction (long term and fast) */
- if (sbssatcorr(time,sat,nav,rs,dts,var)) return 1;
+ if (sbssatcorr(time,sat,nav,rs,dts,var)){
+ //printf("OK!\n");
+ return 1;
+ }
+ //printf("no sbas correction (!sbssatcorr())\n");
*svh=-1;
return 0;
}
@@ -739,8 +768,13 @@ extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
gtime_t time[2*MAXOBS]={{0}};
double dt,pr;
int i,j;
-
+
trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt);
+ /*if (ephopt==2)
+ {
+ printf("====satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt);
+ print_sbassat(&nav->sbssat);
+ }*/
for (i=0;i<n&&i<2*MAXOBS;i++) {
for (j=0;j<6;j++) rs [j+i*6]=0.0;
@@ -781,6 +815,9 @@ extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
trace(4,"%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
time_str(time[i],6),obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],
dts[i*2]*1E9,var[i],svh[i]);
+ //printf("%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
+ // time_str(time[i],6),obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],
+ // dts[i*2]*1E9,var[i],svh[i]);
}
}
/* select satellite ephemeris --------------------------------------------------
diff --git a/src/pntpos.c b/src/pntpos.c
index 60e0bf165c06c26239935c0cc08385ad8d0af978..ddaf284c4d16c1812e28a87c2af21299c23760ad 100644
--- a/src/pntpos.c
+++ b/src/pntpos.c
@@ -114,7 +114,20 @@ extern double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
}
if (opt->ionoopt==IONOOPT_IFLC) { /* dual-frequency */
- if (P1==0.0||P2==0.0) return 0.0;
+// if (sys&SYS_GPS)
+// printf("prange iono-free GPS : ");
+// else if (sys&SYS_GAL)
+// printf("prange iono-free GAL : ");
+// else if (sys&SYS_GLO)
+// printf("prange iono-free GLO : ");
+// else
+// printf("prange iono-free %i : ", sys);
+
+ if (P1==0.0||P2==0.0){
+ //printf("0 pseudorange! P1 = %f P2 = %f\n", P1, P2);
+ return 0.0;
+ }
+ //printf("P1_P2 = %f, P1_C1=%f, P2_C2=%f\n", sys, P1_P2, P1_C1, P2_C2);
if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
if (obs->code[j]==CODE_L2C) P2+=P2_C2; /* C2->P2 */
@@ -232,9 +245,13 @@ static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
ecef2pos(rr,pos);
+ //printf("////////// rescode iter=%i\n",iter);
+
for (i=*ns=0;i<n&&i<MAXOBS;i++) {
vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;
+ //printf("\tsat %3d - ", obs[i].sat);
+
if (!(sys=satsys(obs[i].sat,NULL)))
continue;
@@ -248,20 +265,31 @@ static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
/* geometric distance/azimuth/elevation angle */
if ((r=geodist(rs+i*6,rr,e))<=0.0||
satazel(pos,e,azel+i*2)<opt->elmin)
+ {
+ //printf("discarding because of distance r = %f / elevation = %f\n", r, satazel(pos,e,azel+i*2)*R2D);
continue;
-
+ }
/* psudorange with code bias correction */
if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0)
+ {
+ //printf("discarding because of null prange\n");
continue;
+ }
/* excluded satellite? */
if (satexclude(obs[i].sat,vare[i],svh[i],opt))
+ {
+ //printf("excluded\n");
continue;
+ }
/* ionospheric corrections */
if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2,
iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion))
+ {
+ //printf("discarding because error in iono corr\n");
continue;
+ }
/* GPS-L1 -> L1/B1 */
if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) {
@@ -270,8 +298,12 @@ static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
/* tropospheric corrections */
if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp))
+ {
+ //printf("discarding because error in tropo corr\n");
continue;
+ }
+ //printf(" OK!\n");
/* pseudorange residual */
v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);
@@ -330,6 +362,20 @@ static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;
var[nv++]=0.01;
}
+ // DEBUGGING ================
+ /*{
+ printf("H:\n");
+ for (int row = 0; row < nv; row++)
+ printf("\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", H[row*NX],
+ H[row*NX+1],
+ H[row*NX+2],
+ H[row*NX+3],
+ H[row*NX+4],
+ H[row*NX+5],
+ H[row*NX+6]);
+ printf("\n");
+
+ }//*/
return nv;
}
/* validate solution ---------------------------------------------------------*/
@@ -385,7 +431,8 @@ static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
&ns);
if (nv<NX) {
- sprintf(msg,"lack of valid sats ns=%d",nv);
+ //sprintf(msg,"lack of valid sats ns=%d",nv);
+ sprintf(msg,"lack of valid sats ns=%d, nv=%2d",ns, nv);//JV
break;
}
/* weight by variance */
@@ -397,7 +444,6 @@ static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
- break;
}
for (j=0;j<NX;j++) x[j]+=dx[j];
diff --git a/src/rcv/ublox.c b/src/rcv/ublox.c
index 1fe6734180985e4680a79ee3685ad5b1f9f6188c..1e07b25a68d4b3841b27196c2344244a873f0aa2 100644
--- a/src/rcv/ublox.c
+++ b/src/rcv/ublox.c
@@ -210,8 +210,8 @@ static int sig_idx(int sys, int code)
else if (sys == SYS_GAL) {
if (code==CODE_L1C) return 1;
if (code==CODE_L1B) return 1;
- if (code==CODE_L7I) return 2; /* E5bI */
- if (code==CODE_L7Q) return 2; /* E5bQ */
+ if (code==CODE_L7I) return 3; /* E5bI */
+ if (code==CODE_L7Q) return 3; /* E5bQ */
}
else if (sys == SYS_QZS) {
if (code==CODE_L1C) return 1;
@@ -262,7 +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);
+ //printf("\tdecode_rxmraw: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX RXM-RAW (%4d): nsat=%d",raw->len,U1(p+6));
@@ -340,7 +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);
+ //printf("\tdecode_rxmrawx: len=%d\n",raw->len);
if (raw->len<24) {
trace(2,"ubx rxmrawx length error: len=%d\n",raw->len);
@@ -399,8 +399,14 @@ static int decode_rxmrawx(raw_t *raw)
cpstd=U1(p+28)&15; /* cpStdev (m) */
prstd=1<<(prstd>=5?prstd-5:0); /* prstd=2^(x-5) */
tstat=U1(p+30); /* trkStat */
- if (!(tstat&1)) P=0.0;
- if (!(tstat&2)||L==-0.5||cpstd>cpstd_valid) L=0.0; /* invalid phase */
+ if (!(tstat&1)){
+ //printf("tstat&1 invalid psedorange setting P=0.0\n");
+ P=0.0;
+ }
+ if (!(tstat&2)||L==-0.5||cpstd>cpstd_valid){
+ //printf("tstat&2 invalid phase setting L=0.0\n");
+ L=0.0; /* invalid phase */
+ }
if (!(sys=ubx_sys(gnss))) {
trace(2,"ubx rxmrawx: system error gnss=%d\n", gnss);
@@ -517,6 +523,7 @@ static int decode_ephem(int sat, raw_t *raw)
eph.sat=sat;
raw->nav.eph[sat-1]=eph;
raw->ephsat=sat;
+ //printf("\tdecode_ephem sat %3d eph.iode = %d\n", sat, eph.iode);
return 2;
}
/* decode almanac and ion/utc ------------------------------------------------*/
@@ -563,7 +570,7 @@ static int decode_rxmsfrb(raw_t *raw)
unsigned char *p=raw->buff+6;
trace(4,"decode_rxmsfrb: len=%d\n",raw->len);
- //printf("decode_rxmsfrb: len=%d\n",raw->len);
+ //printf("\tdecode_rxmsfrb: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX RXM-SFRB (%4d): prn=%2d",raw->len,U1(p+1));
@@ -599,7 +606,7 @@ static int decode_navsol(raw_t *raw)
unsigned char *p=raw->buff+6;
trace(4,"decode_navsol: len=%d\n",raw->len);
- //printf("decode_navsol: len=%d\n",raw->len);
+ //printf("\tdecode_navsol: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX NAV-SOL (%4d):",raw->len);
@@ -619,7 +626,7 @@ static int decode_navtime(raw_t *raw)
unsigned char *p=raw->buff+6;
trace(4,"decode_navtime: len=%d\n",raw->len);
- //printf("decode_navtime: len=%d\n",raw->len);
+ //printf("\tdecode_navtime: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX NAV-TIME (%4d):",raw->len);
@@ -647,7 +654,7 @@ static int decode_trkmeas(raw_t *raw)
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);
+ //printf("\tdecode_trkmeas: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-MEAS (%4d):",raw->len);
@@ -780,7 +787,7 @@ static int decode_trkd5(raw_t *raw)
unsigned char *p=raw->buff+6;
trace(4,"decode_trkd5: len=%d\n",raw->len);
- //printf("decode_trkd5: len=%d\n",raw->len);
+ //printf("\tdecode_trkd5: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-D5 (%4d):",raw->len);
@@ -897,24 +904,24 @@ static int decode_nav(raw_t *raw, int sat, int off)
//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);
+ //printf("\tdecode_nav: p = %hhx\n", *p);
+ //printf("\tdecode_nav: p+1 = %hhx\n", *(p+1));
+ //printf("\tdecode_nav: p+2 = %hhx\n", *(p+2));
+ //printf("\tdecode_nav: p+3 = %hhx\n", *(p+3));
+ //printf("\tdecode_nav: U4(p) = %lu\n",U4(p));
+ //printf("\tdecode_nav: U4(p)>>24 = %lu\n",U4(p)>>24);
+// printf("\tdecode_nav: U4(p+4) = %lu\n",U4(p+4));
+// printf("\tdecode_nav: U4(p+4)>>24 = %lu\n",U4(p+4)>>24);
+// printf("\tdecode_nav: U4(p+8) = %lu\n",U4(p+8));
+// printf("\tdecode_nav: U4(p+8)>>24 = %lu\n",U4(p+8)>>24);
+// printf("\tdecode_nav: U4(p+12) = %lu\n",U4(p+12));
+// printf("\tdecode_nav: U4(p+12)>>24 = %lu\n",U4(p+12)>>24);
+// printf("\tdecode_nav: U4(p+16) = %lu\n",U4(p+16));
+// printf("\tdecode_nav: U4(p+16)>>24 = %lu\n",U4(p+16)>>24);
+// printf("\tdecode_nav: U4(p+20) = %lu\n",U4(p+20));
+// printf("\tdecode_nav: U4(p+20)>>24 = %lu\n",U4(p+20)>>24);
+// printf("\tdecode_nav: U4(p+24) = %lu\n",U4(p+24));
+// printf("\tdecode_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);
@@ -924,7 +931,7 @@ static int decode_nav(raw_t *raw, int sat, int off)
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);
+ //printf("\tdecode_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);
@@ -942,7 +949,7 @@ 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");
+ //printf("\tdecode_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;
@@ -1015,12 +1022,13 @@ static int decode_enav(raw_t *raw, int sat, int off)
eph.sat=sat;
raw->nav.eph[sat-1]=eph;
raw->ephsat=sat;
+ //printf("\tdecode_enav sat %3d eph.iode = %d\n", sat, eph.iode);
return 2;
}
/* decode beidou navigation data ---------------------------------------------*/
static int decode_cnav(raw_t *raw, int sat, int off)
{
- //printf("decode_cnav\n");
+ //printf("\tdecode_cnav\n");
eph_t eph={0};
unsigned int words[10];
int i,id,pgn,prn;
@@ -1081,7 +1089,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");
+ //printf("\tdecode_gnav\n");
geph_t geph={0};
int i,j,k,m,prn;
unsigned char *p=raw->buff+6+off,buff[64],*fid;
@@ -1128,12 +1136,13 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq)
}
raw->nav.geph[prn-1]=geph;
raw->ephsat=sat;
+ //printf("\tdecode_gnav geph.iode = %d\n", geph.iode);
return 2;
}
/* decode sbas navigation data -----------------------------------------------*/
static int decode_snav(raw_t *raw, int sat, int off)
{
- //printf("decode_snav\n");
+ //printf("\tdecode_snav\n");
int i,j,k,prn,tow,week;
unsigned char *p=raw->buff+6+off,buff[64];
@@ -1161,7 +1170,7 @@ static int decode_rxmsfrbx(raw_t *raw)
unsigned char *p=raw->buff+6;
trace(4,"decode_rxmsfrbx: len=%d\n",raw->len);
- //printf("decode_rxmsfrbx: len=%d\n",raw->len);
+ //printf("\tdecode_rxmsfrbx: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX RXM-SFRBX (%4d): sys=%d prn=%3d",raw->len,
@@ -1175,7 +1184,7 @@ static int decode_rxmsfrbx(raw_t *raw)
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);
+ //printf("\tdecode_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);
@@ -1199,7 +1208,7 @@ static int decode_trksfrbx(raw_t *raw)
unsigned char *p=raw->buff+6;
trace(4,"decode_trksfrbx: len=%d\n",raw->len);
- //printf("decode_trksfrbx: len=%d\n",raw->len);
+ //printf("\tdecode_trksfrbx: len=%d\n",raw->len);
if (raw->outtype) {
sprintf(raw->msgtype,"UBX TRK-SFRBX (%4d): sys=%d prn=%3d",raw->len,
@@ -1236,7 +1245,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);
+ //printf("\tdecode_timtm2: len=%d\n", raw->len);
if (raw->outtype) {
sprintf(raw->msgtype, "UBX TIM-TM2 (%4d)", raw->len);
@@ -1496,7 +1505,9 @@ extern int gen_ubx(const char *msg, unsigned char *buff)
{FU1,FU1}, /* RINV */
{FU1,FU1,FU2,FU2,FU1,FU1,FU2,FU2,FU2,FU2,FU4}, /* SMGR */
{FU1,FU1,FU2,FI4,FI4,FI4,FU4,FU4,FU4}, /* TMODE2 */
- {FU1,FU1,FU2,FI4,FI4,FI4,FU4,FU4,FU4}, /* TMODE3 */
+ //{FU1,FU1,FU2,FI4,FI4,FI4,FU4,FU4,FU4}, /* TMODE3 */ changed according to RTKLIB pull request 334: https://github.com/tomojitakasu/RTKLIB/pull/334
+ {FU1,FU1,FU2,FI4,FI4,FI4,FI1,FI1,FI1,FU1,FU4,FU4,FU4,FU1,FU1,FU1,FU1,FU1,
+ FU1,FU1,FU1}, /* TMODE3 */
{FU1,FU1,FU1,FU1,FI2,FI2,FU4,FU4,FU4,FU4,FI4,FU4}, /* TPS */
{FU1,FU1,FU1,FU1,FU4,FU4,FU4,FU4,FU4}, /* TXSLOT */
{FU1,FU1,FU1,FU1} /* VALSET */ /* Skip configuration for now */
diff --git a/src/rtkcmn.c b/src/rtkcmn.c
index f8565bdf7d18dba6abbd6e9c235cb8227c94ec8a..419a8a50449a957d112f281ea994e54b8a5f7349 100644
--- a/src/rtkcmn.c
+++ b/src/rtkcmn.c
@@ -535,19 +535,33 @@ extern int satexclude(int sat, double var, int svh, const prcopt_t *opt)
{
int sys=satsys(sat,NULL);
- if (svh<0) return 1; /* ephemeris unavailable */
+ if (svh<0)
+ {
+ //printf("svh < 0 (eph unavailable) ");
+ return 1; /* ephemeris unavailable */
+ }
if (opt) {
- if (opt->exsats[sat-1]==1) return 1; /* excluded satellite */
+ if (opt->exsats[sat-1]==1)
+ {
+ //printf("in exsats list! ");
+ return 1; /* excluded satellite */
+ }
if (opt->exsats[sat-1]==2) return 0; /* included satellite */
- if (!(sys&opt->navsys)) return 1; /* unselected sat sys */
+ if (!(sys&opt->navsys))
+ {
+ //printf("constellation not used! ");
+ return 1; /* unselected sat sys */
+ }
}
if (sys==SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */
if (svh) {
+ //printf("unhealthy satellite! ");
trace(3,"unhealthy satellite: sat=%3d svh=%02X\n",sat,svh);
return 1;
}
if (var>MAX_VAR_EPH) {
+ //printf("invalid ura satellite! ");
trace(3,"invalid ura satellite: sat=%3d ura=%.2f\n",sat,sqrt(var));
return 1;
}
@@ -2697,6 +2711,14 @@ extern void uniqnav(nav_t *nav)
nav->lam[i][j]=satwavelen(i+1,j,nav);
}
}
+/* update navigation data ----------------------------------------------------*/
+extern void updatenav(nav_t *nav)
+{
+ int i,j;
+ for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) {
+ nav->lam[i][j]=satwavelen(i+1,j,nav);
+ }
+}
/* compare observation data -------------------------------------------------*/
static int cmpobs(const void *p1, const void *p2)
{
diff --git a/src/rtklib.h b/src/rtklib.h
index d7bf3a162b8b8bf0cff291e6958ec3078380b4c8..e28fdac31028e939f79e108ae3b012d48964a63b 100644
--- a/src/rtklib.h
+++ b/src/rtklib.h
@@ -1821,6 +1821,7 @@ EXPORT int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, double *delay, double *var);
EXPORT double sbstropcorr(gtime_t time, const double *pos, const double *azel,
double *var);
+EXPORT void print_sbassat(const sbssat_t *sbssat);
/* options functions ---------------------------------------------------------*/
EXPORT opt_t *searchopt(const char *name, const opt_t *opts);
diff --git a/src/rtksvr.c b/src/rtksvr.c
index 34a33607185a67c118e2e119cd70b1f0788e0093..c2954ac48cfef14faf57aa596356aeb4fa71ca79 100644
--- a/src/rtksvr.c
+++ b/src/rtksvr.c
@@ -112,16 +112,16 @@ static void writesol(rtksvr_t *svr, int index)
static void updatefcn(rtksvr_t *svr)
{
int i,j,sat,frq;
-
+
for (i=0;i<MAXPRNGLO;i++) {
sat=satno(SYS_GLO,i+1);
-
+
for (j=0,frq=-999;j<3;j++) {
if (svr->raw[j].nav.geph[i].sat!=sat) continue;
frq=svr->raw[j].nav.geph[i].frq;
}
if (frq<-7||frq>6) continue;
-
+
for (j=0;j<3;j++) {
if (svr->raw[j].nav.geph[i].sat==sat) continue;
svr->raw[j].nav.geph[i].sat=sat;
diff --git a/src/sbas.c b/src/sbas.c
index 6920ffbc99a27e037b13f1285b0fe35b38daf70d..b36c8c2b8c6b4182ceb97e87377dc1d9247e058a 100644
--- a/src/sbas.c
+++ b/src/sbas.c
@@ -123,14 +123,79 @@ static double degfcorr(int ai)
};
return 0<ai&&ai<=15?degf[ai]:0.0058;
}
+/* print sbsfcorr_t (JV) -------------------------------------------------------*/
+static void print_sbsfcorr(const sbsfcorr_t *sbsfcorr)
+{
+ printf("iodf: %3d, udre: %2d, [%f, %f, %f, %d]\n",
+ sbsfcorr->iodf,
+ sbsfcorr->udre-1,//udre = UDRE+1
+ sbsfcorr->prc,
+ sbsfcorr->rrc,
+ sbsfcorr->dt,
+ sbsfcorr->ai);
+}
+/* print sbslcorr_t (JV) -------------------------------------------------------*/
+static void print_sbslcorr(const sbslcorr_t *sbslcorr)
+{
+ printf("iode: %3d, [%f, %f, %f | %f, %f, %f | %f, %f]\n",
+ sbslcorr->iode,
+ sbslcorr->dpos[0],sbslcorr->dpos[1],sbslcorr->dpos[2],
+ sbslcorr->dvel[0],sbslcorr->dvel[1],sbslcorr->dvel[2],
+ sbslcorr->daf0,sbslcorr->daf1);
+}
+/* print sbssatp_t (JV) -------------------------------------------------------*/
+static void print_sbssatp(const sbssatp_t *sbssatp)
+{
+ printf(" sat %3d:", sbssatp->sat); /* satellite number */
+ printf(" fcorr: ");
+ print_sbsfcorr(&sbssatp->fcorr);/* fast correction */
+ if (sbssatp->lcorr.iode > 0)
+ {
+ printf(" lcorr: ");
+ print_sbslcorr(&sbssatp->lcorr);/* long term correction */
+ }
+}
+/* print sbssat_t (JV) -------------------------------------------------------*/
+extern void print_sbassat(const sbssat_t *sbssat)
+{
+ printf("iodp: %d\n", sbssat->iodp); /* IODP (issue of date mask) */
+ printf("tlat: %d\n", sbssat->tlat); /* system latency (s) */
+ printf("nsat: %d\n", sbssat->nsat); /* number of satellites */
+ printf("sat[]:\n");
+ for (int i = 0; i < sbssat->nsat; i++)
+ print_sbssatp(&sbssat->sat[i]);
+}
/* decode type 1: prn masks --------------------------------------------------*/
static int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
{
- int i,n,sat;
+ int i,i_new,i_old,new_n,n,sat;
+ sbssatp_t sbssat_zero = { 0 };
+ sbssat_zero.lcorr.iode = -1;
+ sbssat_zero.fcorr.udre = 15; // udre=UDRE+1: udre=15->UDRE=14 (not monitored)
trace(4,"decode_sbstype1:\n");
-
- for (i=1,n=0;i<=210&&n<MAXSAT;i++) {
+ //printf(" decode_sbstype1: \n");
+
+ // old_sat to store old data (to be used if changing IODP, prn mask)
+ sbssatp_t old_sat[MAXSAT];
+ sbssatp_t new_sat[MAXSAT];
+ int old_n = sbssat->nsat;
+ // store and remove old sat (if changing IODP)
+ //if (sbssat->iodp!=getbitu(msg->msg,224,2))
+ {
+ for (i=0; i<MAXSAT;i++)
+ {
+ // store old
+ old_sat[i] = sbssat->sat[i];
+ //printf("old_sat[%d].sat = %d\n", i, old_sat[i].sat);
+ // remove current
+ sbssat->sat[i] = sbssat_zero;
+ // initialize new
+ new_sat[i] = sbssat_zero;
+ }
+ }
+
+ for (i=1,new_n=0;i<=210&&new_n<MAXSAT;i++) {
if (getbitu(msg->msg,13+i,1)) {
if (i<= 37) sat=satno(SYS_GPS,i); /* 0- 37: gps */
else if (i<= 61) sat=satno(SYS_GLO,i-37); /* 38- 61: glonass */
@@ -140,13 +205,75 @@ static int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
else if (i<=192) sat=satno(SYS_SBS,i+10); /* 183-192: qzss ref [2] */
else if (i<=202) sat=satno(SYS_QZS,i); /* 193-202: qzss ref [2] */
else sat=0; /* 203- : reserved */
- sbssat->sat[n++].sat=sat;
+ new_sat[new_n++].sat=sat;
+ //sbssat->sat[n++].sat=sat;
+ //printf(" sat: %d\n",sat);
+ //printf("new_sat[%d].sat = %d\n", new_n-1, new_sat[new_n-1].sat);
}
}
+
+ // DEBUG
+ /*if (sbssat->iodp==getbitu(msg->msg,224,2))
+ {
+ if (new_n != old_n)
+ printf("!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!\nnew_n != old_n!");
+ else
+ for (i=0; i<new_n;i++)
+ if (old_sat[i].sat != new_sat[i].sat)
+ printf("!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!\n!!!!!!!!!!!!!!!!!!!!\ni=%d different sats!",i);
+ }
+ if (sbssat->iodp!=getbitu(msg->msg,224,2))
+ printf(">>>>>>>>>>>>>>>>>>>>>>>\n>>>>>>>>>>>>>>>>>>>>>>>\n>>>>>>>>>>>>>>>>>>>>>>>\nIODP CHANGED\n>>>>>>>>>>>>>>>>>>>>>>>\n>>>>>>>>>>>>>>>>>>>>>>>\n>>>>>>>>>>>>>>>>>>>>>>>\n");
+ //*/
+
+ // merge new and old data
+ //if (new_n != old_n || sbssat->iodp!=getbitu(msg->msg,224,2))
+ //{
+ for (n=0,i_new=0, i_old=0; n<MAXSAT;)
+ {
+ // no more new nor old
+ if (i_new==new_n && i_old==old_n)
+ break;
+ // no more new
+ else if (i_new==new_n)
+ {
+ sbssat->sat[n++] = old_sat[i_old];
+ i_old++;
+ }
+ // no more old
+ else if (i_old==old_n)
+ {
+ sbssat->sat[n++] = new_sat[i_new];
+ i_new++;
+ }
+ // more new and old
+ else
+ {
+ if (old_sat[i_old].sat == new_sat[i_new].sat)
+ {
+ sbssat->sat[n++] = old_sat[i_old];
+ i_new++;
+ i_old++;
+ }
+ else if (old_sat[i_old].sat > new_sat[i_new].sat)
+ {
+ sbssat->sat[n++] = new_sat[i_new];
+ i_new++;
+ }
+ else
+ {
+ sbssat->sat[n++] = old_sat[i_old];
+ i_old++;
+ }
+ }
+ //printf("sat[%d].sat = %d\n", n-1, sbssat->sat[n-1].sat);
+ }
+ //}
sbssat->iodp=getbitu(msg->msg,224,2);
sbssat->nsat=n;
-
+
trace(5,"decode_sbstype1: nprn=%d iodp=%d\n",n,sbssat->iodp);
+ //printf(" decode_sbstype1: %d sats, iodp=%d\n",n,sbssat->iodp);
return 1;
}
/* decode type 2-5,0: fast corrections ---------------------------------------*/
@@ -157,11 +284,15 @@ static int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
gtime_t t0;
trace(4,"decode_sbstype2:\n");
-
- if (sbssat->iodp!=(int)getbitu(msg->msg,16,2)) return 0;
+
+ if (sbssat->iodp!=(int)getbitu(msg->msg,16,2)){
+ //printf("decode_sbstype24 wrong iodp %3d (current %3d)\n", (int)getbitu(msg->msg,16,2), sbssat->iodp);
+ return 0;
+ }
type=getbitu(msg->msg, 8,6);
iodf=getbitu(msg->msg,14,2);
+ //printf(" decode_sbstype2: type=%d iodf=%d\n",type,iodf);
for (i=0;i<13;i++) {
if ((j=13*((type==0?2:type)-2)+i)>=sbssat->nsat) break;
@@ -181,6 +312,7 @@ static int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
sbssat->sat[j].fcorr.dt=dt;
}
sbssat->sat[j].fcorr.iodf=iodf;
+ //printf(" sat %d\n",sbssat->sat[j].sat);
}
trace(5,"decode_sbstype2: type=%d iodf=%d\n",type,iodf);
return 1;
@@ -201,6 +333,7 @@ static int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
sbssat->sat[i].fcorr.udre=udre+1;
}
trace(5,"decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]);
+ //printf(" decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]);
return 1;
}
/* decode type 7: fast correction degradation factor -------------------------*/
@@ -210,8 +343,10 @@ static int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4,"decode_sbstype7\n");
- if (sbssat->iodp!=(int)getbitu(msg->msg,18,2)) return 0;
-
+ if (sbssat->iodp!=(int)getbitu(msg->msg,18,2)){
+ //printf("decode_sbstype24 wrong iodp %3d (current %3d)\n", (int)getbitu(msg->msg,18,2), sbssat->iodp);
+ return 0;
+ }
sbssat->tlat=getbitu(msg->msg,14,4);
for (i=0;i<sbssat->nsat&&i<MAXSAT;i++) {
@@ -301,6 +436,7 @@ static int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
if (n==0||n>MAXSAT) return 0;
sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8);
+ //printf(" decode_longcorr0: sat=%d n=%d iode=%d\n",sbssat->sat[n-1].sat,n,sbssat->sat[n-1].lcorr.iode);
for (i=0;i<3;i++) {
sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+9*i,9)*0.125;
@@ -309,7 +445,7 @@ static int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+41,10)*P2_31;
sbssat->sat[n-1].lcorr.daf1=0.0;
sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow);
-
+
trace(5,"decode_longcorr0:sat=%2d\n",sbssat->sat[n-1].sat);
return 1;
}
@@ -323,6 +459,7 @@ static int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
if (n==0||n>MAXSAT) return 0;
sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8);
+ //printf(" decode_longcorr1: sat=%d n=%d iode=%d\n",sbssat->sat[n-1].sat,n,sbssat->sat[n-1].lcorr.iode);
for (i=0;i<3;i++) {
sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+i*11,11)*0.125;
@@ -334,7 +471,7 @@ static int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
if (t<=-43200) t+=86400;
else if (t> 43200) t-=86400;
sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t);
-
+
trace(5,"decode_longcorr1: sat=%2d\n",sbssat->sat[n-1].sat);
return 1;
}
@@ -361,7 +498,10 @@ static int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4,"decode_sbstype24:\n");
- if (sbssat->iodp!=(int)getbitu(msg->msg,110,2)) return 0; /* check IODP */
+ if (sbssat->iodp!=(int)getbitu(msg->msg,110,2)){
+ //printf("decode_sbstype24 wrong iodp %3d (current %3d)\n", (int)getbitu(msg->msg,110,2), sbssat->iodp);
+ return 0; /* check IODP */
+ }
blk =getbitu(msg->msg,112,2);
iodf=getbitu(msg->msg,114,2);
@@ -374,6 +514,7 @@ static int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
sbssat->sat[j].fcorr.prc =getbits(msg->msg,14+i*12,12)*0.125f;
sbssat->sat[j].fcorr.udre=udre+1;
sbssat->sat[j].fcorr.iodf=iodf;
+ //printf(" decode_sbstype24: sat=%2d fcorr.iodf=%2d\n",sbssat->sat[j].sat,iodf);
}
return decode_longcorrh(msg,120,sbssat);
}
@@ -423,11 +564,14 @@ static int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
extern int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
{
int type=getbitu(msg->msg,8,6),stat=-1;
-
trace(3,"sbsupdatecorr: type=%d\n",type);
+ //printf("sbsupdatecorr: type=%d\n",type);
if (msg->week==0) return -1;
+ //printf("============== sbsupdatecorr: PROCESSING PRN %3d type=%d...\n",msg->prn, type);
+ //print_sbassat(&nav->sbssat);
+
switch (type) {
case 0: stat=decode_sbstype2 (msg,&nav->sbssat); break;
case 1: stat=decode_sbstype1 (msg,&nav->sbssat); break;
@@ -446,6 +590,8 @@ extern int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
/*default: trace(2,"unsupported sbas message: type=%d\n",type); break;*/
}
+ //printf("============== sbsupdatecorr: type=%d PROCESSED result = %d!\n",type, stat?type:-1);
+ //print_sbassat(&nav->sbssat);
return stat?type:-1;
}
/* read sbas log file --------------------------------------------------------*/
@@ -788,6 +934,7 @@ static int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
if (fabs(t)>MAXSBSAGEL) {
trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n",
time_str(time,0),sat,t);
+ //printf("sbas long-term corr expired");
return 0;
}
for (i=0;i<3;i++) drs[i]=p->lcorr.dpos[i]+p->lcorr.dvel[i]*t;
@@ -802,6 +949,7 @@ static int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
if (satsys(sat,NULL)==SYS_SBS) return 1;
trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
+ //printf("no sbas long-term corr");
return 0;
}
/* fast correction -----------------------------------------------------------*/
@@ -818,7 +966,7 @@ static int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
if (p->fcorr.t0.time==0) break;
t=timediff(time,p->fcorr.t0)+sbssat->tlat;
- /* expire age of correction or UDRE==14 (not monitored) */
+ /* expire age of correction or UDRE==14 (not monitored) (udre=UDRE+1) */
if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue;
*prc=p->fcorr.prc;
#ifdef RRCENA
@@ -833,6 +981,7 @@ static int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
return 1;
}
trace(2,"no sbas fast correction: %s sat=%2d\n",time_str(time,0),sat);
+ //printf("no sbas fast corr");
return 0;
}
/* sbas satellite ephemeris and clock correction -------------------------------