From 3e0b4d468e0823095e1f0a94d79aada7fe92f8ba Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 7 Jul 2020 16:52:24 +0200
Subject: [PATCH] WIP
---
src/ephemeris.c | 21 ++++++++++++++++-----
src/pntpos.c | 19 ++++++++++++++++---
src/rcv/ublox.c | 17 +++++++++++++----
src/rtkcmn.c | 32 +++++++++++++++++++++++++++-----
src/rtklib.h | 3 +++
src/rtksvr.c | 14 +++-----------
src/sbas.c | 16 +++++++++++-----
7 files changed, 89 insertions(+), 33 deletions(-)
diff --git a/src/ephemeris.c b/src/ephemeris.c
index 75a6d2a..685dce0 100644
--- a/src/ephemeris.c
+++ b/src/ephemeris.c
@@ -427,6 +427,8 @@ static eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
if (iode>=0||j<0) {
trace(3,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0),
sat,iode);
+ printf("seleph: no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0),
+ sat,iode);
return NULL;
}
return nav->eph+j;
@@ -552,8 +554,9 @@ 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);
+ printf("satpos_sbas: sat=%2d\n",sat);
/* search sbas satellite correciton */
for (i=0;i<nav->sbssat.nsat;i++) {
@@ -562,15 +565,22 @@ static int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
}
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
+ printf("satpos_sbas: no sbas correction (i>=nav->sbssat.nsat) for sat=%2d\n",sat);
+ if (ephpos(time,teph,sat,nav,-1,rs,dts,var,svh))
+ *svh=-2;// JV: error code for detecting available ephemeris without sbas correction
+ else
+ 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;
+ if (!ephpos(time,teph,sat,nav,sbs->lcorr.iode,rs,dts,var,svh)){
+ printf("satpos_sbas: no broadcast eph for sat %2d at sbs->lcorr.iode %2d\n",sat,sbs->lcorr.iode);
+ return 0;
+ }
/* sbas satellite correction (long term and fast) */
if (sbssatcorr(time,sat,nav,rs,dts,var)) return 1;
+ printf("satpos_sbas: no sbas correction (!sbssatcorr()) for sat=%2d\n",sat);
*svh=-1;
return 0;
}
@@ -739,8 +749,9 @@ 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);
+ printf("====satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt);
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/pntpos.c b/src/pntpos.c
index dffafce..db7843f 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 */
@@ -462,7 +475,7 @@ static int raim_fde(const obsd_t *obs, int n, const double *rs,
}
/* estimate receiver position without a satellite */
if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,ssat,&sol_e,azel_e,
- vsat_e,resp_e,msg_e,1)) {
+ vsat_e,resp_e,msg_e,0)) {
trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);
continue;
}
@@ -637,7 +650,7 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg,n>=6&&opt->posopt[4]);// check chi-square if afterwards RAIM can be called
/* raim fde */
- if (!stat&&n>=6&&opt->posopt[4]) {
+ if (/*!stat&&*/n>=6&&opt->posopt[4]) {
stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg);
}
diff --git a/src/rcv/ublox.c b/src/rcv/ublox.c
index 1fe6734..b524fb9 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;
@@ -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("decode_ephem eph.iode = %d\n", eph.iode);
return 2;
}
/* decode almanac and ion/utc ------------------------------------------------*/
@@ -1015,6 +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("decode_enav eph.iode = %d\n", eph.iode);
return 2;
}
/* decode beidou navigation data ---------------------------------------------*/
@@ -1128,6 +1136,7 @@ static int decode_gnav(raw_t *raw, int sat, int off, int frq)
}
raw->nav.geph[prn-1]=geph;
raw->ephsat=sat;
+ printf("decode_gnav geph.iode = %d\n", geph.iode);
return 2;
}
/* decode sbas navigation data -----------------------------------------------*/
diff --git a/src/rtkcmn.c b/src/rtkcmn.c
index 9d1ab96..e47627f 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("excluding sat: svh < 0 (eph unavailable)\n");
+ return 1; /* ephemeris unavailable */
+ }
if (opt) {
- if (opt->exsats[sat-1]==1) return 1; /* excluded satellite */
+ if (opt->exsats[sat-1]==1)
+ {
+ //printf("excluding sat: in exsats\n");
+ 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("excluding sat: constellation not used\n");
+ return 1; /* unselected sat sys */
+ }
}
if (sys==SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */
if (svh) {
+ //printf("excluding sat: unhealthy satellite\n");
trace(3,"unhealthy satellite: sat=%3d svh=%02X\n",sat,svh);
return 1;
}
if (var>MAX_VAR_EPH) {
+ //printf("excluding sat: invalid ura satellite\n");
trace(3,"invalid ura satellite: sat=%3d ura=%.2f\n",sat,sqrt(var));
return 1;
}
@@ -2531,7 +2545,7 @@ static int cmpeph(const void *p1, const void *p2)
q1->sat-q2->sat);
}
/* sort and unique ephemeris -------------------------------------------------*/
-static void uniqeph(nav_t *nav)
+extern void uniqeph(nav_t *nav)
{
eph_t *nav_eph;
int i,j;
@@ -2569,7 +2583,7 @@ static int cmpgeph(const void *p1, const void *p2)
q1->sat-q2->sat);
}
/* sort and unique glonass ephemeris -----------------------------------------*/
-static void uniqgeph(nav_t *nav)
+extern void uniqgeph(nav_t *nav)
{
geph_t *nav_geph;
int i,j;
@@ -2688,6 +2702,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 c4927f8..d7bf3a1 100644
--- a/src/rtklib.h
+++ b/src/rtklib.h
@@ -1544,6 +1544,9 @@ EXPORT double dms2deg(const double *dms);
/* input and output functions ------------------------------------------------*/
EXPORT void readpos(const char *file, const char *rcv, double *pos);
EXPORT int sortobs(obs_t *obs);
+EXPORT void updatenav(nav_t *nav);
+EXPORT void uniqeph(nav_t *nav);
+EXPORT void uniqgeph(nav_t *nav);
EXPORT void uniqnav(nav_t *nav);
EXPORT int screent(gtime_t time, gtime_t ts, gtime_t te, double tint);
EXPORT int readnav(const char *file, nav_t *nav);
diff --git a/src/rtksvr.c b/src/rtksvr.c
index a82da2c..c2954ac 100644
--- a/src/rtksvr.c
+++ b/src/rtksvr.c
@@ -108,28 +108,20 @@ static void writesol(rtksvr_t *svr, int index)
rtksvrunlock(svr);
}
}
-/* update navigation data ----------------------------------------------------*/
-static 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);
- }
-}
/* update glonass frequency channel number in raw data struct ----------------*/
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 6920ffb..5c15010 100644
--- a/src/sbas.c
+++ b/src/sbas.c
@@ -145,8 +145,9 @@ static int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
}
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: nprn=%d iodp=%d\n",n,sbssat->iodp);
return 1;
}
/* decode type 2-5,0: fast corrections ---------------------------------------*/
@@ -157,7 +158,7 @@ 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;
type=getbitu(msg->msg, 8,6);
@@ -183,6 +184,7 @@ static int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
sbssat->sat[j].fcorr.iodf=iodf;
}
trace(5,"decode_sbstype2: type=%d iodf=%d\n",type,iodf);
+ printf("decode_sbstype2: type=%d iodf=%d\n",type,iodf);
return 1;
}
/* decode type 6: integrity info ---------------------------------------------*/
@@ -201,6 +203,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 -------------------------*/
@@ -309,7 +312,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;
}
@@ -334,7 +337,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;
}
@@ -374,6 +377,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,8 +427,8 @@ 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;
@@ -446,6 +450,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);
+
return stat?type:-1;
}
/* read sbas log file --------------------------------------------------------*/
--
GitLab