diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 7fe7119c5..0f59b6dbd 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -39,10 +39,10 @@ #include "rtklib_solution.h" #include #include +#include "iostream" #include #include #include -#include "iostream" using namespace std; @@ -1006,8 +1006,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ d_nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &d_nav_data); } } - - result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); + std::vector tropo_vec; + std::vector iono_vec; + std::vector pr_corrected_code_bias_vec; + result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, tropo_vec, + iono_vec, pr_corrected_code_bias_vec); if (result == 0) { @@ -1114,7 +1117,10 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.sat_health_flag(n) = svh.at(n); new_vtl_data.sat_CN0_dB_hz(n) = d_obs_data.at(n).SNR[0]; // TODO: first version of VTL works only with ONE frequency band (band #0 is L1) - new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; + //new_vtl_data.pr_m(n) = d_obs_data.at(n).P[0]; //RAW pseudoranges + //To.Do: check it VTL uses all the information as in rtklib rescode function: v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); + //corrected pr with code bias, iono and tropo. Still needs the dtr(rx clock bias) and satellite clock bias (dts) + new_vtl_data.pr_m(n) = pr_corrected_code_bias_vec[n] - tropo_vec[n] - iono_vec[n]; new_vtl_data.doppler_hz(n) = d_obs_data.at(n).D[0]; new_vtl_data.carrier_phase_rads(n) = d_obs_data.at(n).L[0]; } @@ -1138,9 +1144,9 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ new_vtl_data.rx_pvt_var[6] = pvt_sol.qr[0]; //time new_vtl_data.rx_pvt_var[7] = pvt_sol.qr[0]; //doppler //receiver clock offset and receiver clock drift - new_vtl_data.rx_dts(0)=rx_position_and_time[3]; - new_vtl_data.rx_dts(1)=pvt_sol.dtr[5]/1e6; // [ppm] to [s] - + new_vtl_data.rx_dts(0) = rx_position_and_time[3]; + new_vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] + //Call the VTL engine loop: miguel: Should we wait until valid PVT solution? vtl_engine.vtl_loop(new_vtl_data); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 594815996..f163ad1be 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -34,7 +34,7 @@ #include "rtklib_ionex.h" #include "rtklib_sbas.h" #include -#include + /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) @@ -403,7 +403,7 @@ int rescode(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, const prcopt_t *opt, double *v, double *H, double *var, double *azel, int *vsat, - double *resp, int *ns) + double *resp, int *ns, double *tropo_m, double *iono_m, double *pr_corrected_code_bias) { double r; double dion; @@ -500,6 +500,11 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, /* pseudorange residual */ v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp); + /* MOD ARRIBAS */ + pr_corrected_code_bias[i] = P; + tropo_m[i] = dtrp; + iono_m[i] = dion; + /* design matrix */ for (j = 0; j < NX; j++) { @@ -603,7 +608,8 @@ int valsol(const double *azel, const int *vsat, int n, int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, - double *resp, char *msg) + double *resp, char *msg, + std::vector &tropo_vec, std::vector &iono_vec, std::vector &pr_corrected_code_bias_vec) { double x[NX] = {0}; double dx[NX]; @@ -621,6 +627,11 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, int ns; char msg_aux[128]; + double *tropo_m, *iono_m, *pr_corrected_code_bias; + tropo_m = mat(n + 4, 1); + iono_m = mat(n + 4, 1); + pr_corrected_code_bias = mat(n + 4, 1); + trace(3, "estpos : n=%d\n", n); v = mat(n + 4, 1); @@ -635,7 +646,8 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, for (i = 0; i < MAXITR; i++) { /* pseudorange residuals */ - nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, &ns); + nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, + &ns, tropo_m, iono_m, pr_corrected_code_bias); if (nv < NX) { @@ -690,6 +702,13 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, { sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE; } + + for (k = 0; k < n; k++) + { + tropo_vec.push_back(tropo_m[k]); + iono_vec.push_back(iono_m[k]); + pr_corrected_code_bias_vec.push_back(pr_corrected_code_bias[k]); + } free(v); free(H); free(var); @@ -766,9 +785,12 @@ int raim_fde(const obsd_t *obs, int n, const double *rs, vare_e[k] = vare[j]; svh_e[k++] = svh[j]; } + std::vector tropo_vec; + std::vector iono_vec; + std::vector pr_corrected_code_bias_vec; /* estimate receiver position without a satellite */ if (!estpos(obs_e, n - 1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, - vsat_e, resp_e, msg_e)) + vsat_e, resp_e, msg_e, iono_vec, tropo_vec, pr_corrected_code_bias_vec)) { trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); continue; @@ -978,7 +1000,9 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, *-----------------------------------------------------------------------------*/ int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, - char *msg) + char *msg, std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec) { prcopt_t opt_ = *opt; double *rs; @@ -1022,7 +1046,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh.data()); /* estimate receiver position with pseudorange */ - stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg); + stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg, + iono_vec, tropo_vec, pr_corrected_code_bias_vec); /* raim fde */ if (!stat && n >= 6 && opt->posopt[4]) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index 735df2905..686e15d14 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.h +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -35,6 +35,7 @@ #include "rtklib.h" #include "rtklib_rtkcmn.h" +#include /* constants -----------------------------------------------------------------*/ const int NX = 4 + 3; //!< # of estimated parameters @@ -92,7 +93,7 @@ int rescode(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, const prcopt_t *opt, double *v, double *H, double *var, double *azel, int *vsat, - double *resp, int *ns); + double *resp, int *ns, double *tropo_m, double *iono_m, double *pr_corrected_code_bias); /* validate solution ---------------------------------------------------------*/ int valsol(const double *azel, const int *vsat, int n, @@ -103,7 +104,8 @@ int valsol(const double *azel, const int *vsat, int n, int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, - double *resp, char *msg); + double *resp, char *msg, std::vector &tropo_vec, + std::vector &iono_vec, std::vector &pr_corrected_code_bias_vec); /* raim fde (failure detection and exclution) -------------------------------*/ int raim_fde(const obsd_t *obs, int n, const double *rs, @@ -140,6 +142,8 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, */ int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, - char *msg); + char *msg, std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec); #endif // GNSS_SDR_RTKLIB_PNTPOS_H diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 2e13bde39..6afc7b56f 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -38,7 +38,6 @@ #include #include #include -#include static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)), const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)), @@ -2762,7 +2761,10 @@ void rtkfree(rtk_t *rtk) * notes : before calling function, base station position rtk->sol.rb[] should * be properly set for relative mode except for moving-baseline *-----------------------------------------------------------------------------*/ -int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, + std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec) { prcopt_t *opt = &rtk->opt; sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; @@ -2797,7 +2799,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) time = rtk->sol.time; /* previous epoch */ /* rover position by single point positioning */ - if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg)) + if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg, tropo_vec, + iono_vec, pr_corrected_code_bias_vec)) { errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) @@ -2839,7 +2842,8 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) if (opt->mode == PMODE_MOVEB) { /* moving baseline */ /* estimate position/velocity of base station */ - if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg)) + if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg, tropo_vec, + iono_vec, pr_corrected_code_bias_vec)) { errmsg(rtk, "base station position error (%s)\n", msg); return 0; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.h b/src/algorithms/libs/rtklib/rtklib_rtkpos.h index 29f1924d2..9fcb7ea47 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.h @@ -34,6 +34,7 @@ #include "rtklib.h" #include "rtklib_rtkcmn.h" +#include /** \addtogroup PVT * \{ */ @@ -174,7 +175,10 @@ void rtkinit(rtk_t *rtk, const prcopt_t *opt); void rtkfree(rtk_t *rtk); -int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); +int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, + std::vector &tropo_vec, + std::vector &iono_vec, + std::vector &pr_corrected_code_bias_vec); /** \} */ diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index 975a61f86..5464c1ef3 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -38,6 +38,7 @@ #include "rtklib_solution.h" #include "rtklib_stream.h" #include +#include /* write solution header to output stream ------------------------------------*/ void writesolhead(stream_t *stream, const solopt_t *solopt) @@ -595,7 +596,11 @@ void *rtksvrthread(void *arg) } /* rtk positioning */ rtksvrlock(svr); - rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav); + std::vector tropo_vec; + std::vector iono_vec; + std::vector pr_corrected_code_bias_vec; + rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, tropo_vec, + iono_vec, pr_corrected_code_bias_vec); rtksvrunlock(svr); if (svr->rtk.sol.stat != SOLQ_NONE)