mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-05-03 16:04:12 +00:00
Enables PPP positioning mode by resorting to functions in RTKLIB 2.4.2
This commit is contained in:
Carles Fernandez
commit
afa9edf848
@ -395,6 +395,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
|
||||
if(positioning_mode <= PMODE_FIXED) nx = 4 + 3;
|
||||
if(positioning_mode >= PMODE_PPP_KINEMA) nx = NX_PPP(&rtklib_configuration_options);
|
||||
int na = NP_PPP(&rtklib_configuration_options);
|
||||
|
||||
double x[nx];
|
||||
double Px[nx*nx];
|
||||
double xa[na];
|
||||
|
||||
@ -88,26 +88,26 @@ const int PMODE_PPP_KINEMA = 6; //!< positioning mode: PPP-kinemaric
|
||||
const int PMODE_PPP_STATIC = 7; //!< positioning mode: PPP-static
|
||||
const int PMODE_PPP_FIXED = 8; //!< positioning mode: PPP-fixed
|
||||
|
||||
const unsigned int SOLF_LLH = 0; //!< solution format: lat/lon/height
|
||||
const unsigned int SOLF_XYZ = 1; //!< solution format: x/y/z-ecef
|
||||
const unsigned int SOLF_ENU = 2; //!< solution format: e/n/u-baseline
|
||||
const unsigned int SOLF_NMEA = 3; //!< solution format: NMEA-183
|
||||
const unsigned int SOLF_STAT = 4; //!< solution format: solution status
|
||||
const unsigned int SOLF_GSIF = 5; //!< solution format: GSI F1/F2
|
||||
const int SOLF_LLH = 0; //!< solution format: lat/lon/height
|
||||
const int SOLF_XYZ = 1; //!< solution format: x/y/z-ecef
|
||||
const int SOLF_ENU = 2; //!< solution format: e/n/u-baseline
|
||||
const int SOLF_NMEA = 3; //!< solution format: NMEA-183
|
||||
const int SOLF_STAT = 4; //!< solution format: solution status
|
||||
const int SOLF_GSIF = 5; //!< solution format: GSI F1/F2
|
||||
|
||||
const unsigned int SOLQ_NONE = 0; //!< solution status: no solution
|
||||
const unsigned int SOLQ_FIX = 1; //!< solution status: fix
|
||||
const unsigned int SOLQ_FLOAT = 2; //!< solution status: float
|
||||
const unsigned int SOLQ_SBAS = 3; //!< solution status: SBAS
|
||||
const unsigned int SOLQ_DGPS = 4; //!< solution status: DGPS/DGNSS
|
||||
const unsigned int SOLQ_SINGLE = 5; //!< solution status: single
|
||||
const unsigned int SOLQ_PPP = 6; //!< solution status: PPP
|
||||
const unsigned int SOLQ_DR = 7; //!< solution status: dead reckoning
|
||||
const unsigned int MAXSOLQ = 7; //!< max number of solution status
|
||||
const int SOLQ_NONE = 0; //!< solution status: no solution
|
||||
const int SOLQ_FIX = 1; //!< solution status: fix
|
||||
const int SOLQ_FLOAT = 2; //!< solution status: float
|
||||
const int SOLQ_SBAS = 3; //!< solution status: SBAS
|
||||
const int SOLQ_DGPS = 4; //!< solution status: DGPS/DGNSS
|
||||
const int SOLQ_SINGLE = 5; //!< solution status: single
|
||||
const int SOLQ_PPP = 6; //!< solution status: PPP
|
||||
const int SOLQ_DR = 7; //!< solution status: dead reckoning
|
||||
const int MAXSOLQ = 7; //!< max number of solution status
|
||||
|
||||
const unsigned int TIMES_GPST = 0; //!< time system: gps time
|
||||
const unsigned int TIMES_UTC = 1; //!< time system: utc
|
||||
const unsigned int TIMES_JST = 2; //!< time system: jst
|
||||
const int TIMES_GPST = 0; //!< time system: gps time
|
||||
const int TIMES_UTC = 1; //!< time system: utc
|
||||
const int TIMES_JST = 2; //!< time system: jst
|
||||
|
||||
|
||||
const double ERR_SAAS = 0.3; //!< saastamoinen model error std (m)
|
||||
@ -261,7 +261,9 @@ const int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model
|
||||
const int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model
|
||||
const int TROPOPT_EST = 3; //!< troposphere option: ZTD 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_PREC = 1; //!< ephemeris option: precise ephemeris
|
||||
@ -286,34 +288,25 @@ const int ARMODE_OFF = 0; //!< AR mode: off
|
||||
const int ARMODE_CONT = 1; //!< AR mode: continuous
|
||||
const int ARMODE_INST = 2; //!< AR mode: instantaneous
|
||||
const int ARMODE_FIXHOLD = 3; //!< AR mode: fix and hold
|
||||
const int ARMODE_WLNL = 4; //!< AR mode: wide lane/narrow lane
|
||||
const int ARMODE_TCAR = 5; //!< AR mode: triple carrier ar
|
||||
const int ARMODE_PPPAR = 4; //!< AR mode: PPP-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 */
|
||||
|
||||
|
||||
/* 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
|
||||
|
||||
|
||||
#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 */
|
||||
time_t time; /* time (s) expressed by standard time_t */
|
||||
double sec; /* fraction of second under 1 s */
|
||||
@ -976,7 +969,7 @@ typedef struct { /* RTK control/result type */
|
||||
double *x, *P; /* float states and their covariance */
|
||||
double *xa,*Pa; /* fixed states and their covariance */
|
||||
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 */
|
||||
int neb; /* bytes in error message buffer */
|
||||
char errbuf[MAXERRMSG]; /* error message buffer */
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -56,130 +56,131 @@
|
||||
|
||||
#include "rtklib.h"
|
||||
|
||||
#define MAX_PPP(x,y) ((x)>(y)?(x):(y))
|
||||
|
||||
#define MIN_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 */
|
||||
const double MAX_STD_FIX = 0.15; /* max std-dev (3d) to fix solution */
|
||||
const int MIN_NSAT_SOL = 4; /* min satellite number for solution */
|
||||
const double THRES_REJECT = 4.0; /* reject threshold of posfit-res (sigma) */
|
||||
#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 THRES_MW_JUMP = 10.0;
|
||||
const double MIN_ARC_GAP = 300.0; /* min arc gap (s) */
|
||||
const double CONST_AMB = 0.001; /* constraint to fixed ambiguity */
|
||||
const double THRES_RES = 0.3; /* threashold of residuals test (m) */
|
||||
const double LOG_PI = 1.14472988584940017; /* log(pi) */
|
||||
const double SQRT2 = 1.41421356237309510; /* sqrt(2) */
|
||||
|
||||
const double VAR_POS_PPP = std::pow(60.0, 2.0); /* init variance receiver position (m^2) */
|
||||
const double VAR_CLK = std::pow(60.0, 2.0); /* init variance receiver clock (m^2) */
|
||||
const double VAR_ZTD = std::pow( 0.6, 2.0); /* init variance ztd (m^2) */
|
||||
const double VAR_GRA_PPP = std::pow(0.01, 2.0); /* init variance gradient (m^2) */
|
||||
const double VAR_DCB = std::pow(30.0, 2.0); /* init variance dcb (m^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 VAR_POS_PPP = std::pow(100.0, 2.0); /* init variance receiver position (m^2) */
|
||||
const double VAR_CLK = std::pow(100.0, 2.0); /* init variance receiver clock (m^2) */
|
||||
const double VAR_ZTD = std::pow( 0.3, 2.0); /* init variance ztd (m^2) */
|
||||
const double VAR_GRA_PPP = std::pow(0.001, 2.0); /* init variance gradient (m^2) */
|
||||
const double VAR_BIAS = std::pow(100.0, 2.0); /* init variance phase-bias (m^2) */
|
||||
|
||||
const double EFACT_GPS_L5 = 10.0; /* error factor of GPS/QZS L5 */
|
||||
const double VAR_IONO_OFF = std::pow(10.0, 2.0); /* variance of iono-model-off */
|
||||
|
||||
const double MUDOT_GPS = (0.00836*D2R); /* average angular velocity GPS (rad/s) */
|
||||
const double MUDOT_GLO = (0.00888*D2R); /* average angular velocity GLO (rad/s) */
|
||||
const double EPS0_GPS = (13.5*D2R); /* max shadow crossing angle GPS (rad) */
|
||||
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) */
|
||||
|
||||
/* functions originally included in RTKLIB/src/ppp_ar.c v2.4.2*/
|
||||
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);
|
||||
|
||||
|
||||
|
||||
/* functions originally included in RTKLIB/src/ppp.c v2.4.2 */
|
||||
void pppoutsolstat(rtk_t *rtk, int level, FILE *fp);
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
double STD(rtk_t *rtk, int i);
|
||||
int pppoutstat(rtk_t *rtk, char *buff);
|
||||
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);
|
||||
|
||||
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 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 udclk_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 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,
|
||||
double *dant);
|
||||
void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, double *dant);
|
||||
|
||||
double trop_model_prec(gtime_t time, const double *pos,
|
||||
const double *azel, const double *x, double *dtdx,
|
||||
double *var);
|
||||
|
||||
int model_trop(gtime_t time, const double *pos, const double *azel,
|
||||
double prectrop(gtime_t time, const double *pos, const double *azel,
|
||||
const prcopt_t *opt, const double *x, double *dtdx,
|
||||
const nav_t *nav, double *dtrp, double *var);
|
||||
|
||||
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 res_ppp(int iter, const obsd_t *obs, int n, const double *rs,
|
||||
const double *dts, const double *vare, const int *svh,
|
||||
const nav_t *nav, const double *x, rtk_t *rtk, double *v,
|
||||
double *H, double *R, double *azel);
|
||||
|
||||
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);
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@ -454,7 +454,7 @@ int satexclude(int sat, int svh, const prcopt_t *opt)
|
||||
{
|
||||
int sys = satsys(sat, NULL);
|
||||
|
||||
if (svh<0)
|
||||
if (svh < 0)
|
||||
{
|
||||
trace(3, "ephemeris unavailable: sat=%3d svh=%02X\n", sat, svh);
|
||||
return 1; /* ephemeris unavailable */
|
||||
@ -474,7 +474,7 @@ int satexclude(int sat, int svh, const prcopt_t *opt)
|
||||
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)
|
||||
{
|
||||
trace(3, "unhealthy satellite: sat=%3d svh=%02X\n", sat, svh);
|
||||
@ -4013,3 +4013,52 @@ int expath(const char *path, char *paths[], int nmax)
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
/* From RTKLIB 2.4.2 */
|
||||
void windupcorr(gtime_t time, const double *rs, const double *rr, double *phw)
|
||||
{
|
||||
double ek[3], exs[3], eys[3], ezs[3], ess[3], exr[3], eyr[3], eks[3], ekr[3], E[9];
|
||||
double dr[3], ds[3], drs[3], r[3], pos[3], rsun[3], cosp, ph, erpv[5]={0};
|
||||
int i;
|
||||
|
||||
trace(4, "windupcorr: time=%s\n", time_str(time, 0));
|
||||
|
||||
/* sun position in ecef */
|
||||
sunmoonpos(gpst2utc(time), erpv, rsun, NULL, NULL);
|
||||
|
||||
/* unit vector satellite to receiver */
|
||||
for (i = 0; i < 3; i++) r[i] = rr[i]-rs[i];
|
||||
if (!normv3(r, ek)) return;
|
||||
|
||||
/* unit vectors of satellite antenna */
|
||||
for (i = 0; i < 3; i++) r[i] = -rs[i];
|
||||
if (!normv3(r, ezs)) return;
|
||||
for (i = 0; i < 3; i++) r[i] = rsun[i]-rs[i];
|
||||
if (!normv3(r, ess)) return;
|
||||
cross3(ezs, ess, r);
|
||||
if (!normv3(r, eys)) return;
|
||||
cross3(eys, ezs, exs);
|
||||
|
||||
/* unit vectors of receiver antenna */
|
||||
ecef2pos(rr, pos);
|
||||
xyz2enu(pos, E);
|
||||
exr[0] = E[1]; exr[1] = E[4]; exr[2] = E[7]; /* x = north */
|
||||
eyr[0] = -E[0]; eyr[1] = -E[3]; eyr[2] = -E[6]; /* y = west */
|
||||
|
||||
/* phase windup effect */
|
||||
cross3(ek, eys, eks);
|
||||
cross3(ek, eyr, ekr);
|
||||
for (i = 0; i < 3; i++)
|
||||
{
|
||||
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_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;
|
||||
cross3(ds, dr, drs);
|
||||
if (dot(ek, drs, 3) < 0.0) ph = -ph;
|
||||
|
||||
*phw = ph + floor(*phw - ph + 0.5); /* in cycle */
|
||||
}
|
||||
|
||||
@ -266,5 +266,6 @@ void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun,
|
||||
void csmooth(obs_t *obs, int ns);
|
||||
int rtk_uncompress(const char *file, char *uncfile);
|
||||
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_ */
|
||||
|
||||
@ -180,22 +180,20 @@ void rtkclosestat(void)
|
||||
|
||||
|
||||
/* write solution status to buffer -------------------------------------------*/
|
||||
int rtkoutstat(rtk_t *rtk, char *buff)
|
||||
void rtkoutstat(rtk_t *rtk, char *buff __attribute__((unused)))
|
||||
{
|
||||
ssat_t *ssat;
|
||||
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);
|
||||
char id[32], *p = buff;
|
||||
char id[32];
|
||||
|
||||
if (statlevel <= 0 || !fp_stat) return;
|
||||
|
||||
trace(3, "outsolstat:\n");
|
||||
|
||||
/* swap solution status file */
|
||||
swapsolstat();
|
||||
|
||||
if (rtk->sol.stat <= SOLQ_NONE)
|
||||
{
|
||||
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);
|
||||
@ -203,14 +201,13 @@ int rtkoutstat(rtk_t *rtk, char *buff)
|
||||
/* receiver position */
|
||||
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]);
|
||||
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
|
||||
{
|
||||
p += sprintf(p, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow,
|
||||
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);
|
||||
}
|
||||
@ -222,20 +219,20 @@ int rtkoutstat(rtk_t *rtk, char *buff)
|
||||
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]);
|
||||
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);
|
||||
p += sprintf(p, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
|
||||
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 */
|
||||
p += sprintf(p, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
|
||||
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);
|
||||
|
||||
@ -248,20 +245,20 @@ int rtkoutstat(rtk_t *rtk, char *buff)
|
||||
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;
|
||||
p += sprintf(p, "$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]);
|
||||
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++) {
|
||||
for (i = 0;i<2;i++)
|
||||
{
|
||||
j = IT_RTK(i, &rtk->opt);
|
||||
xa[0] = j<rtk->na ? rtk->xa[j] : 0.0;
|
||||
p += sprintf(p, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow,
|
||||
rtk->sol.stat, i+1, rtk->x[j], xa[0]);
|
||||
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 */
|
||||
@ -270,12 +267,28 @@ int rtkoutstat(rtk_t *rtk, char *buff)
|
||||
for (i = 0;i<nfreq;i++)
|
||||
{
|
||||
j = IL_RTK(i, &rtk->opt);
|
||||
xa[0] = j<rtk->na ? rtk->xa[j] : 0.0;
|
||||
p += sprintf(p, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow,
|
||||
rtk->sol.stat, i+1, rtk->x[j], xa[0]);
|
||||
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]);
|
||||
}
|
||||
}
|
||||
return (int)(p-buff);
|
||||
}
|
||||
|
||||
|
||||
@ -311,39 +324,109 @@ void swapsolstat(void)
|
||||
void outsolstat(rtk_t *rtk)
|
||||
{
|
||||
ssat_t *ssat;
|
||||
double tow;
|
||||
char buff[MAXSOLMSG+1], id[32];
|
||||
int i, j, n, week, nfreq, nf = NF_RTK(&rtk->opt);
|
||||
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);
|
||||
char id[32];
|
||||
|
||||
if (statlevel <= 0 || !fp_stat || !rtk->sol.stat) return;
|
||||
if (statlevel <= 0 || !fp_stat) return;
|
||||
|
||||
trace(3, "outsolstat:\n");
|
||||
|
||||
/* swap solution status file */
|
||||
swapsolstat();
|
||||
|
||||
/* write solution status */
|
||||
n = rtkoutstat(rtk, buff); buff[n] = '\0';
|
||||
|
||||
fputs(buff, fp_stat);
|
||||
|
||||
if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return;
|
||||
|
||||
est = rtk->opt.mode >= PMODE_DGPS;
|
||||
nfreq = est ? nf : 1;
|
||||
tow = time2gpst(rtk->sol.time, &week);
|
||||
nfreq = rtk->opt.mode >= PMODE_DGPS ? nf : 1;
|
||||
|
||||
/* write residuals and status */
|
||||
/* 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);
|
||||
|
||||
/* 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);
|
||||
for (j = 0;j<nfreq;j++) {
|
||||
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->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]);
|
||||
}
|
||||
}
|
||||
@ -2052,7 +2135,7 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
|
||||
if (opt->refpos <= POSOPT_RINEX && opt->mode != PMODE_SINGLE &&
|
||||
opt->mode != PMODE_MOVEB)
|
||||
{
|
||||
for (i = 0;i<6;i++) rtk->rb[i] = i<3 ? opt->rb[i] : 0.0;
|
||||
for (i = 0;i<6;i++) rtk->rb[i] = i < 3 ? opt->rb[i] : 0.0;
|
||||
}
|
||||
/* count rover/base station observations */
|
||||
for (nu = 0;nu <n && obs[nu ].rcv == 1;nu++) ;
|
||||
@ -2064,7 +2147,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))
|
||||
{
|
||||
errmsg(rtk, "point pos error (%s)\n", msg);
|
||||
|
||||
if (!rtk->opt.dynamics)
|
||||
{
|
||||
outsolstat(rtk);
|
||||
@ -2123,7 +2205,7 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
|
||||
{
|
||||
rtk->sol.age = (float)timediff(obs[0].time, obs[nu].time);
|
||||
|
||||
if (fabs(rtk->sol.age)>opt->maxtdiff)
|
||||
if (fabs(rtk->sol.age) > opt->maxtdiff)
|
||||
{
|
||||
errmsg(rtk, "age of differential error (age=%.1f)\n", rtk->sol.age);
|
||||
outsolstat(rtk);
|
||||
|
||||
@ -93,7 +93,7 @@ int rtkopenstat(const char *file, int level);
|
||||
|
||||
void rtkclosestat(void);
|
||||
|
||||
int rtkoutstat(rtk_t *rtk, char *buff);
|
||||
void rtkoutstat(rtk_t *rtk);
|
||||
|
||||
void swapsolstat(void);
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user