mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-13 19:50:34 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -283,8 +283,6 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
If the baseline length is very short like 1 m, the iteration may be effective to handle
the nonlinearity of measurement equation. */
/// Statistics
double bias_0 = configuration->property(role + ".bias_0", 30.0);
@ -307,12 +305,16 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
double sigma_pos = configuration->property(role + ".sigma_pos", 0.0);
snrmask_t snrmask = { {}, {{},{}} };
sbssat_t sbssat;
sbsion_t sbsion[MAXBAND+1] = {};
prcopt_t rtklib_configuration_options = {positioning_mode, /* positioning mode (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */
0, /* solution type (0:forward,1:backward,2:combined) */
number_of_frequencies, /* number of frequencies (1:L1, 2:L1+L2, 3:L1+L2+L5)*/
navigation_system, /* navigation system */
elevation_mask * D2R, /* elevation mask angle (degrees) */
{ {}, {{},{}} }, /* snrmask_t snrmask SNR mask */
snrmask, /* snrmask_t snrmask SNR mask */
0, /* satellite ephemeris/clock (EPHOPT_XXX) */
integer_ambiguity_resolution_gps, /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */
integer_ambiguity_resolution_glo, /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */
@ -355,7 +357,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
{}, /* unsigned char exsats[MAXSAT] excluded satellites (1:excluded, 2:included) */
0, /* max averaging epoches */
0, /* initialize by restart */
0, /* output single by dgps/float/fix/ppp outage */
1, /* output single by dgps/float/fix/ppp outage */
{"",""}, /* char rnxopt[2][256] rinex options {rover,base} */
{sat_PCV,rec_PCV,phwindup,reject_GPS_IIA,raim_fde}, /* posopt[6] positioning options [0]: satellite and receiver antenna PCV model; [1]: interpolate antenna parameters; [2]: apply phase wind-up correction for PPP modes; [3]: exclude measurements of GPS Block IIA satellites satellite [4]: RAIM FDE (fault detection and exclusion) [5]: handle day-boundary clock jump */
0, /* solution sync mode (0:off,1:on) */
@ -391,19 +393,27 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
{{},{}} /* previous carrier-phase observable (cycle) */
};
int nx = 0; /* Number of estimated states */
if(positioning_mode <= PMODE_FIXED) nx = 4 + 3;
if(positioning_mode >= PMODE_PPP_KINEMA) nx = NX_PPP(&rtklib_configuration_options);
int na = NR_PPP(&rtklib_configuration_options);
double x[nx];
double Px[nx];
double xa[na];
double Pa[na];
rtk = { sol_, /* RTK solution */
{}, /* base position/velocity (ecef) (m|m/s) */
0, /* number of float states */
0, /* number of fixed states */
nx, /* number of float states */
na, /* number of fixed states */
output_rate_ms / 1000.0, /* time difference between current and previous (s) */
{}, /* float states */
{}, /* float states covariance */
{}, /* fixed states */
{}, /* fixed states covariance */
x, /* float states */
Px, /* float states covariance */
xa, /* fixed states */
Pa, /* fixed states covariance */
3, /* number of continuous fixes of ambiguity */
{ambc_}, /* ambiguity control */
{ssat_}, /* satellite status */
0, /* bytes in error message buffer */
128, /* bytes in error message buffer */
{'0'}, /* error message buffer */
rtklib_configuration_options /* processing options */
};

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);