diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index c3716e594..e7bfb3694 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -91,6 +91,12 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, pvt_output_parameters.kf_use_imu_vel = configuration->property(role + ".kf_use_imu_vel", false); // PVT VTL settings + pvt_output_parameters.enable_pvt_vtl = configuration->property(role + ".enable_pvt_vtl", false); + pvt_output_parameters.enable_pvt_output_vtl = configuration->property(role + ".enable_pvt_output_vtl", false); + pvt_output_parameters.enable_pvt_closure_vtl = configuration->property(role + ".enable_pvt_closure_vtl", false); + pvt_output_parameters.vtl_kinematic = configuration->property(role + ".vtl_kinematic", false); + pvt_output_parameters.vtl_dump = configuration->property(role + ".vtl_dump", pvt_output_parameters.vtl_dump); + pvt_output_parameters.vtl_dump_filename = configuration->property(role + ".vtl_dump_filename", pvt_output_parameters.vtl_dump_filename); const int gps_1C_count = configuration->property("Channels_1C.count", 0); const int gps_2S_count = configuration->property("Channels_2S.count", 0); const int gps_L5_count = configuration->property("Channels_L5.count", 0); @@ -98,12 +104,6 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, const int gal_E5a_count = configuration->property("Channels_5X.count", 0); const int gal_E5b_count = configuration->property("Channels_7X.count", 0); const int gal_E6_count = configuration->property("Channels_E6.count", 0); - pvt_output_parameters.enable_pvt_vtl = configuration->property(role + ".enable_pvt_vtl", false); - pvt_output_parameters.enable_pvt_output_vtl = configuration->property(role + ".enable_pvt_output_vtl", false); - pvt_output_parameters.enable_pvt_closure_vtl = configuration->property(role + ".enable_pvt_closure_vtl", false); - pvt_output_parameters.vtl_kinematic = configuration->property(role + ".vtl_kinematic", false); - pvt_output_parameters.vtl_dump = configuration->property(role + ".vtl_dump", pvt_output_parameters.vtl_dump); - pvt_output_parameters.vtl_dump_filename = configuration->property(role + ".vtl_dump_filename", pvt_output_parameters.vtl_dump_filename); pvt_output_parameters.vtl_gps_channels = gps_1C_count + gps_2S_count + gps_L5_count; pvt_output_parameters.vtl_gal_channels = gal_1B_count + gal_E5a_count + gal_E5b_count + gal_E6_count; pvt_output_parameters.vtl_init_pos_ecef_sd_m = configuration->property(role + ".vtl_init_pos_ecef_sd_m", 10.0); diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc old mode 100755 new mode 100644 index 92812a458..83ecc834f --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -158,6 +158,10 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, vtl_data->init_storage(d_conf.vtl_gps_channels + d_conf.vtl_gal_channels); vtl_Core = std::make_unique(d_conf); vtl_Core->vtl_init(d_conf); + vtl_epoch = 0; + vtl_output = false; + d_rx_clk_b_idx = (d_conf.vtl_gal_channels > 0) ? 7 : 6; + d_rx_clk_d_idx = d_rx_clk_b_idx + 1; } } @@ -907,6 +911,17 @@ void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, ui } } +const Gnss_Synchro *Rtklib_Solver::get_synchro(const std::map &map, const Gnss_Synchro &freq1_synchro) +{ + for (const auto &[freq2_ch_id, freq2_synchro] : map) + { + if (freq2_synchro.System == freq1_synchro.System && freq2_synchro.PRN == freq1_synchro.PRN && freq2_ch_id != freq1_synchro.Channel_ID) + { + return &freq2_synchro; + } + } + return nullptr; +} bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s, const SensorDataAggregator &sensor_data_aggregator) { @@ -1505,7 +1520,18 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } } - result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); + int N_sv = valid_obs + glo_valid_obs; + std::vector obs_pr_vec; + std::vector tropo_m_vec; + std::vector iono_m_vec; + std::vector code_bias_m_vec; + std::vector rho_vec; + double *rs; + double *dts; + rs = mat(6, N_sv); + dts = mat(2, N_sv); + + result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data, obs_pr_vec, tropo_m_vec, iono_m_vec, code_bias_m_vec, rs, dts); if (result == 0) { @@ -1606,6 +1632,142 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ } this->set_rx_pos({rx_position_and_time[0], rx_position_and_time[1], rx_position_and_time[2]}); // save ECEF position for the next iteration + + /******************** VECTOR TRACKING LOOP ********************/ + if ((d_conf.enable_pvt_vtl) && (kf_update_interval_s == 0.02)) // 20ms - observable interval + { + vtl_data->clear_storage(); + uint8_t ch_id; + gnss_observables_iter = gnss_observables_map.cbegin(); + + // timming + vtl_data->dt_s = gnss_observables_iter->second.RX_time - vtl_data->rx_time; + vtl_data->rx_time = gnss_observables_iter->second.RX_time; + + int j = 0; + for (int n = 0; n < d_rtk.sol.ns; n++) + { + // one observation per satellite - exclude invalid observables + while (gnss_observables_iter->second.Pseudorange_m != obs_pr_vec[j]) + { + if (obs_pr_vec[j] == 0) + { + j++; + } + else + { + ++gnss_observables_iter; + } + } + + const uint8_t idx6n = 6 * j; + const uint8_t idx2n = 2 * j; + + // channel id + ch_id = gnss_observables_iter->second.Channel_ID; + vtl_data->rx_ch(ch_id) = ch_id; + + // satellite info + vtl_data->sv_id(ch_id) = gnss_observables_iter->second.PRN; + vtl_data->sv_p.row(ch_id) = arma::Row({rs[0 + idx6n], rs[1 + idx6n], rs[2 + idx6n]}); + vtl_data->sv_v.row(ch_id) = arma::Row({rs[3 + idx6n], rs[4 + idx6n], rs[5 + idx6n]}); + vtl_data->sv_clk.row(ch_id) = arma::Row({dts[0 + idx2n] * SPEED_OF_LIGHT_M_S, dts[1 + idx2n] * SPEED_OF_LIGHT_M_S}); + vtl_data->sv_elev(ch_id) = d_rtk.ssat[d_obs_data.at(j).sat - 1].azel[1]; + // atmosferic and code bias + vtl_data->tropo_bias(ch_id) = tropo_m_vec[j]; + vtl_data->iono_bias(ch_id) = iono_m_vec[j]; + vtl_data->code_bias(ch_id) = code_bias_m_vec[j]; + + // tracking info + vtl_data->CN0_dB_hz(ch_id) = gnss_observables_iter->second.CN0_dB_hz; + + // observations + vtl_data->ch_sample_counter(ch_id) = gnss_observables_iter->second.Tracking_sample_counter; + // pseudorange + vtl_data->obs_pr(ch_id) = gnss_observables_iter->second.Pseudorange_m; + + // pseudorange rate + if (std::string(gnss_observables_iter->second.Signal) == "1C" || std::string(gnss_observables_iter->second.Signal) == "1B") + { + vtl_data->obs_prr(ch_id) = -gnss_observables_iter->second.Carrier_Doppler_hz * Lambda_GPS_L1; + vtl_data->band(ch_id) = 0; // L1E1 + } + else + { + vtl_data->obs_prr(ch_id) = -gnss_observables_iter->second.Carrier_Doppler_hz * Lambda_GPS_L5; + vtl_data->band(ch_id) = 1; // L5E5 + } + + // available observations + vtl_data->active_ch(ch_id) = 1; + // new observations + if (vtl_data->past_active_ch(ch_id) == 0) + { + vtl_data->new_ch(ch_id) = 1; + } + + // needs at least one vtl epoch to have feedback + if ((vtl_epoch >= 20) && (!vtl_data->new_ch(ch_id))) // close loop after 20 epochs + { + vtl_data->loop_closure(ch_id) = d_conf.enable_pvt_closure_vtl ? 1 : 0; + } + + // in dual-frequency get channel info from both frequencies + if (d_rtk.opt.ionoopt == IONOOPT_IFLC) + { + const Gnss_Synchro *freq2_signal = get_synchro(gnss_observables_map, gnss_observables_iter->second); + vtl_data->rx_ch2(ch_id) = freq2_signal->Channel_ID; + vtl_data->ch2_sample_counter(ch_id) = freq2_signal->Tracking_sample_counter; + } + + ++gnss_observables_iter; + j++; + } + + // ionosphere option + vtl_data->ionoopt = d_rtk.opt.ionoopt; + + // number of observations + vtl_data->N_sv = arma::sum(vtl_data->active_ch == 1); + // which observations disappeard? + vtl_data->old_ch = vtl_data->past_active_ch % (1 - vtl_data->active_ch); + + // VTL initialization from rtklib + // receiver position and velocity + vtl_data->rx_p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; + vtl_data->rx_v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; + // receiver clock offset and receiver clock drift + vtl_data->rx_clk = {(pvt_sol.dtr[0]) * SPEED_OF_LIGHT_M_S, + (pvt_sol.dtr[0] + pvt_sol.dtr[2]) * SPEED_OF_LIGHT_M_S, + pvt_sol.dtr[5]}; + + // current active channels + vtl_data->past_active_ch = vtl_data->active_ch; + + // execute VTL + vtl_Core->vtl_work(*vtl_data); + + + // 60s + 2s, output VTL estimation + vtl_output = (vtl_epoch == 20) ? d_conf.enable_pvt_output_vtl : vtl_output; // VTL output after 10 epochs + + vtl_epoch++; + } + if (vtl_output) + { + // replace rx pos, vel, clk b/d, with vtl data + const arma::colvec &rx_pvt = vtl_Core->get_rx_pvt(); + pvt_sol.rr[0] = rx_pvt[0]; + pvt_sol.rr[1] = rx_pvt[2]; + pvt_sol.rr[2] = rx_pvt[4]; + pvt_sol.rr[3] = rx_pvt[1]; + pvt_sol.rr[4] = rx_pvt[3]; + pvt_sol.rr[5] = rx_pvt[5]; + rx_position_and_time[3] = rx_pvt[d_rx_clk_b_idx] / SPEED_OF_LIGHT_M_S; + pvt_sol.dtr[5] = rx_pvt[d_rx_clk_d_idx]; + this->set_rx_pos({pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}); + } + // compute Ground speed and COG double ground_speed_ms = 0.0; std::array pos{}; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 57319db5a..73d855a1a 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -136,6 +136,7 @@ private: void check_has_orbit_clock_validity(const std::map& obs_map); void get_has_biases(const std::map& obs_map); void get_current_has_obs_correction(const std::string& signal, uint32_t tow_obs, int prn); + const Gnss_Synchro* get_synchro(const std::map& map, const Gnss_Synchro& freq1_synchro); std::array d_obs_data{}; std::array d_dop{}; @@ -164,6 +165,10 @@ private: // vector tracking std::unique_ptr vtl_data; std::unique_ptr vtl_Core; + uint32_t d_rx_clk_b_idx; + uint32_t d_rx_clk_d_idx; + int vtl_epoch; + bool vtl_output; }; diff --git a/src/algorithms/PVT/libs/vtl_core.cc b/src/algorithms/PVT/libs/vtl_core.cc index 7db4f1990..1e14fcef2 100755 --- a/src/algorithms/PVT/libs/vtl_core.cc +++ b/src/algorithms/PVT/libs/vtl_core.cc @@ -665,12 +665,6 @@ void Vtl_Core::saveVTLdata(const Vtl_Data &rtk_data) tmp_double = ekf_comp_Z(i + N_ch); dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } - // rtk code freq - for (int i = 0; i < N_ch; i++) - { - tmp_double = rtk_data.code_freq(i); - dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } // VTL code freq for (int i = 0; i < N_ch; i++) { diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 8528d9f3d..c21fe7974 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -409,7 +409,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 *obs_pr, double *tropo_m, double *iono_m, double *code_bias_m) { double r; double dion; @@ -443,6 +443,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, { vsat[i] = 0; azel[i * 2] = azel[1 + i * 2] = resp[i] = 0.0; + obs_pr[i] = 0; if (!(sys = satsys(obs[i].sat, nullptr))) { @@ -505,6 +506,23 @@ 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); + iono_m[i] = dion; + tropo_m[i] = dtrp; + int j; + if (obs[i].code[0] != CODE_NONE) + { + j = 0; + } + else if (obs[i].code[1] != CODE_NONE) + { + j = 1; + } + else + { + j = 2; + } + code_bias_m[i] = obs[i].P[j] - P; + obs_pr[i] = obs[i].P[j]; /* design matrix */ for (j = 0; j < NX; j++) @@ -684,7 +702,8 @@ arma::vec rough_bancroft(const arma::mat &B_pass) 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 &obs_pr_vec, std::vector &tropo_m_vec, + std::vector &iono_m_vec, std::vector &code_bias_m_vec) { double x[NX] = {0}; double dx[NX]; @@ -701,6 +720,14 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, int nv; int ns; char msg_aux[128]; + double *obs_pr; + double *tropo_m; + double *iono_m; + double *code_bias_m; + obs_pr = mat(n + 4, 1); + tropo_m = mat(n + 4, 1); + iono_m = mat(n + 4, 1); + code_bias_m = mat(n + 4, 1); trace(3, "estpos : n=%d\n", n); @@ -744,7 +771,7 @@ 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, obs_pr, tropo_m, iono_m, code_bias_m); if (nv < NX) { @@ -799,6 +826,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++) + { + obs_pr_vec.push_back(obs_pr[k]); + tropo_m_vec.push_back(tropo_m[k]); + iono_m_vec.push_back(iono_m[k]); + code_bias_m_vec.push_back(code_bias_m[k]); + } free(v); free(H); free(var); @@ -876,8 +910,12 @@ int raim_fde(const obsd_t *obs, int n, const double *rs, svh_e[k++] = svh[j]; } /* estimate receiver position without a satellite */ + std::vector obs_pr_vec; + std::vector tropo_m_vec; + std::vector iono_m_vec; + std::vector code_bias_m_vec; 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, obs_pr_vec, tropo_m_vec, iono_m_vec, code_bias_m_vec)) { trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); continue; @@ -1087,11 +1125,12 @@ 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 &obs_pr_vec, std::vector &tropo_m_vec, + std::vector &iono_m_vec, std::vector &code_bias_m_vec, double *rs, double *dts) { prcopt_t opt_ = *opt; - double *rs; - double *dts; + // double *rs; + // double *dts; double *var; double *azel_; double *resp; @@ -1113,8 +1152,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, sol->time = obs[0].time; msg[0] = '\0'; - rs = mat(6, n); - dts = mat(2, n); + // rs = mat(6, n); + // dts = mat(2, n); var = mat(1, n); azel_ = zeros(2, n); resp = mat(1, n); @@ -1131,7 +1170,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, obs_pr_vec, tropo_m_vec, + iono_m_vec, code_bias_m_vec); /* raim fde */ if (!stat && n >= 6 && opt->posopt[4]) @@ -1173,8 +1213,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, ssat[obs[i].sat - 1].resp[0] = resp[i]; } } - free(rs); - free(dts); + // free(rs); + // free(dts); free(var); free(azel_); free(resp); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index 735df2905..592110bcb 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,8 @@ 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 *obs_pr, double *tropo_m, double *iono_m, + double *code_bias_m); /* validate solution ---------------------------------------------------------*/ int valsol(const double *azel, const int *vsat, int n, @@ -103,7 +105,9 @@ 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 &obs_pr_vec, + std::vector &tropo_m_vec, std::vector &iono_m_vec, + std::vector &code_bias_m_vec); /* raim fde (failure detection and exclution) -------------------------------*/ int raim_fde(const obsd_t *obs, int n, const double *rs, @@ -140,6 +144,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 &obs_pr_vec, std::vector &tropo_m_vec, + std::vector &iono_m_vec, std::vector &code_bias_m_vec, + double *rs, double *dts); #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 136e9d0f8..8baf10ba4 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -2762,7 +2762,8 @@ 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 &obs_pr_vec, std::vector &tropo_m_vec, + std::vector &iono_m_vec, std::vector &code_bias_m_vec, double *rs, double *dts) { prcopt_t *opt = &rtk->opt; sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; @@ -2797,7 +2798,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, obs_pr_vec, tropo_m_vec, + iono_m_vec, code_bias_m_vec, rs, dts)) { errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) @@ -2839,7 +2841,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, obs_pr_vec, tropo_m_vec, + iono_m_vec, code_bias_m_vec, rs, dts)) { 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..6e85ef7e4 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 &obs_pr_vec, std::vector &tropo_m_vec, + std::vector &iono_m_vec, std::vector &code_bias_m_vec, + double *rs, double *dts); /** \} */ diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index 7593eb0bc..4b172c9b2 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -602,7 +602,15 @@ void *rtksvrthread(void *arg) } /* rtk positioning */ rtksvrlock(svr); - rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav); + std::vector obs_pr_vec; + std::vector tropo_m_vec; + std::vector iono_m_vec; + std::vector code_bias_m_vec; + double *rs; + double *dts; + rs = mat(6, 2); + dts = mat(2, 2); + rtkpos(&svr->rtk, obs.data, obs.n, &svr->nav, obs_pr_vec, tropo_m_vec, iono_m_vec, code_bias_m_vec, rs, dts); rtksvrunlock(svr); if (svr->rtk.sol.stat != SOLQ_NONE)