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

Use rtkpos instead of pntpos in rtklib_solver

Browse Source 
so more positioning modes are available
This commit is contained in:
Carles Fernandez 2017-05-02 19:31:38 +02:00
parent 9eee209f40
commit 902da19194
7 changed files with 36 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

@ -214,8 +214,6 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
std::string dump_ls_pvt_filename = dump_filename; std::string dump_ls_pvt_filename = dump_filename;
type_of_rx = type_of_receiver; type_of_rx = type_of_receiver;
rtklib_options = rtk.opt;
// GPS Ephemeris data message port in // GPS Ephemeris data message port in
this->message_port_register_in(pmt::mp("telemetry")); this->message_port_register_in(pmt::mp("telemetry"));
this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&rtklib_pvt_cc::msg_handler_telemetry, this, _1)); this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&rtklib_pvt_cc::msg_handler_telemetry, this, _1));
@ -278,7 +276,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
d_dump_filename.append("_raw.dat"); d_dump_filename.append("_raw.dat");
dump_ls_pvt_filename.append("_ls_pvt.dat"); dump_ls_pvt_filename.append("_ls_pvt.dat");
d_ls_pvt = std::make_shared<rtklib_solver>((int)nchannels, dump_ls_pvt_filename, d_dump, rtklib_options); d_ls_pvt = std::make_shared<rtklib_solver>((int)nchannels, dump_ls_pvt_filename, d_dump, rtk);
d_ls_pvt->set_averaging_depth(1); d_ls_pvt->set_averaging_depth(1);
d_rx_time = 0.0; d_rx_time = 0.0;

2
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@ -129,7 +129,7 @@ private:
double last_RINEX_obs_output_time; double last_RINEX_obs_output_time;
double last_RINEX_nav_output_time; double last_RINEX_nav_output_time;
std::shared_ptr<rtklib_solver> d_ls_pvt; std::shared_ptr<rtklib_solver> d_ls_pvt;
prcopt_t rtklib_options;
std::map<int,Gnss_Synchro> gnss_observables_map; std::map<int,Gnss_Synchro> gnss_observables_map;
bool observables_pairCompare_min(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b); bool observables_pairCompare_min(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b);

22
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -58,7 +58,7 @@
using google::LogMessage; using google::LogMessage;
rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, const prcopt_t & rtklib_opt) rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t & rtk)
{ {
// init empty ephemeris for all the available GNSS channels // init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels; d_nchannels = nchannels;
@ -66,10 +66,9 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
d_flag_dump_enabled = flag_dump_to_file; d_flag_dump_enabled = flag_dump_to_file;
count_valid_position = 0; count_valid_position = 0;
d_flag_averaging = false; d_flag_averaging = false;
rtk_ = rtk;
rtklib_options = rtklib_opt; pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0},
old_pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0},
'0', '0', '0', 0, 0, 0 }; '0', '0', '0', 0, 0, 0 };
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
@ -235,24 +234,25 @@ bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
nav_data.lam[i][1]=SPEED_OF_LIGHT/FREQ2; /* L2 */ nav_data.lam[i][1]=SPEED_OF_LIGHT/FREQ2; /* L2 */
} }
result=pntpos(obs_data, valid_obs, &nav_data, &rtklib_options, &old_pvt_sol, NULL, NULL,rtklib_msg); result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data);
if(result==0) if(result==0)
{ {
DLOG(INFO)<<"RTKLIB pntpos error message: "<<rtklib_msg; DLOG(INFO)<<"RTKLIB rtkpos error message: "<<rtk_.errbuf;
d_rx_dt_s = 0; //reset rx time estimation d_rx_dt_s = 0; //reset rx time estimation
}else{ }else{
pvt_sol=rtk_.sol;
b_valid_position=true; b_valid_position=true;
arma::vec rx_position_and_time(4); arma::vec rx_position_and_time(4);
rx_position_and_time(0)=old_pvt_sol.rr[0]; rx_position_and_time(0)=pvt_sol.rr[0];
rx_position_and_time(1)=old_pvt_sol.rr[1]; rx_position_and_time(1)=pvt_sol.rr[1];
rx_position_and_time(2)=old_pvt_sol.rr[2]; rx_position_and_time(2)=pvt_sol.rr[2];
rx_position_and_time(3)=old_pvt_sol.dtr[0]; rx_position_and_time(3)=pvt_sol.dtr[0];
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "Hybrid Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; DLOG(INFO) << "Hybrid Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
boost::posix_time::ptime p_time; boost::posix_time::ptime p_time;
gtime_t rtklib_utc_time=gpst2utc(old_pvt_sol.time); gtime_t rtklib_utc_time=gpst2utc(pvt_sol.time);
p_time=boost::posix_time::from_time_t(rtklib_utc_time.time); p_time=boost::posix_time::from_time_t(rtklib_utc_time.time);
p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec*1e6)); p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec*1e6));
d_position_UTC_time = p_time; d_position_UTC_time = p_time;

8
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -55,7 +55,7 @@
#include <iostream> #include <iostream>
#include <map> #include <map>
#include <string> #include <string>
#include "rtklib_pntpos.h" #include "rtklib_rtkpos.h"
#include "galileo_navigation_message.h" #include "galileo_navigation_message.h"
#include "gps_navigation_message.h" #include "gps_navigation_message.h"
#include "gps_cnav_navigation_message.h" #include "gps_cnav_navigation_message.h"
@ -70,7 +70,7 @@ class rtklib_solver : public Pvt_Solution
private: private:
public: public:
rtklib_solver(int nchannels,std::string dump_filename, bool flag_dump_to_file, const prcopt_t & rtklib_opt); rtklib_solver(int nchannels,std::string dump_filename, bool flag_dump_to_file, rtk_t & rtk);
~rtklib_solver(); ~rtklib_solver();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging); bool get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging);
@ -94,8 +94,8 @@ public:
bool d_flag_dump_enabled; bool d_flag_dump_enabled;
prcopt_t rtklib_options; sol_t pvt_sol;
sol_t old_pvt_sol; rtk_t rtk_;
std::string d_dump_filename; std::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;

1
src/algorithms/libs/rtklib/rtklib_conversions.cc
View File

@ -44,6 +44,7 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch
rtklib_obs.SNR[band] = CN0_dB_Hz; rtklib_obs.SNR[band] = CN0_dB_Hz;
rtklib_obs.sat = gnss_synchro.PRN; rtklib_obs.sat = gnss_synchro.PRN;
rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time); rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time);
rtklib_obs.rcv = 1;
//printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec); //printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec);
return rtklib_obs; return rtklib_obs;
} }

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

@ -422,7 +422,7 @@ double baseline(const double *ru, const double *rb, double *dr)
/* initialize state and covariance -------------------------------------------*/ /* initialize state and covariance -------------------------------------------*/
void initx(rtk_t *rtk, double xi, double var, int i) void initx_rtk(rtk_t *rtk, double xi, double var, int i)
{ {
int j; int j;
rtk->x[i] = xi; rtk->x[i] = xi;
@ -465,17 +465,17 @@ void udpos(rtk_t *rtk, double tt)
/* fixed mode */ /* fixed mode */
if (rtk->opt.mode == PMODE_FIXED) if (rtk->opt.mode == PMODE_FIXED)
{ {
for (i = 0;i<3;i++) initx(rtk, rtk->opt.ru[i], 1E-8, i); for (i = 0;i<3;i++) initx_rtk(rtk, rtk->opt.ru[i], 1E-8, i);
return; return;
} }
/* initialize position for first epoch */ /* initialize position for first epoch */
if (norm(rtk->x, 3) <= 0.0) if (norm(rtk->x, 3) <= 0.0)
{ {
for (i = 0;i<3;i++) initx(rtk, rtk->sol.rr[i], VAR_POS, i); for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i);
if (rtk->opt.dynamics) if (rtk->opt.dynamics)
{ {
for (i = 3;i<6;i++) initx(rtk, rtk->sol.rr[i], VAR_VEL, i); for (i = 3;i<6;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_VEL, i);
for (i = 6;i<9;i++) initx(rtk, 1E-6, VAR_ACC, i); for (i = 6;i<9;i++) initx_rtk(rtk, 1E-6, VAR_ACC, i);
} }
} }
/* static mode */ /* static mode */
@ -484,7 +484,7 @@ void udpos(rtk_t *rtk, double tt)
/* kinmatic mode without dynamics */ /* kinmatic mode without dynamics */
if (!rtk->opt.dynamics) if (!rtk->opt.dynamics)
{ {
for (i = 0;i<3;i++) initx(rtk, rtk->sol.rr[i], VAR_POS, i); for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i);
return; return;
} }
/* check variance of estimated postion */ /* check variance of estimated postion */
@ -493,9 +493,9 @@ void udpos(rtk_t *rtk, double tt)
if (var>VAR_POS) if (var>VAR_POS)
{ {
/* reset position with large variance */ /* reset position with large variance */
for (i = 0;i<3;i++) initx(rtk, rtk->sol.rr[i], VAR_POS, i); for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i);
for (i = 3;i<6;i++) initx(rtk, rtk->sol.rr[i], VAR_VEL, i); for (i = 3;i<6;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_VEL, i);
for (i = 6;i<9;i++) initx(rtk, 1E-6, VAR_ACC, i); for (i = 6;i<9;i++) initx_rtk(rtk, 1E-6, VAR_ACC, i);
trace(2, "reset rtk position due to large variance: var=%.3f\n", var); trace(2, "reset rtk position due to large variance: var=%.3f\n", var);
return; return;
} }
@ -545,7 +545,7 @@ void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns)
if (rtk->x[j] == 0.0) if (rtk->x[j] == 0.0)
{ {
initx(rtk, 1E-6, std::pow(rtk->opt.std[1]*bl/1e4, 2.0), j); initx_rtk(rtk, 1E-6, std::pow(rtk->opt.std[1]*bl/1e4, 2.0), j);
} }
else else
{ {
@ -571,11 +571,11 @@ void udtrop(rtk_t *rtk, double tt, double bl __attribute((unused)))
if (rtk->x[j] == 0.0) if (rtk->x[j] == 0.0)
{ {
initx(rtk, INIT_ZWD, std::pow(rtk->opt.std[2], 2.0), j); /* initial zwd */ initx_rtk(rtk, INIT_ZWD, std::pow(rtk->opt.std[2], 2.0), j); /* initial zwd */
if (rtk->opt.tropopt >= TROPOPT_ESTG) if (rtk->opt.tropopt >= TROPOPT_ESTG)
{ {
for (k = 0;k<2;k++) initx(rtk, 1e-6, VAR_GRA, ++j); for (k = 0;k<2;k++) initx_rtk(rtk, 1e-6, VAR_GRA, ++j);
} }
} }
else else
@ -607,12 +607,12 @@ void udrcvbias(rtk_t *rtk, double tt)
if (rtk->x[j] == 0.0) if (rtk->x[j] == 0.0)
{ {
initx(rtk, 1E-6, VAR_HWBIAS, j); initx_rtk(rtk, 1E-6, VAR_HWBIAS, j);
} }
/* hold to fixed solution */ /* hold to fixed solution */
else if (rtk->nfix >= rtk->opt.minfix && rtk->sol.ratio>rtk->opt.thresar[0]) else if (rtk->nfix >= rtk->opt.minfix && rtk->sol.ratio>rtk->opt.thresar[0])
{ {
initx(rtk, rtk->xa[j], rtk->Pa[j+j*rtk->na], j); initx_rtk(rtk, rtk->xa[j], rtk->Pa[j+j*rtk->na], j);
} }
else else
{ {
@ -801,11 +801,11 @@ void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
if (rtk->opt.modear == ARMODE_INST && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) if (rtk->opt.modear == ARMODE_INST && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0)
{ {
initx(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt)); initx_rtk(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt));
} }
else if (reset && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) else if (reset && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0)
{ {
initx(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt)); initx_rtk(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt));
trace(3, "udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n", trace(3, "udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n",
i, f+1, rtk->ssat[i-1].outc[f]); i, f+1, rtk->ssat[i-1].outc[f]);
} }
@ -871,7 +871,7 @@ void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
for (i = 0;i<ns;i++) for (i = 0;i<ns;i++)
{ {
if (bias[i] == 0.0 || rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) continue; if (bias[i] == 0.0 || rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) continue;
initx(rtk, bias[i], std::pow(rtk->opt.std[0], 2.0), IB_RTK(sat[i], f, &rtk->opt)); initx_rtk(rtk, bias[i], std::pow(rtk->opt.std[0], 2.0), IB_RTK(sat[i], f, &rtk->opt));
} }
free(bias); free(bias);
} }

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

@ -128,7 +128,7 @@ double varerr(int sat, int sys, double el, double bl, double dt, int f,
double baseline(const double *ru, const double *rb, double *dr); double baseline(const double *ru, const double *rb, double *dr);
void initx(rtk_t *rtk, double xi, double var, int i); void initx_rtk(rtk_t *rtk, double xi, double var, int i);
int selsat(const obsd_t *obs, double *azel, int nu, int nr, int selsat(const obsd_t *obs, double *azel, int nu, int nr,
const prcopt_t *opt, int *sat, int *iu, int *ir); const prcopt_t *opt, int *sat, int *iu, int *ir);