mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-19 16:24:58 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://gitlab.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-06-06 17:28:58 +02:00
commit efd52be704
23 changed files with 374 additions and 304 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
conf/gnss-sdr_Hybrid_byte.conf
View File

@ -14,7 +14,7 @@ GNSS-SDR.internal_fs_sps=20000000
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=File_Signal_Source
SignalSource.filename=/datalogger/signals/Fraunhofer/L125_III1b_210s_L1.bin ; <- PUT YOUR FILE HERE
SignalSource.filename=/media/javier/Extreme 500/fraunhofer/L125_III1b_210s_L1.bin ; <- PUT YOUR FILE HERE
SignalSource.item_type=byte
SignalSource.sampling_frequency=20000000
SignalSource.samples=0
@ -46,12 +46,20 @@ Resampler.dump_filename=../data/resampler.dat
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=8
Channels_1B.count=8
Channels_1C.count=10
Channels_1B.count=10
Channels.in_acquisition=1
;#signal:
;# "1C" GPS L1 C/A
;# "1B" GALILEO E1 B (I/NAV OS/CS/SoL)
;# "1G" GLONASS L1 C/A
;# "2S" GPS L2 L2C (M)
;# "5X" GALILEO E5a I+Q
;# "L5" GPS L5
;#if the option is disabled by default is assigned "1C" GPS L1 C/A
Channel0.signal=1C
Channel1.signal=1C
Channel2.signal=1C
Channel3.signal=1C
@ -67,16 +75,19 @@ Channel12.signal=1B
Channel13.signal=1B
Channel14.signal=1B
Channel15.signal=1B
Channel16.signal=1B
Channel17.signal=1B
Channel18.signal=1B
Channel19.signal=1B
;######### GPS ACQUISITION CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.coherent_integration_time_ms=1
Acquisition_1C.threshold=0.0060
;Acquisition_1C.pfa=0.01
Acquisition_1C.doppler_max=10000
Acquisition_1C.doppler_step=500
Acquisition_1C.threshold=18
Acquisition_1C.use_CFAR_algorithm=false
Acquisition_1C.blocking=true
Acquisition_1C.doppler_max=5000
Acquisition_1C.doppler_step=250
Acquisition_1C.dump=false
Acquisition_1C.dump_filename=./acq_dump.dat
@ -84,21 +95,23 @@ Acquisition_1C.dump_filename=./acq_dump.dat
;######### GALILEO ACQUISITION CONFIG ############
Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition
Acquisition_1B.item_type=gr_complex
Acquisition_1B.coherent_integration_time_ms=4
;Acquisition_1B.threshold=0
Acquisition_1B.pfa=0.0000008
Acquisition_1B.doppler_max=15000
Acquisition_1B.threshold=25
Acquisition_1B.use_CFAR_algorithm=false
Acquisition_1B.blocking=true
Acquisition_1B.doppler_max=5000
Acquisition_1B.doppler_step=125
Acquisition_1B.dump=false
Acquisition_1B.dump_filename=./acq_dump.dat
;######### TRACKING GPS CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking
Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.pll_bw_hz=45.0;
Tracking_1C.dll_bw_hz=4.0;
Tracking_1C.order=3;
Tracking_1C.extend_correlation_ms=1
Tracking_1C.pll_bw_hz=40;
Tracking_1C.pll_bw_narrow_hz=30;
Tracking_1C.dll_bw_hz=2.0;
Tracking_1C.dll_bw_narrow_hz=1.5;
Tracking_1C.order=2;
Tracking_1C.dump=false
Tracking_1C.dump_filename=../data/epl_tracking_ch_
@ -107,7 +120,7 @@ Tracking_1C.dump_filename=../data/epl_tracking_ch_
Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking
Tracking_1B.item_type=gr_complex
Tracking_1B.pll_bw_hz=15.0;
Tracking_1B.dll_bw_hz=2.0;
Tracking_1B.dll_bw_hz=3.0;
Tracking_1B.order=3;
Tracking_1B.early_late_space_chips=0.15;
Tracking_1B.very_early_late_space_chips=0.6;
@ -126,6 +139,7 @@ TelemetryDecoder_1B.dump=false
;######### OBSERVABLES CONFIG ############
;#implementation:
Observables.implementation=Hybrid_Observables
Observables.dump=false
Observables.dump_filename=./observables.dat
@ -133,13 +147,14 @@ Observables.dump_filename=./observables.dat
;######### PVT CONFIG ############
PVT.implementation=RTKLIB_PVT
PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX
PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
PVT.output_rate_ms=100;
PVT.output_rate_ms=10;
PVT.display_rate_ms=500;
PVT.dump=false
PVT.elevation_mask=15;
PVT.flag_rtcm_server=false
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1
PVT.dump=false
PVT.dump_filename=./PVT

42
conf/gnss-sdr_Hybrid_nsr.conf
View File

@ -14,7 +14,7 @@ GNSS-SDR.internal_fs_sps=2560000
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=Nsr_File_Signal_Source
SignalSource.filename=/media/javier/SISTEMA/signals/ifen/E1L1_FE0_Band0.stream ; <- PUT YOUR FILE HERE
SignalSource.filename=/home/javier/signals/ifen/E1L1_FE0_Band0.stream ; <- PUT YOUR FILE HERE
SignalSource.item_type=byte
SignalSource.sampling_frequency=20480000
SignalSource.samples=0
@ -61,8 +61,8 @@ InputFilter.dump_filename=../data/input_filter.dat
Resampler.implementation=Pass_Through
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=8
Channels_1B.count=0
Channels_1C.count=10
Channels_1B.count=10
Channels.in_acquisition=1
;#signal:
@ -82,22 +82,26 @@ Channel4.signal=1C
Channel5.signal=1C
Channel6.signal=1C
Channel7.signal=1C
Channel8.signal=1B
Channel9.signal=1B
Channel8.signal=1C
Channel9.signal=1C
Channel10.signal=1B
Channel11.signal=1B
Channel12.signal=1B
Channel13.signal=1B
Channel14.signal=1B
Channel15.signal=1B
Channel16.signal=1B
Channel17signal=1B
Channel18.signal=1B
Channel19.signal=1B
;######### GPS ACQUISITION CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.coherent_integration_time_ms=1
Acquisition_1C.threshold=0.0075
;Acquisition_1C.pfa=0.01
Acquisition_1C.threshold=25
Acquisition_1C.use_CFAR_algorithm=false
Acquisition_1C.blocking=true
Acquisition_1C.doppler_max=5000
Acquisition_1C.doppler_step=250
Acquisition_1C.dump=false
@ -107,32 +111,31 @@ Acquisition_1C.dump_filename=./acq_dump.dat
;######### GALILEO ACQUISITION CONFIG ############
Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition
Acquisition_1B.item_type=gr_complex
Acquisition_1B.coherent_integration_time_ms=4
;Acquisition_1B.threshold=0
Acquisition_1B.pfa=0.0000002
Acquisition_1B.doppler_max=15000
Acquisition_1B.doppler_step=125
Acquisition_1B.threshold=25
Acquisition_1B.use_CFAR_algorithm=false
Acquisition_1B.blocking=true
Acquisition_1B.doppler_max=5000
Acquisition_1B.doppler_step=250
Acquisition_1B.dump=false
Acquisition_1B.dump_filename=./acq_dump.dat
;######### TRACKING GPS CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.extend_correlation_ms=1
Tracking_1C.pll_bw_hz=40;
Tracking_1C.pll_bw_narrow_hz=20;
Tracking_1C.pll_bw_narrow_hz=30;
Tracking_1C.dll_bw_hz=2.0;
Tracking_1C.dll_bw_narrow_hz=1.0;
Tracking_1C.order=3;
Tracking_1C.dump=true
Tracking_1C.dll_bw_narrow_hz=1.5;
Tracking_1C.order=2;
Tracking_1C.dump=false
Tracking_1C.dump_filename=../data/epl_tracking_ch_
;######### TRACKING GALILEO CONFIG ############
Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking
Tracking_1B.item_type=gr_complex
Tracking_1B.pll_bw_hz=15.0;
Tracking_1B.pll_bw_hz=20.0;
Tracking_1B.dll_bw_hz=2.0;
Tracking_1B.order=3;
Tracking_1B.early_late_space_chips=0.15;
@ -165,6 +168,7 @@ PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate
PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
PVT.output_rate_ms=10;
PVT.display_rate_ms=500;
PVT.elevation_mask=20;
PVT.flag_rtcm_server=false
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1

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

@ -94,6 +94,8 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
{
rinex_version = 2;
}
int rinexobs_rate_ms = boost::math::lcm(configuration->property(role + ".rinexobs_rate_ms", 1000), output_rate_ms);
int rinexnav_rate_ms = boost::math::lcm(configuration->property(role + ".rinexnav_rate_ms", 6000), output_rate_ms);
// RTCM Printer settings
bool flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false);
@ -471,10 +473,10 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
0, /* initialize by restart */
1, /* output single by dgps/float/fix/ppp outage */
{"", ""}, /* char rnxopt[2][256] rinex options {rover,base} */
{sat_PCV, rec_PCV, phwindup, reject_GPS_IIA, raim_fde}, /* posopt[6] positioning options [0]: satellite and receiver antenna PCV model; [1]: interpolate antenna parameters; [2]: apply phase wind-up correction for PPP modes; [3]: exclude measurements of GPS Block IIA satellites satellite [4]: RAIM FDE (fault detection and exclusion) [5]: handle day-boundary clock jump */
{sat_PCV, rec_PCV, phwindup, reject_GPS_IIA, raim_fde}, /* posopt[6] positioning options [0]: satellite and receiver antenna PCV model; [1]: interpolate antenna parameters; [2]: apply phase wind-up correction for PPP modes; [3]: exclude measurements of GPS Block IIA satellites satellite [4]: RAIM FDE (fault detection and exclusion) [5]: handle day-boundary clock jump */
0, /* solution sync mode (0:off,1:on) */
{{}, {}}, /* odisp[2][6*11] ocean tide loading parameters {rov,base} */
{{}, {{}, {}}, {{}, {}}, {}, {}}, /* exterr_t exterr extended receiver error model */
{{}, {}}, /* odisp[2][6*11] ocean tide loading parameters {rov,base} */
{{}, {{}, {}}, {{}, {}}, {}, {}}, /* exterr_t exterr extended receiver error model */
0, /* disable L2-AR */
{} /* char pppopt[256] ppp option "-GAP_RESION=" default gap to reset iono parameters (ep) */
};
@ -482,7 +484,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
rtkinit(&rtk, &rtklib_configuration_options);
// make PVT object
pvt_ = rtklib_make_pvt_cc(in_streams_, dump_, dump_filename_, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, rinex_version, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname, type_of_receiver, rtk);
pvt_ = rtklib_make_pvt_cc(in_streams_, dump_, dump_filename_, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, rinex_version, rinexobs_rate_ms, rinexnav_rate_ms, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname, type_of_receiver, rtk);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";
if (out_streams_ > 0)
{

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

@ -57,6 +57,8 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels,
std::string nmea_dump_filename,
std::string nmea_dump_devname,
int rinex_version,
int rinexobs_rate_ms,
int rinexnav_rate_ms,
bool flag_rtcm_server,
bool flag_rtcm_tty_port,
unsigned short rtcm_tcp_port,
@ -75,6 +77,8 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels,
nmea_dump_filename,
nmea_dump_devname,
rinex_version,
rinexobs_rate_ms,
rinexnav_rate_ms,
flag_rtcm_server,
flag_rtcm_tty_port,
rtcm_tcp_port,
@ -90,7 +94,7 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg)
{
try
{
//************* GPS telemetry *****************
// ************* GPS telemetry *****************
if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Ephemeris>))
{
// ### GPS EPHEMERIS ###
@ -146,7 +150,7 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg)
DLOG(INFO) << "New CNAV UTC record has arrived ";
}
//**************** Galileo telemetry ********************
// **************** Galileo telemetry ********************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Ephemeris>))
{
// ### Galileo EPHEMERIS ###
@ -185,7 +189,7 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg)
DLOG(INFO) << "New Galileo Almanac has arrived ";
}
//**************** GLONASS GNAV Telemetry **************************
// **************** GLONASS GNAV Telemetry **************************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>))
{
// ### GLONASS GNAV EPHEMERIS ###
@ -235,13 +239,27 @@ std::map<int, Gps_Ephemeris> rtklib_pvt_cc::get_GPS_L1_ephemeris_map()
}
rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump_filename,
int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port,
std::string nmea_dump_filename, std::string nmea_dump_devname, int rinex_version,
bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port,
unsigned short rtcm_station_id, std::map<int, int> rtcm_msg_rate_ms, std::string rtcm_dump_devname, const unsigned int type_of_receiver, rtk_t& rtk) : gr::sync_block("rtklib_pvt_cc",
gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
gr::io_signature::make(0, 0, 0))
rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels,
bool dump,
std::string dump_filename,
int output_rate_ms,
int display_rate_ms,
bool flag_nmea_tty_port,
std::string nmea_dump_filename,
std::string nmea_dump_devname,
int rinex_version,
int rinexobs_rate_ms,
int rinexnav_rate_ms,
bool flag_rtcm_server,
bool flag_rtcm_tty_port,
unsigned short rtcm_tcp_port,
unsigned short rtcm_station_id,
std::map<int, int> rtcm_msg_rate_ms,
std::string rtcm_dump_devname,
const unsigned int type_of_receiver,
rtk_t& rtk) : gr::sync_block("rtklib_pvt_cc",
gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
gr::io_signature::make(0, 0, 0))
{
d_output_rate_ms = output_rate_ms;
d_display_rate_ms = display_rate_ms;
@ -255,28 +273,28 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
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));
//initialize kml_printer
// initialize kml_printer
std::string kml_dump_filename;
kml_dump_filename = d_dump_filename;
d_kml_dump = std::make_shared<Kml_Printer>();
d_kml_dump->set_headers(kml_dump_filename);
//initialize gpx_printer
// initialize gpx_printer
std::string gpx_dump_filename;
gpx_dump_filename = d_dump_filename;
d_gpx_dump = std::make_shared<Gpx_Printer>();
d_gpx_dump->set_headers(gpx_dump_filename);
//initialize geojson_printer
// initialize geojson_printer
std::string geojson_dump_filename;
geojson_dump_filename = d_dump_filename;
d_geojson_printer = std::make_shared<GeoJSON_Printer>();
d_geojson_printer->set_headers(geojson_dump_filename);
//initialize nmea_printer
// initialize nmea_printer
d_nmea_printer = std::make_shared<Nmea_Printer>(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname);
//initialize rtcm_printer
// initialize rtcm_printer
std::string rtcm_dump_filename;
rtcm_dump_filename = d_dump_filename;
d_rtcm_printer = std::make_shared<Rtcm_Printer>(rtcm_dump_filename, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_dump_devname);
@ -332,6 +350,14 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
}
b_rtcm_writing_started = false;
// initialize RINEX printer
b_rinex_header_written = false;
b_rinex_header_updated = false;
d_rinex_version = rinex_version;
rp = std::make_shared<Rinex_Printer>(d_rinex_version);
d_rinexobs_rate_ms = rinexobs_rate_ms;
d_rinexnav_rate_ms = rinexnav_rate_ms;
d_dump_filename.append("_raw.dat");
dump_ls_pvt_filename.append("_ls_pvt.dat");
@ -339,21 +365,6 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
d_ls_pvt->set_averaging_depth(1);
d_rx_time = 0.0;
last_pvt_display_T_rx_s = 0.0;
last_RTCM_1019_output_time = 0.0;
last_RTCM_1020_output_time = 0.0;
last_RTCM_1045_output_time = 0.0;
last_RTCM_1077_output_time = 0.0;
last_RTCM_1087_output_time = 0.0;
last_RTCM_1097_output_time = 0.0;
last_RTCM_MSM_output_time = 0.0;
last_RINEX_obs_output_time = 0.0;
last_RINEX_nav_output_time = 0.0;
b_rinex_header_written = false;
b_rinex_header_updated = false;
d_rinex_version = rinex_version;
rp = std::make_shared<Rinex_Printer>(d_rinex_version);
d_last_status_print_seg = 0;
@ -392,7 +403,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
{
msgctl(sysv_msqid, IPC_RMID, NULL);
//save GPS L2CM ephemeris to XML file
// save GPS L2CM ephemeris to XML file
std::string file_name = "eph_GPS_CNAV.xml";
if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0)
@ -415,7 +426,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
LOG(WARNING) << "Failed to save GPS L2CM or L5 Ephemeris, map is empty";
}
//save GPS L1 CA ephemeris to XML file
// save GPS L1 CA ephemeris to XML file
file_name = "eph_GPS_L1CA.xml";
if (d_ls_pvt->gps_ephemeris_map.size() > 0)
@ -438,7 +449,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
LOG(WARNING) << "Failed to save GPS L1 CA Ephemeris, map is empty";
}
//save Galileo E1 ephemeris to XML file
// save Galileo E1 ephemeris to XML file
file_name = "eph_Galileo_E1.xml";
if (d_ls_pvt->galileo_ephemeris_map.size() > 0)
@ -461,7 +472,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty";
}
//save GLONASS GNAV ephemeris to XML file
// save GLONASS GNAV ephemeris to XML file
file_name = "eph_GLONASS_GNAV.xml";
if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0)
@ -483,6 +494,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
{
LOG(WARNING) << "Failed to save GLONASS GNAV Ephemeris, map is empty";
}
if (d_dump_file.is_open() == true)
{
try
@ -505,15 +517,15 @@ bool rtklib_pvt_cc::observables_pairCompare_min(const std::pair<int, Gnss_Synchr
bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff)
{
/* Fill Sys V message structures */
// Fill Sys V message structures
int msgsend_size;
ttff_msgbuf msg;
msg.ttff = ttff.ttff;
msgsend_size = sizeof(msg.ttff);
msg.mtype = 1; /* default message ID */
msg.mtype = 1; // default message ID
/* SEND SOLUTION OVER A MESSAGE QUEUE */
/* non-blocking Sys V message send */
// SEND SOLUTION OVER A MESSAGE QUEUE
// non-blocking Sys V message send
msgsnd(sysv_msqid, &msg, msgsend_size, IPC_NOWAIT);
return true;
}
@ -601,75 +613,73 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
if (gnss_observables_map.size() > 0)
{
double current_RX_time = gnss_observables_map.begin()->second.RX_time;
if (std::fabs(current_RX_time - d_rx_time) * 1000.0 >= static_cast<double>(d_output_rate_ms))
unsigned int current_RX_time_ms = static_cast<unsigned int>(current_RX_time * 1000.0);
if (current_RX_time_ms % d_output_rate_ms == 0)
{
flag_compute_pvt_output = true;
d_rx_time = current_RX_time;
// std::cout.precision(17);
// std::cout << "current_RX_time: " << current_RX_time << " map time: " << gnss_observables_map.begin()->second.RX_time << std::endl;
}
// compute on the fly PVT solution
if (flag_compute_pvt_output == true)
{
if (d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false))
// receiver clock correction is disabled to be coherent with the RINEX and RTCM standard
// std::cout << TEXT_RED << "(internal) accumulated RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]" << TEXT_RESET << std::endl;
// for (std::map<int, Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
// {
// todo: check if it has effect to correct the receiver time for the internal pvt solution
// take into account that the RINEX obs with the RX time (integer ms) CAN NOT be corrected to keep the coherence in obs time
// it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s;
// }
if (d_ls_pvt->get_PVT(gnss_observables_map, false))
{
if (std::fabs(current_RX_time - last_pvt_display_T_rx_s) * 1000.0 >= static_cast<double>(d_display_rate_ms))
if (current_RX_time_ms % d_display_rate_ms == 0)
{
flag_display_pvt = true;
last_pvt_display_T_rx_s = current_RX_time;
}
if ((std::fabs(current_RX_time - last_RTCM_1019_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1019_rate_ms)) and (d_rtcm_MT1019_rate_ms != 0)) // allows deactivating messages by setting rate = 0
if (current_RX_time_ms % d_rtcm_MT1019_rate_ms == 0 and d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0
{
flag_write_RTCM_1019_output = true;
last_RTCM_1019_output_time = current_RX_time;
}
if ((std::fabs(current_RX_time - last_RTCM_1020_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1020_rate_ms)) and (d_rtcm_MT1020_rate_ms != 0)) // allows deactivating messages by setting rate = 0
if (current_RX_time_ms % d_rtcm_MT1020_rate_ms == 0 and d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0
{
flag_write_RTCM_1020_output = true;
last_RTCM_1020_output_time = current_RX_time;
}
if ((std::fabs(current_RX_time - last_RTCM_1045_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1045_rate_ms)) and (d_rtcm_MT1045_rate_ms != 0))
if (current_RX_time_ms % d_rtcm_MT1045_rate_ms == 0 and d_rtcm_MT1045_rate_ms != 0)
{
flag_write_RTCM_1045_output = true;
last_RTCM_1045_output_time = current_RX_time;
}
// TODO: RTCM 1077, 1087 and 1097 are not used, so, disable the output rates
// if (current_RX_time_ms % d_rtcm_MT1077_rate_ms==0 and d_rtcm_MT1077_rate_ms != 0)
// {
// last_RTCM_1077_output_time = current_RX_time;
// }
// if (current_RX_time_ms % d_rtcm_MT1087_rate_ms==0 and d_rtcm_MT1087_rate_ms != 0)
// {
// last_RTCM_1087_output_time = current_RX_time;
// }
// if (current_RX_time_ms % d_rtcm_MT1097_rate_ms==0 and d_rtcm_MT1097_rate_ms != 0)
// {
// last_RTCM_1097_output_time = current_RX_time;
// }
if ((std::fabs(current_RX_time - last_RTCM_1077_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1077_rate_ms)) and (d_rtcm_MT1077_rate_ms != 0))
{
last_RTCM_1077_output_time = current_RX_time;
}
if ((std::fabs(current_RX_time - last_RTCM_1087_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1087_rate_ms)) and (d_rtcm_MT1087_rate_ms != 0))
{
last_RTCM_1087_output_time = current_RX_time;
}
if ((std::fabs(current_RX_time - last_RTCM_1097_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MT1097_rate_ms)) and (d_rtcm_MT1097_rate_ms != 0))
{
last_RTCM_1097_output_time = current_RX_time;
}
if ((std::fabs(current_RX_time - last_RTCM_MSM_output_time) * 1000.0 >= static_cast<double>(d_rtcm_MSM_rate_ms)) and (d_rtcm_MSM_rate_ms != 0))
if (current_RX_time_ms % d_rtcm_MSM_rate_ms == 0 and d_rtcm_MSM_rate_ms != 0)
{
flag_write_RTCM_MSM_output = true;
last_RTCM_MSM_output_time = current_RX_time;
}
if ((std::fabs(current_RX_time - last_RINEX_obs_output_time) >= 1.0)) // TODO: Make it configurable
if (current_RX_time_ms % static_cast<unsigned int>(d_rinexobs_rate_ms) == 0)
{
flag_write_RINEX_obs_output = true;
last_RINEX_obs_output_time = current_RX_time;
}
if ((std::fabs(current_RX_time - last_RINEX_nav_output_time) >= 6.0)) // TODO: Make it configurable
if (current_RX_time_ms % static_cast<unsigned int>(d_rinexnav_rate_ms) == 0)
{
flag_write_RINEX_nav_output = true;
last_RINEX_nav_output_time = current_RX_time;
}
// correct the observable to account for the receiver clock offset
for (std::map<int, Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
{
it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s;
}
if (first_fix == true)
{
std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
@ -1523,8 +1533,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
}
if (flag_write_RTCM_MSM_output == true)
{
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
unsigned int i = 0;
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
{
@ -1588,8 +1598,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
}
if (flag_write_RTCM_MSM_output == true)
{
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
unsigned int i = 0;
for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++)
{
@ -1653,8 +1663,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
}
if (flag_write_RTCM_MSM_output == true)
{
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
unsigned int i = 0;
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
{
@ -1840,8 +1850,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
}
}
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
unsigned int i = 0;
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
{
@ -1955,8 +1965,8 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
}
}
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
unsigned int i = 0;
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
{
@ -2072,18 +2082,31 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
// DEBUG MESSAGE: Display position in console output
if (d_ls_pvt->is_valid_position() and flag_display_pvt)
{
std::cout << TEXT_BOLD_GREEN << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
<< " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl;
std::streamsize ss = std::cout.precision(); // save current precision
std::cout << TEXT_BOLD_GREEN
<< "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using " << d_ls_pvt->get_num_valid_observations()
<< std::setprecision(10)
<< " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
<< std::setprecision(4)
<< " [deg], Height = " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl;
std::cout << std::setprecision(ss);
LOG(INFO) << "RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]";
// boost::posix_time::ptime p_time;
// gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_ls_pvt->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_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));
// std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time) << TEXT_RESET << std::endl;
LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
<< " [deg], Height= " << d_ls_pvt->get_height() << " [m]";
<< " [deg], Height = " << d_ls_pvt->get_height() << " [m]";
/* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using "<< d_ls_pvt->get_num_valid_observations()<<" observations is HDOP = " << d_ls_pvt->get_HDOP() << " VDOP = "
<< d_ls_pvt->get_VDOP() <<" TDOP = " << d_ls_pvt->get_TDOP()
<< " GDOP = " << d_ls_pvt->get_GDOP() << std::endl; */
<< " UTC using "<< d_ls_pvt->get_num_valid_observations() <<" observations is HDOP = " << d_ls_pvt->get_hdop() << " VDOP = "
<< d_ls_pvt->get_vdop()
<< " GDOP = " << d_ls_pvt->get_gdop() << std::endl; */
}
// MULTIPLEXED FILE RECORDING - Record results to file

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

@ -62,6 +62,8 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int n_channels,
std::string nmea_dump_filename,
std::string nmea_dump_devname,
int rinex_version,
int rinexobs_rate_ms,
int rinexnav_rate_ms,
bool flag_rtcm_server,
bool flag_rtcm_tty_port,
unsigned short rtcm_tcp_port,
@ -86,6 +88,8 @@ private:
std::string nmea_dump_filename,
std::string nmea_dump_devname,
int rinex_version,
int rinexobs_rate_ms,
int rinexnav_rate_ms,
bool flag_rtcm_server,
bool flag_rtcm_tty_port,
unsigned short rtcm_tcp_port,
@ -101,6 +105,9 @@ private:
bool b_rinex_header_written;
bool b_rinex_header_updated;
double d_rinex_version;
int d_rinexobs_rate_ms;
int d_rinexnav_rate_ms;
bool b_rtcm_writing_started;
int d_rtcm_MT1045_rate_ms; //!< Galileo Broadcast Ephemeris
int d_rtcm_MT1019_rate_ms; //!< GPS Broadcast Ephemeris (orbits)
@ -126,16 +133,7 @@ private:
std::shared_ptr<GeoJSON_Printer> d_geojson_printer;
std::shared_ptr<Rtcm_Printer> d_rtcm_printer;
double d_rx_time;
double last_pvt_display_T_rx_s;
double last_RTCM_1019_output_time;
double last_RTCM_1020_output_time;
double last_RTCM_1045_output_time;
double last_RTCM_1077_output_time;
double last_RTCM_1087_output_time;
double last_RTCM_1097_output_time;
double last_RTCM_MSM_output_time;
double last_RINEX_obs_output_time;
double last_RINEX_nav_output_time;
std::shared_ptr<rtklib_solver> d_ls_pvt;
std::map<int, Gnss_Synchro> gnss_observables_map;
@ -163,6 +161,8 @@ public:
std::string nmea_dump_filename,
std::string nmea_dump_devname,
int rinex_version,
int rinexobs_rate_ms,
int rinexnav_rate_ms,
bool flag_rtcm_server,
bool flag_rtcm_tty_port,
unsigned short rtcm_tcp_port,

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

@ -133,7 +133,7 @@ double rtklib_solver::get_vdop() const
}
bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double Rx_time, bool flag_averaging)
bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging)
{
std::map<int, Gnss_Synchro>::const_iterator gnss_observables_iter;
std::map<int, Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
@ -147,8 +147,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
// ********************************************************************************
// ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************
// ********************************************************************************
int valid_obs = 0; //valid observations counter
int glo_valid_obs = 0; //GLONASS L1/L2 valid observations counter
int valid_obs = 0; // valid observations counter
int glo_valid_obs = 0; // GLONASS L1/L2 valid observations counter
obsd_t obs_data[MAXOBS];
eph_t eph_data[MAXOBS];
@ -156,7 +156,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
for (gnss_observables_iter = gnss_observables_map.cbegin();
gnss_observables_iter != gnss_observables_map.cend();
gnss_observables_iter++) //CHECK INCONSISTENCY when combining GLONASS + other system
gnss_observables_iter++) // CHECK INCONSISTENCY when combining GLONASS + other system
{
switch (gnss_observables_iter->second.System)
{
@ -170,9 +170,9 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
{
//convert ephemeris from GNSS-SDR class to RTKLIB structure
// convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure
// convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second,
@ -201,17 +201,17 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs],
gnss_observables_iter->second,
galileo_ephemeris_iter->second.WN_5,
2); //Band 3 (L5/E5)
2); // Band 3 (L5/E5)
found_E1_obs = true;
break;
}
}
if (!found_E1_obs)
{
//insert Galileo E5 obs as new obs and also insert its ephemeris
//convert ephemeris from GNSS-SDR class to RTKLIB structure
// insert Galileo E5 obs as new obs and also insert its ephemeris
// convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure
// convert observation from GNSS-SDR class to RTKLIB structure
unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_},
@ -219,7 +219,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second,
galileo_ephemeris_iter->second.WN_5,
2); //Band 3 (L5/E5)
2); // Band 3 (L5/E5)
valid_obs++;
}
}
@ -240,9 +240,9 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.cend())
{
//convert ephemeris from GNSS-SDR class to RTKLIB structure
// convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure
// convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second,
@ -255,7 +255,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
}
}
//GPS L2
// GPS L2
if (sig_.compare("2S") == 0)
{
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
@ -276,7 +276,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs],
gnss_observables_iter->second,
eph_data[i].week,
1); //Band 2 (L2)
1); // Band 2 (L2)
break;
}
}
@ -285,9 +285,9 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
else
{
// 3. If not found, insert the GPS L2 ephemeris and the observation
//convert ephemeris from GNSS-SDR class to RTKLIB structure
// convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure
// convert observation from GNSS-SDR class to RTKLIB structure
unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_},
@ -295,7 +295,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second,
gps_cnav_ephemeris_iter->second.i_GPS_week,
1); //Band 2 (L2)
1); // Band 2 (L2)
valid_obs++;
}
}
@ -304,7 +304,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
}
}
//GPS L5
// GPS L5
if (sig_.compare("L5") == 0)
{
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
@ -324,7 +324,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i],
gnss_observables_iter->second,
gps_cnav_ephemeris_iter->second.i_GPS_week,
2); //Band 3 (L5)
2); // Band 3 (L5)
break;
}
}
@ -332,9 +332,9 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
else
{
// 3. If not found, insert the GPS L5 ephemeris and the observation
//convert ephemeris from GNSS-SDR class to RTKLIB structure
// convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure
// convert observation from GNSS-SDR class to RTKLIB structure
unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_},
@ -342,7 +342,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second,
gps_cnav_ephemeris_iter->second.i_GPS_week,
2); //Band 3 (L5)
2); // Band 3 (L5)
valid_obs++;
}
}
@ -363,14 +363,14 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend())
{
//convert ephemeris from GNSS-SDR class to RTKLIB structure
// convert ephemeris from GNSS-SDR class to RTKLIB structure
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
//convert observation from GNSS-SDR class to RTKLIB structure
// convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second,
glonass_gnav_ephemeris_iter->second.d_WN,
0); //Band 0 (L1)
0); // Band 0 (L1)
glo_valid_obs++;
}
else // the ephemeris are not available for this SV
@ -400,15 +400,15 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
}
if (!found_L1_obs)
{
//insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris
//convert ephemeris from GNSS-SDR class to RTKLIB structure
// insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris
// convert ephemeris from GNSS-SDR class to RTKLIB structure
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
//convert observation from GNSS-SDR class to RTKLIB structure
// convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second,
glonass_gnav_ephemeris_iter->second.d_WN,
1); //Band 1 (L2)
1); // Band 1 (L2)
glo_valid_obs++;
}
}
@ -481,17 +481,32 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
this->set_valid_position(true);
arma::vec rx_position_and_time(4);
rx_position_and_time(0) = pvt_sol.rr[0];
rx_position_and_time(1) = pvt_sol.rr[1];
rx_position_and_time(2) = pvt_sol.rr[2];
rx_position_and_time(3) = pvt_sol.dtr[0];
rx_position_and_time(0) = pvt_sol.rr[0]; // [m]
rx_position_and_time(1) = pvt_sol.rr[1]; // [m]
rx_position_and_time(2) = pvt_sol.rr[2]; // [m]
//todo: fix this ambiguity in the RTKLIB units in receiver clock offset!
if (rtk_.opt.mode == PMODE_SINGLE)
{
rx_position_and_time(3) = pvt_sol.dtr[0]; // if the RTKLIB solver is set to SINGLE, the dtr is already expressed in [s]
}
else
{
rx_position_and_time(3) = pvt_sol.dtr[0] / GPS_C_m_s; // the receiver clock offset is expressed in [meters], so we convert it into [s]
}
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
double offset_s = this->get_time_offset_s();
this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
//observable fix:
//double offset_s = this->get_time_offset_s();
//this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds]
this->set_time_offset_s(rx_position_and_time(3));
DLOG(INFO) << "RTKLIB Position at RX TOW = " << gnss_observables_map.begin()->second.RX_time
<< " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
boost::posix_time::ptime p_time;
gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time);
// gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time);
gtime_t rtklib_utc_time = gpst2time(adjgpsweek(nav_data.eph[0].week), gnss_observables_map.begin()->second.RX_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));
this->set_position_UTC_time(p_time);
@ -509,8 +524,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
try
{
double tmp_double;
// PVT GPS time
tmp_double = Rx_time;
// PVT GPS time
tmp_double = gnss_observables_map.begin()->second.RX_time;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF User Position East [m]
tmp_double = rx_position_and_time(0);

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

@ -85,7 +85,7 @@ public:
rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk);
~rtklib_solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, double Rx_time, bool flag_averaging);
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging);
double get_hdop() const;
double get_vdop() const;
double get_pdop() const;

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

@ -63,7 +63,6 @@
#define SQRT_SOL(x) ((x) < 0.0 ? 0.0 : sqrt(x))
const int MAXFIELD = 64; /* max number of fields in a record */
const int MAXNMEA = 256; /* max length of nmea sentence */
const double KNOT2M = 0.514444444; /* m/knot */

73
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -62,9 +62,9 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
d_nchannels = nchannels_out;
d_dump_filename = dump_filename;
T_rx_s = 0.0;
T_rx_step_s = 0.001; // 1 ms
max_delta = 1.5; // 1.5 s
d_latency = 0.5; // 300 ms
T_rx_step_ms = 1; // 1 ms
max_delta = 1.5; // 1.5 s
d_latency = 0.5; // 300 ms
valid_channels.resize(d_nchannels, false);
d_num_valid_channels = 0;
d_gnss_synchro_history = new Gnss_circular_deque<Gnss_Synchro>(static_cast<unsigned int>(max_delta * 1000.0), d_nchannels);
@ -87,6 +87,8 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
}
}
}
T_rx_TOW_ms = 0;
T_rx_TOW_set = false;
}
@ -308,7 +310,10 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i
}
find_interp_elements(ch, ti);
//Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1)
// 1st: copy the nearest gnss_synchro data for that channel
out = d_gnss_synchro_history->at(ch, 0);
// 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1)
// CARRIER PHASE INTERPOLATION
out.Carrier_phase_rads = d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads + (d_gnss_synchro_history->at(ch, 1).Carrier_phase_rads - d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
@ -317,7 +322,7 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i
out.Carrier_Doppler_hz = d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz + (d_gnss_synchro_history->at(ch, 1).Carrier_Doppler_hz - d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
// TOW INTERPOLATION
out.TOW_at_current_symbol_s = d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_s + (d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_s - d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_s) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
out.interp_TOW_ms = static_cast<double>(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_ms) - static_cast<double>(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms)) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
return true;
}
@ -335,6 +340,7 @@ double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro &a)
}
}
void hybrid_observables_cc::find_interp_elements(const unsigned int &ch, const double &ti)
{
unsigned int closest = 0;
@ -402,10 +408,10 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
{
std::vector<Gnss_Synchro>::iterator it;
/////////////////////// DEBUG //////////////////////////
// Logs if there is a pseudorange difference between
// signals of the same satellite higher than a threshold
////////////////////////////////////////////////////////
/////////////////////// DEBUG //////////////////////////
// Logs if there is a pseudorange difference between
// signals of the same satellite higher than a threshold
////////////////////////////////////////////////////////
#ifndef NDEBUG
std::vector<Gnss_Synchro>::iterator it2;
double thr_ = 250.0 / SPEED_OF_LIGHT; // Maximum pseudorange difference = 250 meters
@ -415,8 +421,8 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
{
if (it->PRN == it2->PRN and it->System == it2->System)
{
double tow_dif_ = std::fabs(it->TOW_at_current_symbol_s - it2->TOW_at_current_symbol_s);
if (tow_dif_ > thr_)
double tow_dif_ = std::fabs(it->TOW_at_current_symbol_ms - it2->TOW_at_current_symbol_ms);
if (tow_dif_ > thr_ * 1000.0)
{
DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal
<< ". TOW difference in PRN " << it->PRN
@ -431,20 +437,37 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
}
}
#endif
///////////////////////////////////////////////////////////
double TOW_ref = std::numeric_limits<double>::lowest();
for (it = data.begin(); it != data.end(); it++)
if (!T_rx_TOW_set)
{
if (it->TOW_at_current_symbol_s > TOW_ref)
unsigned int TOW_ref = std::numeric_limits<unsigned int>::lowest();
for (it = data.begin(); it != data.end(); it++)
{
TOW_ref = it->TOW_at_current_symbol_s;
if (it->TOW_at_current_symbol_ms > TOW_ref)
{
TOW_ref = it->TOW_at_current_symbol_ms;
}
}
T_rx_TOW_ms = TOW_ref;
T_rx_TOW_set = true;
}
else
{
T_rx_TOW_ms += T_rx_step_ms;
//todo: check what happens during the week rollover
if (T_rx_TOW_ms >= 604800000)
{
T_rx_TOW_ms = T_rx_TOW_ms % 604800000;
}
}
for (it = data.begin(); it != data.end(); it++)
{
double traveltime_s = TOW_ref - it->TOW_at_current_symbol_s + GPS_STARTOFFSET_ms / 1000.0;
it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0;
double traveltime_s = (static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
//std::cout.precision(17);
//std::cout << "Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
it->RX_time = (T_rx_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
}
}
@ -466,7 +489,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
}
for (int epoch = 0; epoch < ninput_items[d_nchannels]; epoch++)
{
T_rx_s += T_rx_step_s;
T_rx_s += (static_cast<double>(T_rx_step_ms) / 1000.0);
//////////////////////////////////////////////////////////////////////////
if ((total_input_items == 0) and (d_num_valid_channels == 0))
@ -503,6 +526,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
}
}
}
for (i = 0; i < d_nchannels; i++)
{
if (d_gnss_synchro_history->size(i) > 2)
@ -515,6 +539,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
}
}
d_num_valid_channels = valid_channels.count();
// Check if there is any valid channel after reading the new incoming Gnss_Synchro data
if (d_num_valid_channels == 0)
{
@ -522,7 +547,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
return returned_elements;
}
for (i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold
for (i = 0; i < d_nchannels; i++) // Discard observables with T_rx higher than the threshold
{
if (valid_channels[i])
{
@ -548,7 +573,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
{
if (valid_channels[i])
{
Gnss_Synchro interpolated_gnss_synchro = d_gnss_synchro_history->back(i);
Gnss_Synchro interpolated_gnss_synchro; // empty set, it is required to COPY the nearest in the interpolation history = d_gnss_synchro_history->back(i);
if (interpolate_data(interpolated_gnss_synchro, i, T_rx_s_out))
{
epoch_data.push_back(interpolated_gnss_synchro);
@ -560,11 +585,13 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
}
}
d_num_valid_channels = valid_channels.count();
if (d_num_valid_channels == 0)
{
consume(d_nchannels, epoch + 1);
return returned_elements;
}
correct_TOW_and_compute_prange(epoch_data);
std::vector<Gnss_Synchro>::iterator it = epoch_data.begin();
for (i = 0; i < d_nchannels; i++)
@ -581,6 +608,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
out[i][epoch].Flag_valid_pseudorange = false;
}
}
if (d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
@ -591,7 +619,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
{
tmp_double = out[i][epoch].RX_time;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].TOW_at_current_symbol_s;
tmp_double = out[i][epoch].interp_TOW_ms / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].Carrier_Doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
@ -611,6 +639,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
d_dump = false;
}
}
returned_elements++;
}
consume(d_nchannels, ninput_items[d_nchannels]);

5
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
View File

@ -76,7 +76,10 @@ private:
Gnss_circular_deque<Gnss_Synchro>* d_gnss_synchro_history;
boost::dynamic_bitset<> valid_channels;
double T_rx_s;
double T_rx_step_s;
unsigned int T_rx_step_ms;
//rx time follow GPST
bool T_rx_TOW_set;
unsigned int T_rx_TOW_ms;
double max_delta;
double d_latency;
bool d_dump;

9
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
View File

@ -419,14 +419,14 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
if (d_nav.flag_TOW_5 == true) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
//TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_current_symbol = d_nav.TOW_5 + static_cast<double>(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast<double>(required_symbols - 1) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND;
d_TOW_at_current_symbol = d_nav.TOW_5 + static_cast<double>(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast<double>(required_symbols + 1) * GALILEO_E1_CODE_PERIOD;
d_nav.flag_TOW_5 = false;
}
else if (d_nav.flag_TOW_6 == true) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
//TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_current_symbol = d_nav.TOW_6 + static_cast<double>(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast<double>(required_symbols - 1) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND;
d_TOW_at_current_symbol = d_nav.TOW_6 + static_cast<double>(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast<double>(required_symbols + 1) * GALILEO_E1_CODE_PERIOD;
d_nav.flag_TOW_6 = false;
}
else
@ -456,8 +456,9 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
current_symbol.Flag_valid_word = false;
}
current_symbol.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0;
current_symbol.TOW_at_current_symbol_s -= delta_t; //Galileo to GPS TOW
current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
//todo: Galileo to GPS time conversion should be moved to observable block.
//current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW
if (d_dump == true)
{

2
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
View File

@ -470,7 +470,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
current_sample.Flag_valid_word = false;
}
current_sample.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0;
current_sample.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
if (d_dump)
{

49
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc
View File

@ -61,7 +61,7 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc(
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "Initializing GLONASS L1 CA TELEMETRY DECODING";
// Define the number of sampes per symbol. Notice that GLONASS has 2 rates,
//one for the navigation data and the other for the preamble information
// one for the navigation data and the other for the preamble information
d_samples_per_symbol = (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS) / GLONASS_L1_CA_SYMBOL_RATE_BPS;
// Set the preamble information
@ -268,11 +268,11 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); // Get the output buffer pointer
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]); // Get the input buffer pointer
Gnss_Synchro current_symbol; //structure to save the synchronization information and send the output object to the next block
//1. Copy the current tracking output
Gnss_Synchro current_symbol; // structure to save the synchronization information and send the output object to the next block
// 1. Copy the current tracking output
current_symbol = in[0][0];
d_symbol_history.push_back(current_symbol); //add new symbol to the symbol queue
d_sample_counter++; //count for the processed samples
d_symbol_history.push_back(current_symbol); // add new symbol to the symbol queue
d_sample_counter++; // count for the processed samples
consume_each(1);
d_flag_preamble = false;
@ -280,7 +280,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
if (d_symbol_history.size() > required_symbols)
{
//******* preamble correlation ********
// ******* preamble correlation ********
for (int i = 0; i < d_symbols_per_preamble; i++)
{
if (d_symbol_history.at(i).Prompt_I < 0) // symbols clipping
@ -294,8 +294,8 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
}
}
//******* frame sync ******************
if (d_stat == 0) //no preamble information
// ******* frame sync ******************
if (d_stat == 0) // no preamble information
{
if (abs(corr_value) >= d_symbols_per_preamble)
{
@ -311,15 +311,15 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
{
if (abs(corr_value) >= d_symbols_per_preamble)
{
//check preamble separation
// check preamble separation
preamble_diff = d_sample_counter - d_preamble_index;
// Record the PRN start sample index associated to the preamble
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter;
if (abs(preamble_diff - GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) == 0)
{
//try to decode frame
// try to decode frame
LOG(INFO) << "Starting string decoder for GLONASS L1 C/A SAT " << this->d_satellite;
d_preamble_index = d_sample_counter; //record the preamble sample stamp
d_preamble_index = d_sample_counter; // record the preamble sample stamp
d_stat = 2;
}
else
@ -342,7 +342,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
int string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble;
double string_symbols[GLONASS_GNAV_DATA_SYMBOLS] = {0};
//******* SYMBOL TO BIT *******
// ******* SYMBOL TO BIT *******
for (int i = 0; i < string_length; i++)
{
if (corr_value > 0)
@ -355,13 +355,13 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
}
}
//call the decoder
// call the decoder
decode_string(string_symbols, string_length);
if (d_nav.flag_CRC_test == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; //valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P)
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
@ -372,7 +372,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
else
{
d_CRC_error_counter++;
d_preamble_index = d_sample_counter; //record the preamble sample stamp
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
{
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
@ -384,21 +384,21 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
}
// UPDATE GNSS SYNCHRO DATA
//2. Add the telemetry decoder information
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_TOW_new == true)
//update TOW at the preamble instant
// update TOW at the preamble instant
{
d_TOW_at_current_symbol = floor((d_nav.gnav_ephemeris.d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000;
d_nav.flag_TOW_new = false;
}
else //if there is not a new preamble, we define the TOW of the current symbol
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L1_CA_CODE_PERIOD;
}
//if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
// if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
// if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived
// if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
// {
// delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64)));
// }
@ -413,8 +413,9 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
}
current_symbol.PRN = this->d_satellite.get_PRN();
current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
current_symbol.TOW_at_current_symbol_s -= delta_t; // Galileo to GPS TOW
current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
// todo: glonass time to gps time should be done in observables block
// current_symbol.TOW_at_current_symbol_ms -= -= static_cast<unsigned int>(delta_t) * 1000; // Galileo to GPS TOW
if (d_dump == true)
{
@ -441,7 +442,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
{
d_symbol_history.pop_front();
}
//3. Make the output (copy the object contents to the GNURadio reserved memory)
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;

49
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_cc.cc
View File

@ -61,7 +61,7 @@ glonass_l2_ca_telemetry_decoder_cc::glonass_l2_ca_telemetry_decoder_cc(
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "Initializing GLONASS L2 CA TELEMETRY DECODING";
// Define the number of sampes per symbol. Notice that GLONASS has 2 rates,
//one for the navigation data and the other for the preamble information
// one for the navigation data and the other for the preamble information
d_samples_per_symbol = (GLONASS_L2_CA_CODE_RATE_HZ / GLONASS_L2_CA_CODE_LENGTH_CHIPS) / GLONASS_L2_CA_SYMBOL_RATE_BPS;
// Set the preamble information
@ -268,11 +268,11 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); // Get the output buffer pointer
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]); // Get the input buffer pointer
Gnss_Synchro current_symbol; //structure to save the synchronization information and send the output object to the next block
//1. Copy the current tracking output
Gnss_Synchro current_symbol; // structure to save the synchronization information and send the output object to the next block
// 1. Copy the current tracking output
current_symbol = in[0][0];
d_symbol_history.push_back(current_symbol); //add new symbol to the symbol queue
d_sample_counter++; //count for the processed samples
d_symbol_history.push_back(current_symbol); // add new symbol to the symbol queue
d_sample_counter++; // count for the processed samples
consume_each(1);
d_flag_preamble = false;
@ -280,7 +280,7 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
if (d_symbol_history.size() > required_symbols)
{
//******* preamble correlation ********
// ******* preamble correlation ********
for (int i = 0; i < d_symbols_per_preamble; i++)
{
if (d_symbol_history.at(i).Prompt_I < 0) // symbols clipping
@ -294,8 +294,8 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
}
}
//******* frame sync ******************
if (d_stat == 0) //no preamble information
// ******* frame sync ******************
if (d_stat == 0) // no preamble information
{
if (abs(corr_value) >= d_symbols_per_preamble)
{
@ -311,15 +311,15 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
{
if (abs(corr_value) >= d_symbols_per_preamble)
{
//check preamble separation
// check preamble separation
preamble_diff = d_sample_counter - d_preamble_index;
// Record the PRN start sample index associated to the preamble
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter;
if (abs(preamble_diff - GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS) == 0)
{
//try to decode frame
// try to decode frame
LOG(INFO) << "Starting string decoder for GLONASS L2 C/A SAT " << this->d_satellite;
d_preamble_index = d_sample_counter; //record the preamble sample stamp
d_preamble_index = d_sample_counter; // record the preamble sample stamp
d_stat = 2;
}
else
@ -342,7 +342,7 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
int string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble;
double string_symbols[GLONASS_GNAV_DATA_SYMBOLS] = {0};
//******* SYMBOL TO BIT *******
// ******* SYMBOL TO BIT *******
for (int i = 0; i < string_length; i++)
{
if (corr_value > 0)
@ -355,13 +355,13 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
}
}
//call the decoder
// call the decoder
decode_string(string_symbols, string_length);
if (d_nav.flag_CRC_test == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; //valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P)
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
@ -372,7 +372,7 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
else
{
d_CRC_error_counter++;
d_preamble_index = d_sample_counter; //record the preamble sample stamp
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
{
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
@ -384,21 +384,21 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
}
// UPDATE GNSS SYNCHRO DATA
//2. Add the telemetry decoder information
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_TOW_new == true)
//update TOW at the preamble instant
// update TOW at the preamble instant
{
d_TOW_at_current_symbol = floor((d_nav.gnav_ephemeris.d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000;
d_nav.flag_TOW_new = false;
}
else //if there is not a new preamble, we define the TOW of the current symbol
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L2_CA_CODE_PERIOD;
}
//if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
// if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
// if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived
// if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
// {
// delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64)));
// }
@ -413,8 +413,9 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
}
current_symbol.PRN = this->d_satellite.get_PRN();
current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
current_symbol.TOW_at_current_symbol_s -= delta_t; // Galileo to GPS TOW
current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
// todo: glonass time to gps time should be done in observables block
// current_symbol.TOW_at_current_symbol_ms -= static_cast<unsigned int>(delta_t) * 1000;
if (d_dump == true)
{
@ -441,7 +442,7 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu
{
d_symbol_history.pop_front();
}
//3. Make the output (copy the object contents to the GNURadio reserved memory)
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;

21
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -90,8 +90,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
d_GPS_frame_4bytes = 0;
d_prev_GPS_frame_4bytes = 0;
d_flag_parity = false;
d_TOW_at_Preamble = 0.0;
d_TOW_at_current_symbol = 0.0;
d_TOW_at_Preamble_ms = 0;
flag_TOW_set = false;
d_average_count = 0;
d_flag_preamble = false;
@ -396,25 +395,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
//2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_flag_new_tow_available == true)
{
//double decoder_latency_ms=(double)(current_symbol.Tracking_sample_counter-d_symbol_history.at(0).Tracking_sample_counter)
// /(double)current_symbol.fs;
// update TOW at the preamble instant (account with decoder latency)
d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2.0 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S;
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_GPS_FSM.d_nav.d_TOW) * 1000 + 161;
//d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0;
d_TOW_at_current_symbol = d_TOW_at_Preamble;
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_GPS_FSM.d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS;
d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms;
flag_TOW_set = true;
d_flag_new_tow_available = false;
}
else
{
d_TOW_at_current_symbol += GPS_L1_CA_CODE_PERIOD;
d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS;
}
current_symbol.TOW_at_current_symbol_s = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
//current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_symbol.Flag_valid_word = flag_TOW_set;
if (flag_PLL_180_deg_phase_locked == true)
@ -430,11 +421,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
{
double tmp_double;
unsigned long int tmp_ulong_int;
tmp_double = d_TOW_at_current_symbol;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = d_TOW_at_Preamble;
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) * 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)

3
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

@ -111,8 +111,7 @@ private:
unsigned long int d_preamble_time_samples;
double d_TOW_at_Preamble;
double d_TOW_at_current_symbol;
unsigned int d_TOW_at_Preamble_ms;
unsigned int d_TOW_at_current_symbol_ms;
bool flag_TOW_set;

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc
View File

@ -201,7 +201,7 @@ int gps_l2c_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
d_flag_valid_word = false;
}
}
current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_synchro_data.Flag_valid_word = d_flag_valid_word;
if (d_dump == true)

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc
View File

@ -253,7 +253,7 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u
d_flag_valid_word = false;
}
}
current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_synchro_data.Flag_valid_word = d_flag_valid_word;
if (d_dump == true)

14
src/core/receiver/gnss_flowgraph.cc
View File

@ -775,23 +775,9 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
acq_channels_count_--;
for (unsigned int i = 0; i < channels_count_; i++)
{
unsigned int sat_ = 0;
try
{
sat_ = configuration_->property("Channel" + std::to_string(i) + ".satellite", 0);
}
catch (const std::exception& e)
{
LOG(WARNING) << e.what();
}
if (!available_GNSS_signals_.empty() && (acq_channels_count_ < max_acq_channels_) && (channels_state_[i] == 0))
{
channels_state_[i] = 1;
if (sat_ == 0)
{
std::lock_guard<std::mutex> lock(signal_list_mutex);
channels_[i]->set_signal(search_next_signal(channels_[i]->get_signal().get_signal_str(), true));
}
acq_channels_count_++;
DLOG(INFO) << "Channel " << i << " Starting acquisition " << channels_[i]->get_signal().get_satellite() << ", Signal " << channels_[i]->get_signal().get_signal_str();
channels_[i]->start_acquisition();

5
src/core/system_parameters/GPS_L1_CA.h
View File

@ -67,8 +67,8 @@ const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period
const double MAX_TOA_DELAY_MS = 20;
//#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here
const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)
//const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)
const double GPS_STARTOFFSET_ms = 69.0;
// OBSERVABLE HISTORY DEEP FOR INTERPOLATION
const int GPS_L1_CA_HISTORY_DEEP = 100;
@ -82,6 +82,7 @@ const int GPS_L1_CA_HISTORY_DEEP = 100;
const int GPS_CA_PREAMBLE_LENGTH_BITS = 8;
const int GPS_CA_PREAMBLE_LENGTH_SYMBOLS = 160;
const double GPS_CA_PREAMBLE_DURATION_S = 0.160;
const int GPS_CA_PREAMBLE_DURATION_MS = 160;
const int GPS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s]
const int GPS_CA_TELEMETRY_SYMBOLS_PER_BIT = 20;
const int GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND = GPS_CA_TELEMETRY_RATE_BITS_SECOND * GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s]

5
src/core/system_parameters/gnss_synchro.h
View File

@ -65,13 +65,14 @@ public:
int correlation_length_ms; //!< Set by Tracking processing block
//Telemetry Decoder
bool Flag_valid_word; //!< Set by Telemetry Decoder processing block
double TOW_at_current_symbol_s; //!< Set by Telemetry Decoder processing block
bool Flag_valid_word; //!< Set by Telemetry Decoder processing block
unsigned int TOW_at_current_symbol_ms; //!< Set by Telemetry Decoder processing block
// Observables
double Pseudorange_m; //!< Set by Observables processing block
double RX_time; //!< Set by Observables processing block
bool Flag_valid_pseudorange; //!< Set by Observables processing block
double interp_TOW_ms; //!< Set by Observables processing block
};
#endif

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

@ -255,7 +255,6 @@ int StaticPositionSystemTest::configure_receiver()
const float band1_error = 1.0;
const float band2_error = 1.0;
const int grid_density = 16;
const int decimation_factor = 1;
const float zero = 0.0;
const int number_of_channels = 8;

2
src/utils/matlab/libs/read_hybrid_observables_dump.m
View File

@ -79,7 +79,7 @@ else
% {
% tmp_double = current_gnss_synchro[i].RX_time;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s;
% tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_ms;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz;
% d_dump_file.write((char*)&tmp_double, sizeof(double));