From ccd91b8fcb7771ec32d4198d77eb577f9a80f061 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 24 Jul 2020 13:03:02 +0200
Subject: [PATCH] handling IODP and prn masks in SBAS msgs
---
src/ephemeris.c | 6 ++--
src/rcv/ublox.c | 4 +--
src/sbas.c | 84 +++++++++++++++++++++++++++++++++++++++----------
3 files changed, 73 insertions(+), 21 deletions(-)
diff --git a/src/ephemeris.c b/src/ephemeris.c
index 40fe754..19086d7 100644
--- a/src/ephemeris.c
+++ b/src/ephemeris.c
@@ -567,7 +567,7 @@ static int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
}
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);
- printf("no sbas correction (i>=nav->sbssat.nsat || sbs->lcorr.iode < 0)\n",sat);
+ 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
@@ -760,11 +760,11 @@ extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
int i,j;
trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt);
- if (ephopt==2)
+ /*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;
diff --git a/src/rcv/ublox.c b/src/rcv/ublox.c
index 2b66585..b9d0944 100644
--- a/src/rcv/ublox.c
+++ b/src/rcv/ublox.c
@@ -523,7 +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 eph.iode = %d\n", eph.iode);
+ printf("\tdecode_ephem sat %3d eph.iode = %d\n", sat, eph.iode);
return 2;
}
/* decode almanac and ion/utc ------------------------------------------------*/
@@ -1022,7 +1022,7 @@ 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 eph.iode = %d\n", eph.iode);
+ printf("\tdecode_enav sat %3d eph.iode = %d\n", sat, eph.iode);
return 2;
}
/* decode beidou navigation data ---------------------------------------------*/
diff --git a/src/sbas.c b/src/sbas.c
index f0aedae..e06504c 100644
--- a/src/sbas.c
+++ b/src/sbas.c
@@ -149,8 +149,11 @@ static void print_sbssatp(const sbssatp_t *sbssatp)
printf("\tsat: %d\n", sbssatp->sat); /* satellite number */
printf("\t\tfcorr:\t");
print_sbsfcorr(&sbssatp->fcorr);/* fast correction */
- printf("\t\tlcorr:\t");
- print_sbslcorr(&sbssatp->lcorr);/* long term correction */
+ if (sbssatp->lcorr.iode > 0)
+ {
+ printf("\t\tlcorr:\t");
+ print_sbslcorr(&sbssatp->lcorr);/* long term correction */
+ }
}
/* print sbssat_t (JV) -------------------------------------------------------*/
extern void print_sbassat(const sbssat_t *sbssat)
@@ -158,7 +161,7 @@ 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");
+ printf("sat[]:\n");
for (int i = 0; i < sbssat->nsat; i++)
print_sbssatp(&sbssat->sat[i]);
}
@@ -170,6 +173,21 @@ static int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4,"decode_sbstype1:\n");
//printf("\tdecode_sbstype1: \n");
+ // old_sat to store old data (to be used if changing IODP, prn mask)
+ sbssatp_t old_sat[MAXSAT];
+ int old_nsat = sbssat->nsat;
+ // store and remove old sat (if changing IODP)
+ if (sbssat->iodp!=getbitu(msg->msg,224,2))
+ {
+ sbssatp_t sbssat_zero = { 0 };
+ for (int i=0; i<MAXSAT;i++)
+ {
+ old_sat[i] = sbssat->sat[i];
+ sbssat->sat[i] = sbssat_zero;
+ sbssat->sat[i].lcorr.iode = -1;
+ }
+ }
+
for (i=1,n=0;i<=210&&n<MAXSAT;i++) {
if (getbitu(msg->msg,13+i,1)) {
if (i<= 37) sat=satno(SYS_GPS,i); /* 0- 37: gps */
@@ -184,11 +202,31 @@ static int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
//printf("\t\tsat: %d\n",sat);
}
}
+
+ // copy from old data (if changing IODP)
+ if (sbssat->iodp!=getbitu(msg->msg,224,2))
+ {
+ printf("IODP CHANGED!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n!\n");
+ sbssat_t sbssat_zero = { 0 };
+ for (int i=0, i_old=0; i<n&&i_old<old_nsat;)
+ {
+ if (old_sat[i_old].sat == sbssat->sat[i].sat)
+ {
+ sbssat->sat[i] = old_sat[i_old];
+ i++;
+ i_old++;
+ }
+ else if (old_sat[i_old].sat > sbssat->sat[i].sat)
+ i++;
+ else
+ i_old++;
+ }
+ }
sbssat->iodp=getbitu(msg->msg,224,2);
sbssat->nsat=n;
trace(5,"decode_sbstype1: nprn=%d iodp=%d\n",n,sbssat->iodp);
- printf("\tdecode_sbstype1: %d sats, iodp=%d\n",n,sbssat->iodp);
+ //printf("\tdecode_sbstype1: %d sats, iodp=%d\n",n,sbssat->iodp);
return 1;
}
/* decode type 2-5,0: fast corrections ---------------------------------------*/
@@ -200,11 +238,14 @@ static int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
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("\tdecode_sbstype2: type=%d iodf=%d\n",type,iodf);
+ //printf("\tdecode_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;
@@ -245,7 +286,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("\tdecode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]);
+ //printf("\tdecode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]);
return 1;
}
/* decode type 7: fast correction degradation factor -------------------------*/
@@ -255,8 +296,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++) {
@@ -346,7 +389,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("\tdecode_longcorr0: sat=%d iode=%d\n",sbssat->sat[n-1].sat,sbssat->sat[n-1].lcorr.iode);
+ printf("\tdecode_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;
@@ -369,7 +412,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("\tdecode_longcorr1: sat=%d iode=%d\n",sbssat->sat[n-1].sat,sbssat->sat[n-1].lcorr.iode);
+ printf("\tdecode_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;
@@ -408,7 +451,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);
@@ -421,7 +467,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("\tdecode_sbstype24: sat=%2d fcorr.iodf=%2d\n",sbssat->sat[j].sat,iodf);
+ //printf("\tdecode_sbstype24: sat=%2d fcorr.iodf=%2d\n",sbssat->sat[j].sat,iodf);
}
return decode_longcorrh(msg,120,sbssat);
}
@@ -476,6 +522,9 @@ extern int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
if (msg->week==0) return -1;
+ printf("============== sbsupdatecorr: PROCESSING type=%d...\n",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;
@@ -494,8 +543,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);
-
+ printf("============== sbsupdatecorr: type=%d PROCESSED result = %d!\n",type, stat?type:-1);
+ print_sbassat(&nav->sbssat);
return stat?type:-1;
}
/* read sbas log file --------------------------------------------------------*/
@@ -838,6 +887,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;
@@ -852,6 +902,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 -----------------------------------------------------------*/
@@ -869,7 +920,7 @@ static int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
t=timediff(time,p->fcorr.t0)+sbssat->tlat;
/* expire age of correction or UDRE==14 (not monitored) */
- if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue;
+ if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=14/*JV 15*/) continue;
*prc=p->fcorr.prc;
#ifdef RRCENA
if (p->fcorr.ai>0&&fabs(t)<=8.0*p->fcorr.dt) {
@@ -883,6 +934,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 -------------------------------
--
GitLab