mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-30 14:53:03 +00:00
Code Issues Releases Wiki Activity

Avoid crashing when PPP is invoked

Browse Source
This commit is contained in:
Carles Fernandez
2017-05-04 09:19:57 +02:00
parent 48bd883460
commit 0a4f6cf499
11 changed files with 70 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/algorithms/libs/rtklib/rtklib.h
View File

@@ -298,6 +298,26 @@ const int ARMODE_TCAR = 5; //!< AR mode: triple carrier ar
const int POSOPT_RINEX = 3; //!< pos option: rinex header pos */
/* number and index of states for PPP */
#define NF_PPP(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf)
#define NP_PPP(opt) ((opt)->dynamics?9:3)
#define NC_PPP(opt) (NSYS)
#define NT_PPP(opt) ((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt==TROPOPT_EST?1:3))
#define NI_PPP(opt) ((opt)->ionoopt==IONOOPT_EST?MAXSAT:0)
#define ND_PPP(opt) ((opt)->nf>=3?1:0)
#define NR_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+NI_PPP(opt)+ND_PPP(opt))
#define NB_PPP(opt) (NF_PPP(opt)*MAXSAT)
#define NX_PPP(opt) (NR_PPP(opt)+NB_PPP(opt))
#define IC_PPP(s,opt) (NP_PPP(opt)+(s))
#define IT_PPP(opt) (NP_PPP(opt)+NC_PPP(opt))
#define II_PPP(s,opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+(s)-1)
#define ID_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+NI_PPP(opt))
#define IB_PPP(s,f,opt) (NR_PPP(opt)+MAXSAT*(f)+(s)-1)
typedef void fatalfunc_t(const char *); //!< fatal callback function type

4
src/algorithms/libs/rtklib/rtklib_ephemeris.cc
View File

@@ -692,10 +692,10 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
{
dclk += ssr->hrclk;
}
if (norm(deph, 3) > MAXECORSSR || fabs(dclk) > MAXCCORSSR)
if (norm_rtk(deph, 3) > MAXECORSSR || fabs(dclk) > MAXCCORSSR)
{
trace(3, "invalid ssr correction: %s deph=%.1f dclk=%.1f\n",
time_str(time, 0), norm(deph, 3), dclk);
time_str(time, 0), norm_rtk(deph, 3), dclk);
*svh = -1;
return 0;
}

8
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@@ -198,7 +198,7 @@ int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
return iontec(time, nav, pos, azel, 1, ion, var);
}
/* qzss broadcast model */
if (ionoopt == IONOOPT_QZS && norm(nav->ion_qzs, 8)>0.0)
if (ionoopt == IONOOPT_QZS && norm_rtk(nav->ion_qzs, 8)>0.0)
{
*ion = ionmodel(time, nav->ion_qzs, pos, azel);
*var = std::pow(*ion * ERR_BRDCI, 2.0);
@@ -436,7 +436,7 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
}
for (j = 0; j < NX; j++) x[j] += dx[j];
if (norm(dx, NX) < 1e-4)
if (norm_rtk(dx, NX) < 1e-4)
{
sol->type = 0;
sol->time = timeadd(obs[0].time, -x[3] / SPEED_OF_LIGHT);
@@ -584,7 +584,7 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
{
lam = nav->lam[obs[i].sat-1][0];
if (obs[i].D[0] == 0.0 || lam == 0.0 || !vsat[i] || norm(rs + 3 + i * 6, 3) <= 0.0)
if (obs[i].D[0] == 0.0 || lam == 0.0 || !vsat[i] || norm_rtk(rs + 3 + i * 6, 3) <= 0.0)
{
continue;
}
@@ -639,7 +639,7 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
for (j = 0; j < 4; j++) x[j] += dx[j];
if (norm(dx, 4) < 1e-6)
if (norm_rtk(dx, 4) < 1e-6)
{
for (i = 0; i < 3; i++) sol->rr[i+3] = x[i];
break;

8
src/algorithms/libs/rtklib/rtklib_ppp.cc
View File

@@ -220,7 +220,7 @@ void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs)
{
type = nav->pcvs[obs[i].sat-1].type;
if ((r = norm(rs+i*6, 3)) <= 0.0) continue;
if ((r = norm_rtk(rs+i*6, 3)) <= 0.0) continue;
/* only block IIA */
if (*type && !strstr(type, "BLOCK IIA")) continue;
@@ -329,7 +329,7 @@ int model_phw(gtime_t time, int sat, const char *type, int opt,
ds[i] = exs[i]-ek[i]*dot(ek, exs, 3)-eks[i];
dr[i] = exr[i]-ek[i]*dot(ek, exr, 3)+ekr[i];
}
cosp = dot(ds, dr, 3)/norm(ds, 3)/norm(dr, 3);
cosp = dot(ds, dr, 3)/norm_rtk(ds, 3)/norm_rtk(dr, 3);
if (cosp < -1.0) cosp = -1.0;
else if (cosp> 1.0) cosp = 1.0;
ph = acos(cosp)/2.0/PI;
@@ -539,7 +539,7 @@ void udpos_ppp(rtk_t *rtk)
return;
}
/* initialize position for first epoch */
if (norm(rtk->x, 3) <= 0.0)
if (norm_rtk(rtk->x, 3) <= 0.0)
{
for (i = 0;i < 3;i++) initx(rtk, rtk->sol.rr[i], VAR_POS_PPP, i);
}
@@ -1329,7 +1329,7 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
matcpy(rtk->Pa, Pp, rtk->nx, rtk->nx);
for (i = 0;i < 3;i++) std[i] = sqrt(Pp[i+i*rtk->nx]);
if (norm(std, 3) < MAX_STD_FIX) stat = SOLQ_FIX;
if (norm_rtk(std, 3) < MAX_STD_FIX) stat = SOLQ_FIX;
}
else
{

16
src/algorithms/libs/rtklib/rtklib_ppp.h
View File

@@ -84,21 +84,7 @@ const double EPS0_GLO = (14.2*D2R); /* max shadow crossing angle GLO (rad)
const double T_POSTSHADOW = 1800.0; /* post-shadow recovery time (s) */
const double QZS_EC_BETA = 20.0; /* max beta angle for qzss Ec (deg) */
/* number and index of states */
#define NF_PPP(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf)
#define NP_PPP(opt) ((opt)->dynamics?9:3)
#define NC_PPP(opt) (NSYS)
#define NT_PPP(opt) ((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt==TROPOPT_EST?1:3))
#define NI_PPP(opt) ((opt)->ionoopt==IONOOPT_EST?MAXSAT:0)
#define ND_PPP(opt) ((opt)->nf>=3?1:0)
#define NR_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+NI_PPP(opt)+ND_PPP(opt))
#define NB_PPP(opt) (NF_PPP(opt)*MAXSAT)
#define NX_PPP(opt) (NR_PPP(opt)+NB_PPP(opt))
#define IC_PPP(s,opt) (NP_PPP(opt)+(s))
#define IT_PPP(opt) (NP_PPP(opt)+NC_PPP(opt))
#define II_PPP(s,opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+(s)-1)
#define ID_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+NI_PPP(opt))
#define IB_PPP(s,f,opt) (NR_PPP(opt)+MAXSAT*(f)+(s)-1)
int pppcorr_read(pppcorr_t *corr, const char *file);

6
src/algorithms/libs/rtklib/rtklib_preceph.cc
View File

@@ -268,7 +268,7 @@ void combpeph(nav_t *nav, int opt)
{
for (k = 0; k < MAXSAT; k++)
{
if (norm(nav->peph[j].pos[k], 4) <= 0.0) continue;
if (norm_rtk(nav->peph[j].pos[k], 4) <= 0.0) continue;
for (m = 0;m < 4; m++) nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m];
for (m = 0;m < 4; m++) nav->peph[i].std[k][m] = nav->peph[j].std[k][m];
for (m = 0;m < 4; m++) nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m];
@@ -633,7 +633,7 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
for (j = 0; j <= NMAX; j++)
{
t[j] = timediff(nav->peph[i+j].time, time);
if (norm(nav->peph[i+j].pos[sat-1], 3) <= 0.0)
if (norm_rtk(nav->peph[i+j].pos[sat-1], 3) <= 0.0)
{
trace(3, "prec ephem outage %s sat=%2d\n", time_str(time, 0), sat);
return 0;
@@ -661,7 +661,7 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
if (vare)
{
for (i = 0; i < 3; i++) s[i] = nav->peph[index].std[sat-1][i];
std = norm(s, 3);
std = norm_rtk(s, 3);
/* extrapolation error for orbit */
if (t[0 ] > 0.0) std += EXTERR_EPH * std::pow(t[0 ], 2.0) / 2.0;

13
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

@@ -54,6 +54,7 @@
//#include <stdarg.h>
//#include <ctype.h>
#include <dirent.h>
#include <iostream>
//#include <time.h>
#include <sys/time.h>
#include <sys/stat.h>
@@ -842,9 +843,9 @@ double dot(const double *a, const double *b, int n)
* int n I size of vector a
* return : || a ||
*-----------------------------------------------------------------------------*/
double norm(const double *a, int n)
double norm_rtk(const double *a, int n)
{
return sqrt(dot(a, a, n));
return std::sqrt(dot(a, a, n));
}
@@ -871,7 +872,7 @@ void cross3(const double *a, const double *b, double *c)
int normv3(const double *a, double *b)
{
double r;
if ((r = norm(a, 3)) <= 0.0) return 0;
if ((r = norm_rtk(a, 3)) <= 0.0) return 0;
b[0] = a[0]/r;
b[1] = a[1]/r;
b[2] = a[2]/r;
@@ -3378,9 +3379,9 @@ double geodist(const double *rs, const double *rr, double *e)
double r;
int i;
if (norm(rs, 3)<RE_WGS84) return -1.0;
if (norm_rtk(rs, 3)<RE_WGS84) return -1.0;
for (i = 0; i<3; i++) e[i] = rs[i]-rr[i];
r = norm(e, 3);
r = norm_rtk(e, 3);
for (i = 0; i<3; i++) e[i]/=r;
return r+DEFAULT_OMEGA_EARTH_DOT*(rs[0]*rr[1]-rs[1]*rr[0])/SPEED_OF_LIGHT;
}
@@ -3467,7 +3468,7 @@ double ionmodel(gtime_t t, const double *ion, const double *pos,
int week;
if (pos[2]<-1e3 || azel[1] <= 0) return 0.0;
if (norm(ion, 8) <= 0.0) ion = ion_default;
if (norm_rtk(ion, 8) <= 0.0) ion = ion_default;
/* earth centered angle (semi-circle) */
psi = 0.0137/(azel[1]/PI+0.11)-0.022;

2
src/algorithms/libs/rtklib/rtklib_rtkcmn.h
View File

@@ -124,7 +124,7 @@ int *imat(int n, int m);
double *zeros(int n, int m);
double *eye(int n);
double dot(const double *a, const double *b, int n);
double norm(const double *a, int n);
double norm_rtk(const double *a, int n);
void cross3(const double *a, const double *b, double *c);
int normv3(const double *a, double *b);
void matcpy(double *A, const double *B, int n, int m);

8
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
View File

@@ -430,7 +430,7 @@ double baseline(const double *ru, const double *rb, double *dr)
{
int i;
for (i = 0;i<3;i++) dr[i] = ru[i]-rb[i];
return norm(dr, 3);
return norm_rtk(dr, 3);
}
@@ -482,7 +482,7 @@ void udpos(rtk_t *rtk, double tt)
return;
}
/* initialize position for first epoch */
if (norm(rtk->x, 3) <= 0.0)
if (norm_rtk(rtk->x, 3) <= 0.0)
{
for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i);
if (rtk->opt.dynamics)
@@ -990,7 +990,7 @@ int zdres(int base, const obsd_t *obs, int n, const double *rs,
for (i = 0;i<n*nf*2;i++) y[i] = 0.0;
if (norm(rr, 3) <= 0.0) return 0; /* no receiver position */
if (norm_rtk(rr, 3) <= 0.0) return 0; /* no receiver position */
for (i = 0;i<3;i++) rr_[i] = rr[i];
@@ -1089,7 +1089,7 @@ int constbl(rtk_t *rtk, const double *x, const double *P, double *v,
xb[i] = rtk->rb[i]+rtk->rb[i+3]*rtk->sol.age;
b[i] = x[i]-xb[i];
}
bb = norm(b, 3);
bb = norm_rtk(b, 3);
/* approximate variance of solution */
if (P)

6
src/algorithms/libs/rtklib/rtklib_tides.cc
View File

@@ -66,7 +66,7 @@ void tide_pl(const double *eu, const double *rp, double GMp,
trace(4,"tide_pl : pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
if ((r=norm(rp,3))<=0.0) return;
if ((r=norm_rtk(rp,3))<=0.0) return;
for (i=0;i<3;i++) ep[i]=rp[i]/r;
@@ -279,9 +279,9 @@ void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
dr[0]=dr[1]=dr[2]=0.0;
if (norm(rr,3)<=0.0) return;
if (norm_rtk(rr,3)<=0.0) return;
pos[0]=asin(rr[2]/norm(rr,3));
pos[0]=asin(rr[2]/norm_rtk(rr,3));
pos[1]=atan2(rr[1],rr[0]);
xyz2enu(pos,E);