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

Get PPP working

Browse Source
This commit is contained in:
Carles Fernandez 2017-05-06 10:01:54 +02:00
parent 459abd29a7
commit 4f6ba053ef
9 changed files with 1639 additions and 1397 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -394,7 +394,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
int nx = 0; /* Number of estimated states */ int nx = 0; /* Number of estimated states */
if(positioning_mode <= PMODE_FIXED) nx = 4 + 3; if(positioning_mode <= PMODE_FIXED) nx = 4 + 3;
if(positioning_mode >= PMODE_PPP_KINEMA) nx = NX_PPP(&rtklib_configuration_options); if(positioning_mode >= PMODE_PPP_KINEMA) nx = NX_PPP(&rtklib_configuration_options);
int na = NR_PPP(&rtklib_configuration_options); int na = NP_PPP(&rtklib_configuration_options);std::cout << "NP_PPP: " << na << std::endl;
double x[nx]; double x[nx];
double Px[nx*nx]; double Px[nx*nx];
double xa[na]; double xa[na];
@ -411,7 +411,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
3, /* number of continuous fixes of ambiguity */ 3, /* number of continuous fixes of ambiguity */
{ambc_}, /* ambiguity control */ {ambc_}, /* ambiguity control */
{ssat_}, /* satellite status */ {ssat_}, /* satellite status */
128, /* bytes in error message buffer */ 256, /* bytes in error message buffer */
{'0'}, /* error message buffer */ {'0'}, /* error message buffer */
rtklib_configuration_options /* processing options */ rtklib_configuration_options /* processing options */
}; };

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

@ -156,10 +156,17 @@ const int NSATGLO = 0;
const int NSYSGLO = 0; const int NSYSGLO = 0;
#endif #endif
#ifdef ENAGAL
const int MINPRNGAL = 1; //!< min satellite PRN number of Galileo const int MINPRNGAL = 1; //!< min satellite PRN number of Galileo
const int MAXPRNGAL = 30; //!< max satellite PRN number of Galileo const int MAXPRNGAL = 30; //!< max satellite PRN number of Galileo
const int NSATGAL = (MAXPRNGAL - MINPRNGAL + 1); //!< number of Galileo satellites const int NSATGAL = (MAXPRNGAL - MINPRNGAL + 1); //!< number of Galileo satellites
const int NSYSGAL = 1; const int NSYSGAL = 1;
#else
const int MINPRNGAL = 0;
const int MAXPRNGAL = 0;
const int NSATGAL = 0;
const int NSYSGAL = 0;
#endif
#ifdef ENAQZS #ifdef ENAQZS
const int MINPRNQZS = 193; //!< min satellite PRN number of QZSS const int MINPRNQZS = 193; //!< min satellite PRN number of QZSS
@ -261,7 +268,9 @@ const int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model
const int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model const int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model
const int TROPOPT_EST = 3; //!< troposphere option: ZTD estimation const int TROPOPT_EST = 3; //!< troposphere option: ZTD estimation
const int TROPOPT_ESTG = 4; //!< troposphere option: ZTD+grad estimation const int TROPOPT_ESTG = 4; //!< troposphere option: ZTD+grad estimation
const int TROPOPT_ZTD = 5; //!< troposphere option: ZTD correction const int TROPOPT_COR = 5; //!< troposphere option: ZTD correction
const int TROPOPT_CORG = 6; //!< troposphere option: ZTD+grad correction
const int EPHOPT_BRDC = 0; //!< ephemeris option: broadcast ephemeris const int EPHOPT_BRDC = 0; //!< ephemeris option: broadcast ephemeris
const int EPHOPT_PREC = 1; //!< ephemeris option: precise ephemeris const int EPHOPT_PREC = 1; //!< ephemeris option: precise ephemeris
@ -286,34 +295,26 @@ const int ARMODE_OFF = 0; //!< AR mode: off
const int ARMODE_CONT = 1; //!< AR mode: continuous const int ARMODE_CONT = 1; //!< AR mode: continuous
const int ARMODE_INST = 2; //!< AR mode: instantaneous const int ARMODE_INST = 2; //!< AR mode: instantaneous
const int ARMODE_FIXHOLD = 3; //!< AR mode: fix and hold const int ARMODE_FIXHOLD = 3; //!< AR mode: fix and hold
const int ARMODE_WLNL = 4; //!< AR mode: wide lane/narrow lane const int ARMODE_PPPAR = 4; //!< AR mode: PPP-AR
const int ARMODE_TCAR = 5; //!< AR mode: triple carrier ar const int ARMODE_PPPAR_ILS = 5; //!< AR mode: AR mode: PPP-AR ILS
const int ARMODE_WLNL = 6;
const int ARMODE_TCAR = 7;
const int POSOPT_RINEX = 3; //!< pos option: rinex header pos */ 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 typedef void fatalfunc_t(const char *); //!< fatal callback function type
#define NP_PPP(opt) ((opt)->dynamics?9:3) /* number of pos solution */
#define IC_PPP(s,opt) (NP_PPP(opt)+(s)) /* state index of clocks (s=0:gps,1:glo) */
#define IT_PPP(opt) (IC_PPP(0,opt)+NSYS) /* state index of tropos */
#define NR_PPP(opt) (IT_PPP(opt)+((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt==TROPOPT_EST?1:3)))
/* number of solutions */
#define IB_PPP(s,opt) (NR_PPP(opt)+(s)-1) /* state index of phase bias */
#define NX_PPP(opt) (IB_PPP(MAXSAT,opt)+1) /* number of estimated states */
typedef struct { /* time struct */ typedef struct { /* time struct */
time_t time; /* time (s) expressed by standard time_t */ time_t time; /* time (s) expressed by standard time_t */
double sec; /* fraction of second under 1 s */ double sec; /* fraction of second under 1 s */
@ -457,7 +458,7 @@ typedef struct { /* SBAS ephemeris type */
typedef struct { /* norad two line element data type */ typedef struct { /* norad two line element data type */
char name [32]; /* common name */ char name [32]; /* common name */
char alias[32]; /* alias name */ char alias[32]; /* alias name */
char satno[16]; /* satellite catalog number */ char satno[16]; /* satellilte catalog number */
char satclass; /* classification */ char satclass; /* classification */
char desig[16]; /* international designator */ char desig[16]; /* international designator */
gtime_t epoch; /* element set epoch (UTC) */ gtime_t epoch; /* element set epoch (UTC) */
@ -747,8 +748,8 @@ typedef struct { /* solution type */
unsigned char stat; /* solution status (SOLQ_???) */ unsigned char stat; /* solution status (SOLQ_???) */
unsigned char ns; /* number of valid satellites */ unsigned char ns; /* number of valid satellites */
float age; /* age of differential (s) */ float age; /* age of differential (s) */
float ratio; /* AR ratio factor for validation */ float ratio; /* AR ratio factor for valiation */
float thres; /* AR ratio threshold for validation */ float thres; /* AR ratio threshold for valiation */
} sol_t; } sol_t;
@ -976,7 +977,7 @@ typedef struct { /* RTK control/result type */
double *x, *P; /* float states and their covariance */ double *x, *P; /* float states and their covariance */
double *xa,*Pa; /* fixed states and their covariance */ double *xa,*Pa; /* fixed states and their covariance */
int nfix; /* number of continuous fixes of ambiguity */ int nfix; /* number of continuous fixes of ambiguity */
ambc_t ambc[MAXSAT]; /* ambibuity control */ ambc_t ambc[MAXSAT]; /* ambiguity control */
ssat_t ssat[MAXSAT]; /* satellite status */ ssat_t ssat[MAXSAT]; /* satellite status */
int neb; /* bytes in error message buffer */ int neb; /* bytes in error message buffer */
char errbuf[MAXERRMSG]; /* error message buffer */ char errbuf[MAXERRMSG]; /* error message buffer */

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

File diff suppressed because it is too large Load Diff

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

@ -56,130 +56,159 @@
#include "rtklib.h" #include "rtklib.h"
#define MAX_PPP(x,y) ((x)>(y)?(x):(y))
#define ROUND_PPP(x) (int)floor((x)+0.5)
const int MAX_ITER = 8; /* max number of iterations */ #define SQR(x) ((x)*(x))
const double MAX_STD_FIX = 0.15; /* max std-dev (3d) to fix solution */ #define MIN(x,y) ((x)<=(y)?(x):(y))
const int MIN_NSAT_SOL = 4; /* min satellite number for solution */ #define ROUND(x) (int)floor((x)+0.5)
const double THRES_REJECT = 4.0; /* reject threshold of posfit-res (sigma) */
const double THRES_MW_JUMP = 10.0; #define SWAP_I(x,y) do {int _z=x; x=y; y=_z;} while (0)
#define SWAP_D(x,y) do {double _z=x; x=y; y=_z;} while (0)
const double VAR_POS_PPP = std::pow(60.0, 2.0); /* init variance receiver position (m^2) */ #define MIN_ARC_GAP 300.0 /* min arc gap (s) */
const double VAR_CLK = std::pow(60.0, 2.0); /* init variance receiver clock (m^2) */ #define CONST_AMB 0.001 /* constraint to fixed ambiguity */
const double VAR_ZTD = std::pow( 0.6, 2.0); /* init variance ztd (m^2) */ #define THRES_RES 0.3 /* threashold of residuals test (m) */
const double VAR_GRA_PPP = std::pow(0.01, 2.0); /* init variance gradient (m^2) */ #define LOG_PI 1.14472988584940017 /* log(pi) */
const double VAR_DCB = std::pow(30.0, 2.0); /* init variance dcb (m^2) */ #define SQRT2 1.41421356237309510 /* sqrt(2) */
const double VAR_BIAS = std::pow(60.0, 2.0); /* init variance phase-bias (m^2) */
const double VAR_IONO = std::pow(60.0, 2.0); /* init variance iono-delay */
const double VAR_GLO_IFB = std::pow( 0.6, 2.0); /* variance of glonass ifb */
const double EFACT_GPS_L5 = 10.0; /* error factor of GPS/QZS L5 */ #define AS2R (D2R/3600.0) /* arc sec to radian */
//#define GME 3.986004415E+14 /* earth gravitational constant */
//const double GMS = 1.327124E+20; /* sun gravitational constant */
//#define GMM 4.902801E+12 /* moon gravitational constant */
const double MUDOT_GPS = (0.00836*D2R); /* average angular velocity GPS (rad/s) */ /* initial variances */
const double MUDOT_GLO = (0.00888*D2R); /* average angular velocity GLO (rad/s) */ #define VAR_POS SQR(100.0) /* receiver position (m^2) */
const double EPS0_GPS = (13.5*D2R); /* max shadow crossing angle GPS (rad) */ #define VAR_CLK SQR(100.0) /* receiver clock (m^2) */
const double EPS0_GLO = (14.2*D2R); /* max shadow crossing angle GLO (rad) */ #define VAR_ZTD SQR( 0.3) /* ztd (m^2) */
const double T_POSTSHADOW = 1800.0; /* post-shadow recovery time (s) */ #define VAR_GRA SQR(0.001) /* gradient (m^2) */
const double QZS_EC_BETA = 20.0; /* max beta angle for qzss Ec (deg) */ #define VAR_BIAS SQR(100.0) /* phase-bias (m^2) */
#define VAR_IONO_OFF SQR(10.0) /* variance of iono-model-off */
#define ERR_SAAS 0.3 /* saastamoinen model error std (m) */
#define ERR_BRDCI 0.5 /* broadcast iono model error factor */
#define ERR_CBIAS 0.3 /* code bias error std (m) */
#define REL_HUMI 0.7 /* relative humidity for saastamoinen model */
#define NP(opt) ((opt)->dynamics?9:3) /* number of pos solution */
#define IC(s,opt) (NP(opt)+(s)) /* state index of clocks (s=0:gps,1:glo) */
#define IT(opt) (IC(0,opt)+NSYS) /* state index of tropos */
#define NR(opt) (IT(opt)+((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt==TROPOPT_EST?1:3)))
/* number of solutions */
#define IB(s,opt) (NR(opt)+(s)-1) /* state index of phase bias */
#define NX(opt) (IB(MAXSAT,opt)+1) /* number of estimated states */
double lam_LC(int i, int j, int k);
double L_LC(int i, int j, int k, const double *L);
double P_LC(int i, int j, int k, const double *P);
double var_LC(int i, int j, int k, double sig);
double q_gamma(double a, double x, double log_gamma_a);
double p_gamma(double a, double x, double log_gamma_a);
double f_erfc(double x);
double conffunc(int N, double B, double sig);
void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
const double *azel);
int fix_amb_WL(rtk_t *rtk, const nav_t *nav, int sat1, int sat2, int *NW);
int is_depend(int sat1, int sat2, int *flgs, int *max_flg);
int sel_amb(int *sat1, int *sat2, double *N, double *var, int n);
int fix_sol(rtk_t *rtk, const int *sat1, const int *sat2,
const double *NC, int n);
int fix_amb_ROUND(rtk_t *rtk, int *sat1, int *sat2, const int *NW, int n);
int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n);
int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
const double *azel);
int pppcorr_read(pppcorr_t *corr, const char *file);
void pppcorr_free(pppcorr_t *corr);
int pppcorr_trop(const pppcorr_t *corr, gtime_t time, const double *pos,
double *trp, double *std);
int pppcorr_stec(const pppcorr_t *corr, gtime_t time, const double *pos,
double *ion, double *std);
int ppp_ar(rtk_t *rtk, const obsd_t *obs, int n, int *exc,
const nav_t *nav, const double *azel, double *x, double *P);
double STD(rtk_t *rtk, int i); void pppoutsolstat(rtk_t *rtk, int level, FILE *fp);
int pppoutstat(rtk_t *rtk, char *buff);
void tide_pl(const double *eu, const double *rp, double GMp,
const double *pos, double *dr);
void tide_solid(const double *rsun, const double *rmoon,
const double *pos, const double *E, double gmst, int opt,
double *dr);
void tide_oload(gtime_t tut, const double *odisp, double *denu);
void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar);
void tide_pole(gtime_t tut, const double *pos, const double *erpv,
double *denu);
void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp,
const double *odisp, double *dr);
void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs); void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs);
double yaw_nominal(double beta, double mu);
double yaw_angle(int sat, const char *type, int opt, double beta, double mu,
double *yaw);
int sat_yaw(gtime_t time, int sat, const char *type, int opt,
const double *rs, double *exs, double *eys);
double varerr(int sat, int sys, double el, int type, const prcopt_t *opt);
int model_phw(gtime_t time, int sat, const char *type, int opt,
const double *rs, const double *rr, double *phw);
double varerr(int sat, int sys, double el, int freq, int type,
const prcopt_t *opt);
void initx(rtk_t *rtk, double xi, double var, int i); void initx(rtk_t *rtk, double xi, double var, int i);
int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel,
const prcopt_t *opt, const double *dantr, const double *dants,
double phw, double *meas, double *var);
double gettgd_ppp(int sat, const nav_t *nav);
int corr_ion(gtime_t time, const nav_t *nav, int sat, const double *pos,
const double *azel, int ionoopt, double *ion, double *var,
int *brk);
int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos,
const double *azel, const prcopt_t *opt,
const double *dantr, const double *dants, double phw,
double *meas, double *var, int *brk);
double gfmeas(const obsd_t *obs, const nav_t *nav); double gfmeas(const obsd_t *obs, const nav_t *nav);
double mwmeas(const obsd_t *obs, const nav_t *nav);
void corr_meas(const obsd_t *obs, const nav_t *nav, const double *azel,
const prcopt_t *opt, const double *dantr,
const double *dants, double phw, double *L, double *P,
double *Lc, double *Pc);
void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n);
void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
void detslp_mw(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
void udpos_ppp(rtk_t *rtk); void udpos_ppp(rtk_t *rtk);
void udclk_ppp(rtk_t *rtk); void udclk_ppp(rtk_t *rtk);
void udtrop_ppp(rtk_t *rtk); void udtrop_ppp(rtk_t *rtk);
void udiono_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n);
void uddcb_ppp(rtk_t *rtk); void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, void satantpcv(const double *rs, const double *rr, const pcv_t *pcv,
double *dant); double *dant);
double trop_model_prec(gtime_t time, const double *pos, double prectrop(gtime_t time, const double *pos, const double *azel,
const double *azel, const double *x, double *dtdx, const prcopt_t *opt, const double *x, double *dtdx,
double *var); double *var);
int model_trop(gtime_t time, const double *pos, const double *azel, int res_ppp(int iter, const obsd_t *obs, int n, const double *rs,
const prcopt_t *opt, const double *x, double *dtdx, const double *dts, const double *vare, const int *svh,
const nav_t *nav, double *dtrp, double *var); const nav_t *nav, const double *x, rtk_t *rtk, double *v,
double *H, double *R, double *azel);
int model_iono(gtime_t time, const double *pos, const double *azel,
const prcopt_t *opt, int sat, const double *x,
const nav_t *nav, double *dion, double *var);
int const_corr(const obsd_t *obs, int n, const int *exc,
const nav_t *nav, const double *x, const double *pos,
const double *azel, rtk_t *rtk, double *v, double *H,
double *var);
int ppp_res(int post, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *var_rs, const int *svh,
const double *dr, int *exc, const nav_t *nav,
const double *x, rtk_t *rtk, double *v, double *H, double *R,
double *azel);
int pppnx(const prcopt_t *opt); int pppnx(const prcopt_t *opt);
void update_stat(rtk_t *rtk, const obsd_t *obs, int n, int stat);
int test_hold_amb(rtk_t *rtk);
void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav);
#endif #endif

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

@ -454,25 +454,13 @@ int satexclude(int sat, int svh, const prcopt_t *opt)
{ {
int sys = satsys(sat, NULL); int sys = satsys(sat, NULL);
if (svh<0) if (svh<0) return 1; /* ephemeris unavailable */
{
trace(3, "ephemeris unavailable: sat=%3d svh=%02X\n", sat, svh);
return 1; /* ephemeris unavailable */
}
if (opt) if (opt)
{ {
if (opt->exsats[sat-1] == 1) if (opt->exsats[sat-1] == 1) return 1; /* excluded satellite */
{
trace(3, "excluded satellite: sat=%3d svh=%02X\n", sat, svh);
return 1; /* excluded satellite */
}
if (opt->exsats[sat-1] == 2) return 0; /* included satellite */ if (opt->exsats[sat-1] == 2) return 0; /* included satellite */
if (!(sys&opt->navsys)) if (!(sys&opt->navsys)) return 1; /* unselected sat sys */
{
trace(3, "unselected sat sys: sat=%3d svh=%02X\n", sat, svh);
return 1; /* unselected sat sys */
}
} }
if (sys == SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */ if (sys == SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */
if (svh) if (svh)
@ -3144,12 +3132,12 @@ void traceobs(int level __attribute__((unused)), const obsd_t *obs __attribute__
//void tracelevel(int level) {} //void tracelevel(int level) {}
void trace (int level __attribute__((unused)), const char *format __attribute__((unused)), ...) void trace (int level __attribute__((unused)), const char *format __attribute__((unused)), ...)
{ {
/*va_list ap;*/ // va_list ap;
/* print error message to stderr */ /* print error message to stderr */
/*printf("RTKLIB TRACE[%i]:",level); // printf("RTKLIB TRACE[%i]:",level);
va_start(ap,format); // va_start(ap,format);
vfprintf(stdout,format,ap); // vfprintf(stdout,format,ap);
va_end(ap);*/ // va_end(ap);
} }
//void tracet (int level, const char *format, ...) {} //void tracet (int level, const char *format, ...) {}
//void tracemat(int level, const double *A, int n, int m, int p, int q) {} //void tracemat(int level, const double *A, int n, int m, int p, int q) {}

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

@ -266,5 +266,7 @@ void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun,
void csmooth(obs_t *obs, int ns); void csmooth(obs_t *obs, int ns);
int rtk_uncompress(const char *file, char *uncfile); int rtk_uncompress(const char *file, char *uncfile);
int expath(const char *path, char *paths[], int nmax); int expath(const char *path, char *paths[], int nmax);
void windupcorr(gtime_t time, const double *rs, const double *rr,
double *phw);
#endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */ #endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */

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

@ -51,6 +51,7 @@
* *
*----------------------------------------------------------------------------*/ *----------------------------------------------------------------------------*/
#include <iostream>
#include "rtklib_rtkpos.h" #include "rtklib_rtkpos.h"
#include "rtklib_pntpos.h" #include "rtklib_pntpos.h"
#include "rtklib_ephemeris.h" #include "rtklib_ephemeris.h"
@ -180,102 +181,103 @@ void rtkclosestat(void)
/* write solution status to buffer -------------------------------------------*/ /* write solution status to buffer -------------------------------------------*/
int rtkoutstat(rtk_t *rtk, char *buff) void rtkoutstat(rtk_t *rtk, char *buff)
{ {
ssat_t *ssat; ssat_t *ssat;
double tow, pos[3], vel[3], acc[3], vela[3] = {0}, acca[3] = {0}, xa[3]; double tow,pos[3],vel[3],acc[3],vela[3]={0},acca[3]={0},xa[3];
int i, j, week, est, nfreq, nf = NF_RTK(&rtk->opt); int i,j,week,est,nfreq,nf=NF_RTK(&rtk->opt);
char id[32], *p = buff; char id[32];
if (rtk->sol.stat <= SOLQ_NONE) if (statlevel<=0||!fp_stat) return;
{
return 0;
}
/* write ppp solution status to buffer */
if (rtk->opt.mode >= PMODE_PPP_KINEMA)
{
return pppoutstat(rtk, buff);
}
est = rtk->opt.mode >= PMODE_DGPS;
nfreq = est ? nf : 1;
tow = time2gpst(rtk->sol.time, &week);
/* receiver position */ trace(3,"outsolstat:\n");
if (est)
{
for (i = 0;i<3;i++) xa[i] = i<rtk->na ? rtk->xa[i] : 0.0;
p += sprintf(p, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow,
rtk->sol.stat, rtk->x[0], rtk->x[1], rtk->x[2], xa[0], xa[1],
xa[2]);
}
else
{
p += sprintf(p, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow,
rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2],
0.0, 0.0, 0.0);
}
/* receiver velocity and acceleration */
if (est && rtk->opt.dynamics)
{
ecef2pos(rtk->sol.rr, pos);
ecef2enu(pos, rtk->x+3, vel);
ecef2enu(pos, rtk->x+6, acc);
if (rtk->na >= 6) ecef2enu(pos, rtk->xa+3, vela);
if (rtk->na >= 9) ecef2enu(pos, rtk->xa+6, acca);
p += sprintf(p, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1],
acc[2], vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]);
}
else
{
ecef2pos(rtk->sol.rr, pos);
ecef2enu(pos, rtk->sol.rr+3, vel);
p += sprintf(p, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week, tow, rtk->sol.stat, vel[0], vel[1], vel[2],
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
/* receiver clocks */
p += sprintf(p, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week, tow, rtk->sol.stat, 1, rtk->sol.dtr[0]*1E9, rtk->sol.dtr[1]*1E9,
rtk->sol.dtr[2]*1E9, rtk->sol.dtr[3]*1E9);
/* ionospheric parameters */ /* swap solution status file */
if (est && rtk->opt.ionoopt == IONOOPT_EST) swapsolstat();
{
for (i = 0;i<MAXSAT;i++) est=rtk->opt.mode>=PMODE_DGPS;
{ nfreq=est?nf:1;
ssat = rtk->ssat+i; tow=time2gpst(rtk->sol.time,&week);
if (!ssat->vs) continue;
satno2id(i+1, id); /* receiver position */
j = II_RTK(i+1, &rtk->opt); if (est) {
xa[0] = j<rtk->na ? rtk->xa[j] : 0.0; for (i=0;i<3;i++) xa[i]=i<rtk->na?rtk->xa[i]:0.0;
p += sprintf(p, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
rtk->sol.stat, id, ssat->azel[0]*R2D, ssat->azel[1]*R2D, rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],xa[2]);
rtk->x[j], xa[0]); }
} else {
} fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
/* tropospheric parameters */ rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2],
if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG)) 0.0,0.0,0.0);
{ }
for (i = 0;i<2;i++) { /* receiver velocity and acceleration */
j = IT_RTK(i, &rtk->opt); if (est&&rtk->opt.dynamics) {
xa[0] = j<rtk->na ? rtk->xa[j] : 0.0; ecef2pos(rtk->sol.rr,pos);
p += sprintf(p, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, ecef2enu(pos,rtk->x+3,vel);
rtk->sol.stat, i+1, rtk->x[j], xa[0]); ecef2enu(pos,rtk->x+6,acc);
} if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela);
} if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca);
/* receiver h/w bias */ fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
if (est && rtk->opt.glomodear == 2) week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2],
{ vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]);
for (i = 0;i<nfreq;i++) }
{ else {
j = IL_RTK(i, &rtk->opt); ecef2pos(rtk->sol.rr,pos);
xa[0] = j<rtk->na ? rtk->xa[j] : 0.0; ecef2enu(pos,rtk->sol.rr+3,vel);
p += sprintf(p, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
rtk->sol.stat, i+1, rtk->x[j], xa[0]); week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],
} 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
} }
return (int)(p-buff); /* receiver clocks */
fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9,
rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9);
/* ionospheric parameters */
if (est&&rtk->opt.ionoopt==IONOOPT_EST) {
for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
if (!ssat->vs) continue;
satno2id(i+1,id);
j=II_RTK(i+1,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat,
id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[0]);
}
}
/* tropospheric parameters */
if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) {
for (i=0;i<2;i++) {
j=IT_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
}
}
/* receiver h/w bias */
if (est&&rtk->opt.glomodear==2) {
for (i=0;i<nfreq;i++) {
j=IL_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
}
}
if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return;
/* residuals and status */
for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
if (!ssat->vs) continue;
satno2id(i+1,id);
for (j=0;j<nfreq;j++) {
fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
ssat->resp [j],ssat->resc[j], ssat->vsat[j],ssat->snr[j]*0.25,
ssat->fix [j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j],
ssat->slipc[j],ssat->rejc[j]);
}
}
} }
@ -310,43 +312,101 @@ void swapsolstat(void)
/* output solution status ----------------------------------------------------*/ /* output solution status ----------------------------------------------------*/
void outsolstat(rtk_t *rtk) void outsolstat(rtk_t *rtk)
{ {
ssat_t *ssat; ssat_t *ssat;
double tow; double tow,pos[3],vel[3],acc[3],vela[3]={0},acca[3]={0},xa[3];
char buff[MAXSOLMSG+1], id[32]; int i,j,week,est,nfreq,nf=NF_RTK(&rtk->opt);
int i, j, n, week, nfreq, nf = NF_RTK(&rtk->opt); char id[32];
if (statlevel <= 0 || !fp_stat || !rtk->sol.stat) return; if (statlevel<=0||!fp_stat) return;
trace(3, "outsolstat:\n"); trace(3,"outsolstat:\n");
/* swap solution status file */ /* swap solution status file */
swapsolstat(); swapsolstat();
/* write solution status */ est=rtk->opt.mode >= PMODE_DGPS;
n = rtkoutstat(rtk, buff); buff[n] = '\0'; nfreq=est?nf:1;
tow=time2gpst(rtk->sol.time,&week);
fputs(buff, fp_stat); /* receiver position */
if (est) {
for (i=0;i<3;i++) xa[i]=i<rtk->na?rtk->xa[i]:0.0;
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],xa[2]);
}
else {
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2],
0.0,0.0,0.0);
}
/* receiver velocity and acceleration */
if (est&&rtk->opt.dynamics) {
ecef2pos(rtk->sol.rr,pos);
ecef2enu(pos,rtk->x+3,vel);
ecef2enu(pos,rtk->x+6,acc);
if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela);
if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2],
vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]);
}
else {
ecef2pos(rtk->sol.rr,pos);
ecef2enu(pos,rtk->sol.rr+3,vel);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
}
/* receiver clocks */
fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9,
rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9);
if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return; /* ionospheric parameters */
if (est&&rtk->opt.ionoopt==IONOOPT_EST) {
for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
if (!ssat->vs) continue;
satno2id(i+1,id);
j=II_RTK(i+1,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat,
id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[0]);
}
}
/* tropospheric parameters */
if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) {
for (i=0;i<2;i++) {
j=IT_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
}
}
/* receiver h/w bias */
if (est&&rtk->opt.glomodear==2) {
for (i=0;i<nfreq;i++) {
j=IL_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
}
}
if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return;
tow = time2gpst(rtk->sol.time, &week); /* residuals and status */
nfreq = rtk->opt.mode >= PMODE_DGPS ? nf : 1; for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
/* write residuals and status */ if (!ssat->vs) continue;
for (i = 0;i<MAXSAT;i++) satno2id(i+1,id);
{ for (j=0;j<nfreq;j++) {
ssat = rtk->ssat+i; fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
if (!ssat->vs) continue; week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
satno2id(i+1, id); ssat->resp [j],ssat->resc[j], ssat->vsat[j],ssat->snr[j]*0.25,
for (j = 0;j<nfreq;j++) { ssat->fix [j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j],
ssat->slipc[j],ssat->rejc[j]);
fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n", }
week, tow, id, j+1, ssat->azel[0]*R2D, ssat->azel[1]*R2D, }
ssat->resp[j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25,
ssat->fix[j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j],
ssat->slipc[j], ssat->rejc[j]);
}
}
} }
@ -2064,7 +2124,6 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, NULL, rtk->ssat, msg)) if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, NULL, rtk->ssat, msg))
{ {
errmsg(rtk, "point pos error (%s)\n", msg); errmsg(rtk, "point pos error (%s)\n", msg);
if (!rtk->opt.dynamics) if (!rtk->opt.dynamics)
{ {
outsolstat(rtk); outsolstat(rtk);

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

@ -93,7 +93,7 @@ int rtkopenstat(const char *file, int level);
void rtkclosestat(void); void rtkclosestat(void);
int rtkoutstat(rtk_t *rtk, char *buff); void rtkoutstat(rtk_t *rtk);
void swapsolstat(void); void swapsolstat(void);

5
src/tests/system-tests/position_test.cc
View File

@ -382,7 +382,10 @@ int Position_Gps_L1_System_Test::configure_receiver()
config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1"); config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1");
config->set_property("PVT.dump", "false"); config->set_property("PVT.dump", "false");
config->set_property("PVT.rinex_version", std::to_string(2)); config->set_property("PVT.rinex_version", std::to_string(2));
config->set_property("PVT.positioning_mode", std::to_string(7));
config->set_property("PVT.iono_model", std::to_string(0));
config->set_property("PVT.trop_model", std::to_string(0));
config->set_property("PVT.AR_GPS", std::to_string(4));
return 0; return 0;
} }