mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 15:23:04 +00:00
Code Issues Releases Wiki Activity

Improve const correctness. Improve usage of typeid

Browse Source
This commit is contained in:
Carles Fernandez
2020-07-19 09:39:32 +02:00
parent 8f2973eab9
commit c0f81dd9e2
33 changed files with 1566 additions and 1575 deletions
Download Patch File Download Diff File Expand all files Collapse all files

184
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@@ -74,6 +74,7 @@
#include <stdexcept> // for length_error #include <stdexcept> // for length_error
#include <sys/ipc.h> // for IPC_CREAT #include <sys/ipc.h> // for IPC_CREAT
#include <sys/msg.h> // for msgctl #include <sys/msg.h> // for msgctl
#include <typeinfo> // for std::type_info, typeid
#include <utility> // for pair #include <utility> // for pair
#if HAS_GENERIC_LAMBDA #if HAS_GENERIC_LAMBDA
@@ -471,26 +472,46 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
{ {
// setup two PVT solvers: internal solver for rx clock and user solver // setup two PVT solvers: internal solver for rx clock and user solver
// user PVT solver // user PVT solver
d_user_pvt_solver = std::make_shared<Rtklib_Solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, d_dump_mat, rtk); d_user_pvt_solver = std::make_shared<Rtklib_Solver>(rtk, static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, d_dump_mat);
d_user_pvt_solver->set_averaging_depth(1); d_user_pvt_solver->set_averaging_depth(1);
d_user_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); d_user_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
// internal PVT solver, mainly used to estimate the receiver clock // internal PVT solver, mainly used to estimate the receiver clock
rtk_t internal_rtk = rtk; rtk_t internal_rtk = rtk;
internal_rtk.opt.mode = PMODE_SINGLE; // use single positioning mode in internal PVT solver internal_rtk.opt.mode = PMODE_SINGLE; // use single positioning mode in internal PVT solver
d_internal_pvt_solver = std::make_shared<Rtklib_Solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, false, false, internal_rtk); d_internal_pvt_solver = std::make_shared<Rtklib_Solver>(internal_rtk, static_cast<int32_t>(nchannels), dump_ls_pvt_filename, false, false);
d_internal_pvt_solver->set_averaging_depth(1); d_internal_pvt_solver->set_averaging_depth(1);
d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
} }
else else
{ {
// only one solver, customized by the user options // only one solver, customized by the user options
d_internal_pvt_solver = std::make_shared<Rtklib_Solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, d_dump_mat, rtk); d_internal_pvt_solver = std::make_shared<Rtklib_Solver>(rtk, static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, d_dump_mat);
d_internal_pvt_solver->set_averaging_depth(1); d_internal_pvt_solver->set_averaging_depth(1);
d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file); d_internal_pvt_solver->set_pre_2009_file(conf_.pre_2009_file);
d_user_pvt_solver = d_internal_pvt_solver; d_user_pvt_solver = d_internal_pvt_solver;
} }
d_gps_ephemeris_sptr_type_hash_code = typeid(std::shared_ptr<Gps_Ephemeris>).hash_code();
d_gps_iono_sptr_type_hash_code = typeid(std::shared_ptr<Gps_Iono>).hash_code();
d_gps_utc_model_sptr_type_hash_code = typeid(std::shared_ptr<Gps_Utc_Model>).hash_code();
d_gps_cnav_ephemeris_sptr_type_hash_code = typeid(std::shared_ptr<Gps_CNAV_Ephemeris>).hash_code();
d_gps_cnav_iono_sptr_type_hash_code = typeid(std::shared_ptr<Gps_CNAV_Iono>).hash_code();
d_gps_cnav_utc_model_sptr_type_hash_code = typeid(std::shared_ptr<Gps_CNAV_Utc_Model>).hash_code();
d_gps_almanac_sptr_type_hash_code = typeid(std::shared_ptr<Gps_Almanac>).hash_code();
d_galileo_ephemeris_sptr_type_hash_code = typeid(std::shared_ptr<Galileo_Ephemeris>).hash_code();
d_galileo_iono_sptr_type_hash_code = typeid(std::shared_ptr<Galileo_Iono>).hash_code();
d_galileo_utc_model_sptr_type_hash_code = typeid(std::shared_ptr<Galileo_Utc_Model>).hash_code();
d_galileo_almanac_helper_sptr_type_hash_code = typeid(std::shared_ptr<Galileo_Almanac_Helper>).hash_code();
d_galileo_almanac_sptr_type_hash_code = typeid(std::shared_ptr<Galileo_Almanac>).hash_code();
d_glonass_gnav_ephemeris_sptr_type_hash_code = typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>).hash_code();
d_glonass_gnav_utc_model_sptr_type_hash_code = typeid(std::shared_ptr<Glonass_Gnav_Utc_Model>).hash_code();
d_glonass_gnav_almanac_sptr_type_hash_code = typeid(std::shared_ptr<Glonass_Gnav_Almanac>).hash_code();
d_beidou_dnav_ephemeris_sptr_type_hash_code = typeid(std::shared_ptr<Beidou_Dnav_Ephemeris>).hash_code();
d_beidou_dnav_iono_sptr_type_hash_code = typeid(std::shared_ptr<Beidou_Dnav_Iono>).hash_code();
d_beidou_dnav_utc_model_sptr_type_hash_code = typeid(std::shared_ptr<Beidou_Dnav_Utc_Model>).hash_code();
d_beidou_dnav_almanac_sptr_type_hash_code = typeid(std::shared_ptr<Beidou_Dnav_Almanac>).hash_code();
d_start = std::chrono::system_clock::now(); d_start = std::chrono::system_clock::now();
} }
@@ -1050,12 +1071,12 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
{ {
try try
{ {
// ************* GPS telemetry ***************** const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Ephemeris>)) // ************************* GPS telemetry *************************
if (msg_type_hash_code == d_gps_ephemeris_sptr_type_hash_code)
{ {
// ### GPS EPHEMERIS ### // ### GPS EPHEMERIS ###
std::shared_ptr<Gps_Ephemeris> gps_eph; const std::shared_ptr<Gps_Ephemeris> gps_eph = boost::any_cast<std::shared_ptr<Gps_Ephemeris>>(pmt::any_ref(msg));
gps_eph = boost::any_cast<std::shared_ptr<Gps_Ephemeris>>(pmt::any_ref(msg));
DLOG(INFO) << "Ephemeris record has arrived from SAT ID " DLOG(INFO) << "Ephemeris record has arrived from SAT ID "
<< gps_eph->i_satellite_PRN << " (Block " << gps_eph->i_satellite_PRN << " (Block "
<< gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")"
@@ -1135,11 +1156,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
d_user_pvt_solver->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; d_user_pvt_solver->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph;
} }
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Iono>)) else if (msg_type_hash_code == d_gps_iono_sptr_type_hash_code)
{ {
// ### GPS IONO ### // ### GPS IONO ###
std::shared_ptr<Gps_Iono> gps_iono; const std::shared_ptr<Gps_Iono> gps_iono = boost::any_cast<std::shared_ptr<Gps_Iono>>(pmt::any_ref(msg));
gps_iono = boost::any_cast<std::shared_ptr<Gps_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_iono = *gps_iono; d_internal_pvt_solver->gps_iono = *gps_iono;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1147,11 +1167,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New IONO record has arrived "; DLOG(INFO) << "New IONO record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Utc_Model>)) else if (msg_type_hash_code == d_gps_utc_model_sptr_type_hash_code)
{ {
// ### GPS UTC MODEL ### // ### GPS UTC MODEL ###
std::shared_ptr<Gps_Utc_Model> gps_utc_model; const std::shared_ptr<Gps_Utc_Model> gps_utc_model = boost::any_cast<std::shared_ptr<Gps_Utc_Model>>(pmt::any_ref(msg));
gps_utc_model = boost::any_cast<std::shared_ptr<Gps_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_utc_model = *gps_utc_model; d_internal_pvt_solver->gps_utc_model = *gps_utc_model;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1159,11 +1178,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New UTC record has arrived "; DLOG(INFO) << "New UTC record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Ephemeris>)) else if (msg_type_hash_code == d_gps_cnav_ephemeris_sptr_type_hash_code)
{ {
// ### GPS CNAV message ### // ### GPS CNAV message ###
std::shared_ptr<Gps_CNAV_Ephemeris> gps_cnav_ephemeris; const std::shared_ptr<Gps_CNAV_Ephemeris> gps_cnav_ephemeris = boost::any_cast<std::shared_ptr<Gps_CNAV_Ephemeris>>(pmt::any_ref(msg));
gps_cnav_ephemeris = boost::any_cast<std::shared_ptr<Gps_CNAV_Ephemeris>>(pmt::any_ref(msg));
// update/insert new ephemeris record to the global ephemeris map // update/insert new ephemeris record to the global ephemeris map
if (d_rinex_header_written) // The header is already written, we can now log the navigation message data if (d_rinex_header_written) // The header is already written, we can now log the navigation message data
{ {
@@ -1212,11 +1230,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New GPS CNAV ephemeris record has arrived "; DLOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>)) else if (msg_type_hash_code == d_gps_cnav_iono_sptr_type_hash_code)
{ {
// ### GPS CNAV IONO ### // ### GPS CNAV IONO ###
std::shared_ptr<Gps_CNAV_Iono> gps_cnav_iono; const std::shared_ptr<Gps_CNAV_Iono> gps_cnav_iono = boost::any_cast<std::shared_ptr<Gps_CNAV_Iono>>(pmt::any_ref(msg));
gps_cnav_iono = boost::any_cast<std::shared_ptr<Gps_CNAV_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_cnav_iono = *gps_cnav_iono; d_internal_pvt_solver->gps_cnav_iono = *gps_cnav_iono;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1224,11 +1241,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New CNAV IONO record has arrived "; DLOG(INFO) << "New CNAV IONO record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Utc_Model>)) else if (msg_type_hash_code == d_gps_cnav_utc_model_sptr_type_hash_code)
{ {
// ### GPS CNAV UTC MODEL ### // ### GPS CNAV UTC MODEL ###
std::shared_ptr<Gps_CNAV_Utc_Model> gps_cnav_utc_model; const std::shared_ptr<Gps_CNAV_Utc_Model> gps_cnav_utc_model = boost::any_cast<std::shared_ptr<Gps_CNAV_Utc_Model>>(pmt::any_ref(msg));
gps_cnav_utc_model = boost::any_cast<std::shared_ptr<Gps_CNAV_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model; d_internal_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model;
{ {
d_user_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model; d_user_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model;
@@ -1236,11 +1252,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
DLOG(INFO) << "New CNAV UTC record has arrived "; DLOG(INFO) << "New CNAV UTC record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Almanac>)) else if (msg_type_hash_code == d_gps_almanac_sptr_type_hash_code)
{ {
// ### GPS ALMANAC ### // ### GPS ALMANAC ###
std::shared_ptr<Gps_Almanac> gps_almanac; const std::shared_ptr<Gps_Almanac> gps_almanac = boost::any_cast<std::shared_ptr<Gps_Almanac>>(pmt::any_ref(msg));
gps_almanac = boost::any_cast<std::shared_ptr<Gps_Almanac>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac; d_internal_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1249,12 +1264,11 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
DLOG(INFO) << "New GPS almanac record has arrived "; DLOG(INFO) << "New GPS almanac record has arrived ";
} }
// **************** Galileo telemetry ******************** // *********************** Galileo telemetry ***********************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Ephemeris>)) else if (msg_type_hash_code == d_galileo_ephemeris_sptr_type_hash_code)
{ {
// ### Galileo EPHEMERIS ### // ### Galileo EPHEMERIS ###
std::shared_ptr<Galileo_Ephemeris> galileo_eph; const std::shared_ptr<Galileo_Ephemeris> galileo_eph = boost::any_cast<std::shared_ptr<Galileo_Ephemeris>>(pmt::any_ref(msg));
galileo_eph = boost::any_cast<std::shared_ptr<Galileo_Ephemeris>>(pmt::any_ref(msg));
// insert new ephemeris record // insert new ephemeris record
DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5 DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5
<< ", GALILEO Week Number =" << galileo_eph->WN_5 << ", GALILEO Week Number =" << galileo_eph->WN_5
@@ -1320,11 +1334,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
d_user_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; d_user_pvt_solver->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph;
} }
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Iono>)) else if (msg_type_hash_code == d_galileo_iono_sptr_type_hash_code)
{ {
// ### Galileo IONO ### // ### Galileo IONO ###
std::shared_ptr<Galileo_Iono> galileo_iono; const std::shared_ptr<Galileo_Iono> galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->galileo_iono = *galileo_iono; d_internal_pvt_solver->galileo_iono = *galileo_iono;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1332,11 +1345,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New IONO record has arrived "; DLOG(INFO) << "New IONO record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Utc_Model>)) else if (msg_type_hash_code == d_galileo_utc_model_sptr_type_hash_code)
{ {
// ### Galileo UTC MODEL ### // ### Galileo UTC MODEL ###
std::shared_ptr<Galileo_Utc_Model> galileo_utc_model; const std::shared_ptr<Galileo_Utc_Model> galileo_utc_model = boost::any_cast<std::shared_ptr<Galileo_Utc_Model>>(pmt::any_ref(msg));
galileo_utc_model = boost::any_cast<std::shared_ptr<Galileo_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->galileo_utc_model = *galileo_utc_model; d_internal_pvt_solver->galileo_utc_model = *galileo_utc_model;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1344,12 +1356,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New UTC record has arrived "; DLOG(INFO) << "New UTC record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac_Helper>)) else if (msg_type_hash_code == d_galileo_almanac_helper_sptr_type_hash_code)
{ {
// ### Galileo Almanac ### // ### Galileo Almanac ###
std::shared_ptr<Galileo_Almanac_Helper> galileo_almanac_helper; const std::shared_ptr<Galileo_Almanac_Helper> galileo_almanac_helper = boost::any_cast<std::shared_ptr<Galileo_Almanac_Helper>>(pmt::any_ref(msg));
galileo_almanac_helper = boost::any_cast<std::shared_ptr<Galileo_Almanac_Helper>>(pmt::any_ref(msg));
Galileo_Almanac sv1 = galileo_almanac_helper->get_almanac(1); Galileo_Almanac sv1 = galileo_almanac_helper->get_almanac(1);
Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2); Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2);
Galileo_Almanac sv3 = galileo_almanac_helper->get_almanac(3); Galileo_Almanac sv3 = galileo_almanac_helper->get_almanac(3);
@@ -1380,11 +1390,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New Galileo Almanac data have arrived "; DLOG(INFO) << "New Galileo Almanac data have arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac>)) else if (msg_type_hash_code == d_galileo_almanac_sptr_type_hash_code)
{ {
// ### Galileo Almanac ### // ### Galileo Almanac ###
std::shared_ptr<Galileo_Almanac> galileo_alm; const std::shared_ptr<Galileo_Almanac> galileo_alm = boost::any_cast<std::shared_ptr<Galileo_Almanac>>(pmt::any_ref(msg));
galileo_alm = boost::any_cast<std::shared_ptr<Galileo_Almanac>>(pmt::any_ref(msg));
// update/insert new almanac record to the global almanac map // update/insert new almanac record to the global almanac map
d_internal_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm; d_internal_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
@@ -1393,12 +1402,11 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
} }
// **************** GLONASS GNAV Telemetry ************************** // **************** GLONASS GNAV Telemetry *************************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>)) else if (msg_type_hash_code == d_glonass_gnav_ephemeris_sptr_type_hash_code)
{ {
// ### GLONASS GNAV EPHEMERIS ### // ### GLONASS GNAV EPHEMERIS ###
std::shared_ptr<Glonass_Gnav_Ephemeris> glonass_gnav_eph; const std::shared_ptr<Glonass_Gnav_Ephemeris> glonass_gnav_eph = boost::any_cast<std::shared_ptr<Glonass_Gnav_Ephemeris>>(pmt::any_ref(msg));
glonass_gnav_eph = boost::any_cast<std::shared_ptr<Glonass_Gnav_Ephemeris>>(pmt::any_ref(msg));
// TODO Add GLONASS with gps week number and tow, // TODO Add GLONASS with gps week number and tow,
// insert new ephemeris record // insert new ephemeris record
DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->d_TOW DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->d_TOW
@@ -1478,11 +1486,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
d_user_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; d_user_pvt_solver->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph;
} }
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Utc_Model>)) else if (msg_type_hash_code == d_glonass_gnav_utc_model_sptr_type_hash_code)
{ {
// ### GLONASS GNAV UTC MODEL ### // ### GLONASS GNAV UTC MODEL ###
std::shared_ptr<Glonass_Gnav_Utc_Model> glonass_gnav_utc_model; const std::shared_ptr<Glonass_Gnav_Utc_Model> glonass_gnav_utc_model = boost::any_cast<std::shared_ptr<Glonass_Gnav_Utc_Model>>(pmt::any_ref(msg));
glonass_gnav_utc_model = boost::any_cast<std::shared_ptr<Glonass_Gnav_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model; d_internal_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1490,11 +1497,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New GLONASS GNAV UTC record has arrived "; DLOG(INFO) << "New GLONASS GNAV UTC record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Almanac>)) else if (msg_type_hash_code == d_glonass_gnav_almanac_sptr_type_hash_code)
{ {
// ### GLONASS GNAV Almanac ### // ### GLONASS GNAV Almanac ###
std::shared_ptr<Glonass_Gnav_Almanac> glonass_gnav_almanac; const std::shared_ptr<Glonass_Gnav_Almanac> glonass_gnav_almanac = boost::any_cast<std::shared_ptr<Glonass_Gnav_Almanac>>(pmt::any_ref(msg));
glonass_gnav_almanac = boost::any_cast<std::shared_ptr<Glonass_Gnav_Almanac>>(pmt::any_ref(msg));
d_internal_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac; d_internal_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1504,12 +1510,11 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
<< ", GLONASS GNAV Slot Number =" << glonass_gnav_almanac->d_n_A; << ", GLONASS GNAV Slot Number =" << glonass_gnav_almanac->d_n_A;
} }
// ************* BeiDou telemetry ***************** // *********************** BeiDou telemetry ************************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Ephemeris>)) else if (msg_type_hash_code == d_beidou_dnav_ephemeris_sptr_type_hash_code)
{ {
// ### Beidou EPHEMERIS ### // ### Beidou EPHEMERIS ###
std::shared_ptr<Beidou_Dnav_Ephemeris> bds_dnav_eph; const std::shared_ptr<Beidou_Dnav_Ephemeris> bds_dnav_eph = boost::any_cast<std::shared_ptr<Beidou_Dnav_Ephemeris>>(pmt::any_ref(msg));
bds_dnav_eph = boost::any_cast<std::shared_ptr<Beidou_Dnav_Ephemeris>>(pmt::any_ref(msg));
DLOG(INFO) << "Ephemeris record has arrived from SAT ID " DLOG(INFO) << "Ephemeris record has arrived from SAT ID "
<< bds_dnav_eph->i_satellite_PRN << " (Block " << bds_dnav_eph->i_satellite_PRN << " (Block "
<< bds_dnav_eph->satelliteBlock[bds_dnav_eph->i_satellite_PRN] << ")" << bds_dnav_eph->satelliteBlock[bds_dnav_eph->i_satellite_PRN] << ")"
@@ -1552,11 +1557,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
d_user_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph; d_user_pvt_solver->beidou_dnav_ephemeris_map[bds_dnav_eph->i_satellite_PRN] = *bds_dnav_eph;
} }
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Iono>)) else if (msg_type_hash_code == d_beidou_dnav_iono_sptr_type_hash_code)
{ {
// ### BeiDou IONO ### // ### BeiDou IONO ###
std::shared_ptr<Beidou_Dnav_Iono> bds_dnav_iono; const std::shared_ptr<Beidou_Dnav_Iono> bds_dnav_iono = boost::any_cast<std::shared_ptr<Beidou_Dnav_Iono>>(pmt::any_ref(msg));
bds_dnav_iono = boost::any_cast<std::shared_ptr<Beidou_Dnav_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->beidou_dnav_iono = *bds_dnav_iono; d_internal_pvt_solver->beidou_dnav_iono = *bds_dnav_iono;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1564,11 +1568,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New BeiDou DNAV IONO record has arrived "; DLOG(INFO) << "New BeiDou DNAV IONO record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Utc_Model>)) else if (msg_type_hash_code == d_beidou_dnav_utc_model_sptr_type_hash_code)
{ {
// ### BeiDou UTC MODEL ### // ### BeiDou UTC MODEL ###
std::shared_ptr<Beidou_Dnav_Utc_Model> bds_dnav_utc_model; const std::shared_ptr<Beidou_Dnav_Utc_Model> bds_dnav_utc_model = boost::any_cast<std::shared_ptr<Beidou_Dnav_Utc_Model>>(pmt::any_ref(msg));
bds_dnav_utc_model = boost::any_cast<std::shared_ptr<Beidou_Dnav_Utc_Model>>(pmt::any_ref(msg));
d_internal_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model; d_internal_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1576,11 +1579,10 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
} }
DLOG(INFO) << "New BeiDou DNAV UTC record has arrived "; DLOG(INFO) << "New BeiDou DNAV UTC record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Almanac>)) else if (msg_type_hash_code == d_beidou_dnav_almanac_sptr_type_hash_code)
{ {
// ### BeiDou ALMANAC ### // ### BeiDou ALMANAC ###
std::shared_ptr<Beidou_Dnav_Almanac> bds_dnav_almanac; const std::shared_ptr<Beidou_Dnav_Almanac> bds_dnav_almanac = boost::any_cast<std::shared_ptr<Beidou_Dnav_Almanac>>(pmt::any_ref(msg));
bds_dnav_almanac = boost::any_cast<std::shared_ptr<Beidou_Dnav_Almanac>>(pmt::any_ref(msg));
d_internal_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac; d_internal_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac;
if (d_enable_rx_clock_correction == true) if (d_enable_rx_clock_correction == true)
{ {
@@ -1917,7 +1919,7 @@ void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset()
default: default:
break; break;
} }
double wrap_carrier_phase_rad = fmod(observables_iter->second.Carrier_phase_rads, TWO_PI); const double wrap_carrier_phase_rad = fmod(observables_iter->second.Carrier_phase_rads, TWO_PI);
d_initial_carrier_phase_offset_estimation_rads.at(observables_iter->second.Channel_ID) = TWO_PI * round(observables_iter->second.Pseudorange_m / wavelength_m) - observables_iter->second.Carrier_phase_rads + wrap_carrier_phase_rad; d_initial_carrier_phase_offset_estimation_rads.at(observables_iter->second.Channel_ID) = TWO_PI * round(observables_iter->second.Pseudorange_m / wavelength_m) - observables_iter->second.Carrier_phase_rads + wrap_carrier_phase_rad;
d_channel_initialized.at(observables_iter->second.Channel_ID) = true; d_channel_initialized.at(observables_iter->second.Channel_ID) = true;
DLOG(INFO) << "initialized carrier phase at channel " << observables_iter->second.Channel_ID; DLOG(INFO) << "initialized carrier phase at channel " << observables_iter->second.Channel_ID;
@@ -1948,17 +1950,17 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
{ {
if (in[i][epoch].Flag_valid_pseudorange) if (in[i][epoch].Flag_valid_pseudorange)
{ {
auto tmp_eph_iter_gps = d_internal_pvt_solver->gps_ephemeris_map.find(in[i][epoch].PRN); const auto tmp_eph_iter_gps = d_internal_pvt_solver->gps_ephemeris_map.find(in[i][epoch].PRN);
auto tmp_eph_iter_gal = d_internal_pvt_solver->galileo_ephemeris_map.find(in[i][epoch].PRN); const auto tmp_eph_iter_gal = d_internal_pvt_solver->galileo_ephemeris_map.find(in[i][epoch].PRN);
auto tmp_eph_iter_cnav = d_internal_pvt_solver->gps_cnav_ephemeris_map.find(in[i][epoch].PRN); const auto tmp_eph_iter_cnav = d_internal_pvt_solver->gps_cnav_ephemeris_map.find(in[i][epoch].PRN);
auto tmp_eph_iter_glo_gnav = d_internal_pvt_solver->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN); const auto tmp_eph_iter_glo_gnav = d_internal_pvt_solver->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN);
auto tmp_eph_iter_bds_dnav = d_internal_pvt_solver->beidou_dnav_ephemeris_map.find(in[i][epoch].PRN); const auto tmp_eph_iter_bds_dnav = d_internal_pvt_solver->beidou_dnav_ephemeris_map.find(in[i][epoch].PRN);
bool store_valid_observable = false; bool store_valid_observable = false;
if (tmp_eph_iter_gps != d_internal_pvt_solver->gps_ephemeris_map.cend()) if (tmp_eph_iter_gps != d_internal_pvt_solver->gps_ephemeris_map.cend())
{ {
uint32_t prn_aux = tmp_eph_iter_gps->second.i_satellite_PRN; const uint32_t prn_aux = tmp_eph_iter_gps->second.i_satellite_PRN;
if ((prn_aux == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal) == "1C")) if ((prn_aux == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal) == "1C"))
{ {
store_valid_observable = true; store_valid_observable = true;
@@ -1966,7 +1968,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
} }
if (tmp_eph_iter_gal != d_internal_pvt_solver->galileo_ephemeris_map.cend()) if (tmp_eph_iter_gal != d_internal_pvt_solver->galileo_ephemeris_map.cend())
{ {
uint32_t prn_aux = tmp_eph_iter_gal->second.i_satellite_PRN; const uint32_t prn_aux = tmp_eph_iter_gal->second.i_satellite_PRN;
if ((prn_aux == in[i][epoch].PRN) and ((std::string(in[i][epoch].Signal) == "1B") or (std::string(in[i][epoch].Signal) == "5X"))) if ((prn_aux == in[i][epoch].PRN) and ((std::string(in[i][epoch].Signal) == "1B") or (std::string(in[i][epoch].Signal) == "5X")))
{ {
store_valid_observable = true; store_valid_observable = true;
@@ -1974,7 +1976,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
} }
if (tmp_eph_iter_cnav != d_internal_pvt_solver->gps_cnav_ephemeris_map.cend()) if (tmp_eph_iter_cnav != d_internal_pvt_solver->gps_cnav_ephemeris_map.cend())
{ {
uint32_t prn_aux = tmp_eph_iter_cnav->second.i_satellite_PRN; const uint32_t prn_aux = tmp_eph_iter_cnav->second.i_satellite_PRN;
if ((prn_aux == in[i][epoch].PRN) and ((std::string(in[i][epoch].Signal) == "2S") or (std::string(in[i][epoch].Signal) == "L5"))) if ((prn_aux == in[i][epoch].PRN) and ((std::string(in[i][epoch].Signal) == "2S") or (std::string(in[i][epoch].Signal) == "L5")))
{ {
store_valid_observable = true; store_valid_observable = true;
@@ -1982,7 +1984,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
} }
if (tmp_eph_iter_glo_gnav != d_internal_pvt_solver->glonass_gnav_ephemeris_map.cend()) if (tmp_eph_iter_glo_gnav != d_internal_pvt_solver->glonass_gnav_ephemeris_map.cend())
{ {
uint32_t prn_aux = tmp_eph_iter_glo_gnav->second.i_satellite_PRN; const uint32_t prn_aux = tmp_eph_iter_glo_gnav->second.i_satellite_PRN;
if ((prn_aux == in[i][epoch].PRN) and ((std::string(in[i][epoch].Signal) == "1G") or (std::string(in[i][epoch].Signal) == "2G"))) if ((prn_aux == in[i][epoch].PRN) and ((std::string(in[i][epoch].Signal) == "1G") or (std::string(in[i][epoch].Signal) == "2G")))
{ {
store_valid_observable = true; store_valid_observable = true;
@@ -1990,7 +1992,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
} }
if (tmp_eph_iter_bds_dnav != d_internal_pvt_solver->beidou_dnav_ephemeris_map.cend()) if (tmp_eph_iter_bds_dnav != d_internal_pvt_solver->beidou_dnav_ephemeris_map.cend())
{ {
uint32_t prn_aux = tmp_eph_iter_bds_dnav->second.i_satellite_PRN; const uint32_t prn_aux = tmp_eph_iter_bds_dnav->second.i_satellite_PRN;
if ((prn_aux == in[i][epoch].PRN) and ((std::string(in[i][epoch].Signal) == "B1") or (std::string(in[i][epoch].Signal) == "B3"))) if ((prn_aux == in[i][epoch].PRN) and ((std::string(in[i][epoch].Signal) == "B1") or (std::string(in[i][epoch].Signal) == "B3")))
{ {
store_valid_observable = true; store_valid_observable = true;
@@ -2138,7 +2140,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
// required to report accumulated phase cycles comparable to pseudoranges // required to report accumulated phase cycles comparable to pseudoranges
initialize_and_apply_carrier_phase_offset(); initialize_and_apply_carrier_phase_offset();
double Rx_clock_offset_s = d_user_pvt_solver->get_time_offset_s(); const double Rx_clock_offset_s = d_user_pvt_solver->get_time_offset_s();
if (d_enable_rx_clock_correction == true and fabs(Rx_clock_offset_s) > 0.000001) // 1us !! if (d_enable_rx_clock_correction == true and fabs(Rx_clock_offset_s) > 0.000001) // 1us !!
{ {
LOG(INFO) << "Warning: Rx clock offset at interpolated RX time: " << Rx_clock_offset_s * 1000.0 << "[ms]" LOG(INFO) << "Warning: Rx clock offset at interpolated RX time: " << Rx_clock_offset_s * 1000.0 << "[ms]"
@@ -3207,7 +3209,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int gps_channel = 0; int gps_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gps_channel == 0) if (gps_channel == 0)
{ {
if (system == "G") if (system == "G")
@@ -3305,7 +3307,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int glo_channel = 0; int glo_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gps_channel == 0) if (gps_channel == 0)
{ {
if (system == "G") if (system == "G")
@@ -3365,7 +3367,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int glo_channel = 0; int glo_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gal_channel == 0) if (gal_channel == 0)
{ {
if (system == "E") if (system == "E")
@@ -3424,7 +3426,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int glo_channel = 0; int glo_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gps_channel == 0) if (gps_channel == 0)
{ {
if (system == "G") if (system == "G")
@@ -3483,7 +3485,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int glo_channel = 0; int glo_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gal_channel == 0) if (gal_channel == 0)
{ {
if (system == "E") if (system == "E")
@@ -3542,7 +3544,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int gps_channel = 0; int gps_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gal_channel == 0) if (gal_channel == 0)
{ {
if (system == "E") if (system == "E")
@@ -3687,7 +3689,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int gal_channel = 0; int gal_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gps_channel == 0) if (gps_channel == 0)
{ {
if (system == "G") if (system == "G")
@@ -3739,7 +3741,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int gal_channel = 0; int gal_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gal_channel == 0) if (gal_channel == 0)
{ {
if (system == "E") if (system == "E")
@@ -3823,7 +3825,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int glo_channel = 0; int glo_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gps_channel == 0) if (gps_channel == 0)
{ {
if (system == "G") if (system == "G")
@@ -3883,7 +3885,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
auto glonass_gnav_eph_iter = d_user_pvt_solver->glonass_gnav_ephemeris_map.cbegin(); auto glonass_gnav_eph_iter = d_user_pvt_solver->glonass_gnav_ephemeris_map.cbegin();
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gal_channel == 0) if (gal_channel == 0)
{ {
if (system == "E") if (system == "E")
@@ -3943,7 +3945,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int glo_channel = 0; int glo_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gps_channel == 0) if (gps_channel == 0)
{ {
if (system == "G") if (system == "G")
@@ -4004,7 +4006,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
auto glonass_gnav_eph_iter = d_user_pvt_solver->glonass_gnav_ephemeris_map.cbegin(); auto glonass_gnav_eph_iter = d_user_pvt_solver->glonass_gnav_ephemeris_map.cbegin();
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gal_channel == 0) if (gal_channel == 0)
{ {
if (system == "E") if (system == "E")
@@ -4064,7 +4066,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
int gal_channel = 0; int gal_channel = 0;
for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++) for (gnss_observables_iter = d_gnss_observables_map.cbegin(); gnss_observables_iter != d_gnss_observables_map.cend(); gnss_observables_iter++)
{ {
std::string system(&gnss_observables_iter->second.System, 1); const std::string system(&gnss_observables_iter->second.System, 1);
if (gps_channel == 0) if (gps_channel == 0)
{ {
if (system == "G") if (system == "G")
@@ -4177,7 +4179,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
// PVT MONITOR // PVT MONITOR
if (d_user_pvt_solver->is_valid_position()) if (d_user_pvt_solver->is_valid_position())
{ {
std::shared_ptr<Monitor_Pvt> monitor_pvt = std::make_shared<Monitor_Pvt>(d_user_pvt_solver->get_monitor_pvt()); const std::shared_ptr<Monitor_Pvt> monitor_pvt = std::make_shared<Monitor_Pvt>(d_user_pvt_solver->get_monitor_pvt());
// publish new position to the gnss_flowgraph channel status monitor // publish new position to the gnss_flowgraph channel status monitor
if (current_RX_time_ms % d_report_rate_ms == 0) if (current_RX_time_ms % d_report_rate_ms == 0)

21
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h
View File

@@ -28,6 +28,7 @@
#include <gnuradio/types.h> // for gr_vector_const_void_star #include <gnuradio/types.h> // for gr_vector_const_void_star
#include <pmt/pmt.h> // for pmt_t #include <pmt/pmt.h> // for pmt_t
#include <chrono> // for system_clock #include <chrono> // for system_clock
#include <cstddef> // for size_t
#include <cstdint> // for int32_t #include <cstdint> // for int32_t
#include <ctime> // for time_t #include <ctime> // for time_t
#include <map> // for map #include <map> // for map
@@ -202,6 +203,26 @@ private:
boost::posix_time::time_duration d_utc_diff_time; boost::posix_time::time_duration d_utc_diff_time;
size_t d_gps_ephemeris_sptr_type_hash_code;
size_t d_gps_iono_sptr_type_hash_code;
size_t d_gps_utc_model_sptr_type_hash_code;
size_t d_gps_cnav_ephemeris_sptr_type_hash_code;
size_t d_gps_cnav_iono_sptr_type_hash_code;
size_t d_gps_cnav_utc_model_sptr_type_hash_code;
size_t d_gps_almanac_sptr_type_hash_code;
size_t d_galileo_ephemeris_sptr_type_hash_code;
size_t d_galileo_iono_sptr_type_hash_code;
size_t d_galileo_utc_model_sptr_type_hash_code;
size_t d_galileo_almanac_helper_sptr_type_hash_code;
size_t d_galileo_almanac_sptr_type_hash_code;
size_t d_glonass_gnav_ephemeris_sptr_type_hash_code;
size_t d_glonass_gnav_utc_model_sptr_type_hash_code;
size_t d_glonass_gnav_almanac_sptr_type_hash_code;
size_t d_beidou_dnav_ephemeris_sptr_type_hash_code;
size_t d_beidou_dnav_iono_sptr_type_hash_code;
size_t d_beidou_dnav_utc_model_sptr_type_hash_code;
size_t d_beidou_dnav_almanac_sptr_type_hash_code;
double d_rinex_version; double d_rinex_version;
double d_rx_time; double d_rx_time;

4
src/algorithms/PVT/libs/geojson_printer.cc
View File

@@ -101,8 +101,8 @@ GeoJSON_Printer::~GeoJSON_Printer()
bool GeoJSON_Printer::set_headers(const std::string& filename, bool time_tag_name) bool GeoJSON_Printer::set_headers(const std::string& filename, bool time_tag_name)
{ {
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time(); const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt); const tm timeinfo = boost::posix_time::to_tm(pt);
if (time_tag_name) if (time_tag_name)
{ {

34
src/algorithms/PVT/libs/gpx_printer.cc
View File

@@ -89,8 +89,8 @@ Gpx_Printer::Gpx_Printer(const std::string& base_path)
bool Gpx_Printer::set_headers(const std::string& filename, bool time_tag_name) bool Gpx_Printer::set_headers(const std::string& filename, bool time_tag_name)
{ {
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time(); const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt); const tm timeinfo = boost::posix_time::to_tm(pt);
if (time_tag_name) if (time_tag_name)
{ {
@@ -146,14 +146,14 @@ bool Gpx_Printer::set_headers(const std::string& filename, bool time_tag_name)
gpx_file << std::setprecision(14); gpx_file << std::setprecision(14);
gpx_file << R"(<?xml version="1.0" encoding="UTF-8"?>)" << '\n' gpx_file << R"(<?xml version="1.0" encoding="UTF-8"?>)" << '\n'
<< R"(<gpx version="1.1" creator="GNSS-SDR")" << '\n' << R"(<gpx version="1.1" creator="GNSS-SDR")" << '\n'
<< indent << "xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd http://www.garmin.com/xmlschemas/GpxExtensions/v3 http://www.garmin.com/xmlschemas/GpxExtensionsv3.xsd http://www.garmin.com/xmlschemas/TrackPointExtension/v2 http://www.garmin.com/xmlschemas/TrackPointExtensionv2.xsd\"" << '\n' << indent << "xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd http://www.garmin.com/xmlschemas/GpxExtensions/v3 http://www.garmin.com/xmlschemas/GpxExtensionsv3.xsd http://www.garmin.com/xmlschemas/TrackPointExtension/v2 http://www.garmin.com/xmlschemas/TrackPointExtensionv2.xsd\"\n"
<< indent << "xmlns=\"http://www.topografix.com/GPX/1/1\"" << '\n' << indent << "xmlns=\"http://www.topografix.com/GPX/1/1\"\n"
<< indent << "xmlns:gpxx=\"http://www.garmin.com/xmlschemas/GpxExtensions/v3\"" << '\n' << indent << "xmlns:gpxx=\"http://www.garmin.com/xmlschemas/GpxExtensions/v3\"\n"
<< indent << "xmlns:gpxtpx=\"http://www.garmin.com/xmlschemas/TrackPointExtension/v2\"" << '\n' << indent << "xmlns:gpxtpx=\"http://www.garmin.com/xmlschemas/TrackPointExtension/v2\"\n"
<< indent << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << '\n' << indent << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">\n"
<< indent << "<trk>" << '\n' << indent << "<trk>\n"
<< indent << indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>" << '\n' << indent << indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>\n"
<< indent << indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>" << '\n' << indent << indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>\n"
<< indent << indent << "<trkseg>\n"; << indent << indent << "<trkseg>\n";
return true; return true;
} }
@@ -170,12 +170,12 @@ bool Gpx_Printer::print_position(const Pvt_Solution* position, bool print_averag
positions_printed = true; positions_printed = true;
double speed_over_ground = position->get_speed_over_ground(); // expressed in m/s const double speed_over_ground = position->get_speed_over_ground(); // expressed in m/s
double course_over_ground = position->get_course_over_ground(); // expressed in deg const double course_over_ground = position->get_course_over_ground(); // expressed in deg
double hdop = position->get_hdop(); const double hdop = position->get_hdop();
double vdop = position->get_vdop(); const double vdop = position->get_vdop();
double pdop = position->get_pdop(); const double pdop = position->get_pdop();
std::string utc_time = to_iso_extended_string(position->get_position_UTC_time()); std::string utc_time = to_iso_extended_string(position->get_position_UTC_time());
if (utc_time.length() < 23) if (utc_time.length() < 23)
{ {
@@ -216,8 +216,8 @@ bool Gpx_Printer::close_file()
{ {
if (gpx_file.is_open()) if (gpx_file.is_open())
{ {
gpx_file << indent << indent << "</trkseg>" << '\n' gpx_file << indent << indent << "</trkseg>\n"
<< indent << "</trk>" << '\n' << indent << "</trk>\n"
<< "</gpx>"; << "</gpx>";
gpx_file.close(); gpx_file.close();
return true; return true;

186
src/algorithms/PVT/libs/kml_printer.cc
View File

@@ -109,8 +109,8 @@ Kml_Printer::Kml_Printer(const std::string& base_path)
bool Kml_Printer::set_headers(const std::string& filename, bool time_tag_name) bool Kml_Printer::set_headers(const std::string& filename, bool time_tag_name)
{ {
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time(); const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt); const tm timeinfo = boost::posix_time::to_tm(pt);
if (time_tag_name) if (time_tag_name)
{ {
@@ -171,57 +171,57 @@ bool Kml_Printer::set_headers(const std::string& filename, bool time_tag_name)
kml_file << R"(<?xml version="1.0" encoding="UTF-8"?>)" << '\n' kml_file << R"(<?xml version="1.0" encoding="UTF-8"?>)" << '\n'
<< R"(<kml xmlns="http://www.opengis.net/kml/2.2" xmlns:gx="http://www.google.com/kml/ext/2.2">)" << '\n' << R"(<kml xmlns="http://www.opengis.net/kml/2.2" xmlns:gx="http://www.google.com/kml/ext/2.2">)" << '\n'
<< indent << "<Document>" << '\n' << indent << "<Document>\n"
<< indent << indent << "<name>GNSS Track</name>" << '\n' << indent << indent << "<name>GNSS Track</name>\n"
<< indent << indent << "<description><![CDATA[" << '\n' << indent << indent << "<description><![CDATA[\n"
<< indent << indent << indent << "<table>" << '\n' << indent << indent << indent << "<table>\n"
<< indent << indent << indent << indent << "<tr><td>GNSS-SDR Receiver position log file created at " << pt << "</td></tr>" << '\n' << indent << indent << indent << indent << "<tr><td>GNSS-SDR Receiver position log file created at " << pt << "</td></tr>\n"
<< indent << indent << indent << indent << "<tr><td>https://gnss-sdr.org/</td></tr>" << '\n' << indent << indent << indent << indent << "<tr><td>https://gnss-sdr.org/</td></tr>\n"
<< indent << indent << indent << "</table>" << '\n' << indent << indent << indent << "</table>\n"
<< indent << indent << "]]></description>" << '\n' << indent << indent << "]]></description>\n"
<< indent << indent << "<!-- Normal track style -->" << '\n' << indent << indent << "<!-- Normal track style -->\n"
<< indent << indent << "<Style id=\"track_n\">" << '\n' << indent << indent << "<Style id=\"track_n\">\n"
<< indent << indent << indent << "<IconStyle>" << '\n' << indent << indent << indent << "<IconStyle>\n"
<< indent << indent << indent << indent << "<color>ff00ffff</color>" << '\n' << indent << indent << indent << indent << "<color>ff00ffff</color>\n"
<< indent << indent << indent << indent << "<scale>0.3</scale>" << '\n' << indent << indent << indent << indent << "<scale>0.3</scale>\n"
<< indent << indent << indent << indent << "<Icon>" << '\n' << indent << indent << indent << indent << "<Icon>\n"
<< indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>" << '\n' << indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>\n"
<< indent << indent << indent << indent << "</Icon>" << '\n' << indent << indent << indent << indent << "</Icon>\n"
<< indent << indent << indent << "</IconStyle>" << '\n' << indent << indent << indent << "</IconStyle>\n"
<< indent << indent << indent << "<LabelStyle>" << '\n' << indent << indent << indent << "<LabelStyle>\n"
<< indent << indent << indent << indent << "<scale>0</scale>" << '\n' << indent << indent << indent << indent << "<scale>0</scale>\n"
<< indent << indent << indent << "</LabelStyle>" << '\n' << indent << indent << indent << "</LabelStyle>\n"
<< indent << indent << "</Style>" << '\n' << indent << indent << "</Style>\n"
<< indent << indent << "<!-- Highlighted track style -->" << '\n' << indent << indent << "<!-- Highlighted track style -->\n"
<< indent << indent << "<Style id=\"track_h\">" << '\n' << indent << indent << "<Style id=\"track_h\">\n"
<< indent << indent << indent << "<IconStyle>" << '\n' << indent << indent << indent << "<IconStyle>\n"
<< indent << indent << indent << indent << "<color>ff00ffff</color>" << '\n' << indent << indent << indent << indent << "<color>ff00ffff</color>\n"
<< indent << indent << indent << indent << "<scale>1</scale>" << '\n' << indent << indent << indent << indent << "<scale>1</scale>\n"
<< indent << indent << indent << indent << "<Icon>" << '\n' << indent << indent << indent << indent << "<Icon>\n"
<< indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>" << '\n' << indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>\n"
<< indent << indent << indent << indent << "</Icon>" << '\n' << indent << indent << indent << indent << "</Icon>\n"
<< indent << indent << indent << "</IconStyle>" << '\n' << indent << indent << indent << "</IconStyle>\n"
<< indent << indent << "</Style>" << '\n' << indent << indent << "</Style>\n"
<< indent << indent << "<StyleMap id=\"track\">" << '\n' << indent << indent << "<StyleMap id=\"track\">\n"
<< indent << indent << indent << "<Pair>" << '\n' << indent << indent << indent << "<Pair>\n"
<< indent << indent << indent << indent << "<key>normal</key>" << '\n' << indent << indent << indent << indent << "<key>normal</key>\n"
<< indent << indent << indent << indent << "<styleUrl>#track_n</styleUrl>" << '\n' << indent << indent << indent << indent << "<styleUrl>#track_n</styleUrl>\n"
<< indent << indent << indent << "</Pair>" << '\n' << indent << indent << indent << "</Pair>\n"
<< indent << indent << indent << "<Pair>" << '\n' << indent << indent << indent << "<Pair>\n"
<< indent << indent << indent << indent << "<key>highlight</key>" << '\n' << indent << indent << indent << indent << "<key>highlight</key>\n"
<< indent << indent << indent << indent << "<styleUrl>#track_h</styleUrl>" << '\n' << indent << indent << indent << indent << "<styleUrl>#track_h</styleUrl>\n"
<< indent << indent << indent << "</Pair>" << '\n' << indent << indent << indent << "</Pair>\n"
<< indent << indent << "</StyleMap>" << '\n' << indent << indent << "</StyleMap>\n"
<< indent << indent << "<Style id=\"yellowLineGreenPoly\">" << '\n' << indent << indent << "<Style id=\"yellowLineGreenPoly\">\n"
<< indent << indent << indent << "<LineStyle>" << '\n' << indent << indent << indent << "<LineStyle>\n"
<< indent << indent << indent << indent << "<color>7f00ffff</color>" << '\n' << indent << indent << indent << indent << "<color>7f00ffff</color>\n"
<< indent << indent << indent << indent << "<width>1</width>" << '\n' << indent << indent << indent << indent << "<width>1</width>\n"
<< indent << indent << indent << "</LineStyle>" << '\n' << indent << indent << indent << "</LineStyle>\n"
<< indent << indent << indent << "<PolyStyle>" << '\n' << indent << indent << indent << "<PolyStyle>\n"
<< indent << indent << indent << indent << "<color>7f00ff00</color>" << '\n' << indent << indent << indent << indent << "<color>7f00ff00</color>\n"
<< indent << indent << indent << "</PolyStyle>" << '\n' << indent << indent << indent << "</PolyStyle>\n"
<< indent << indent << "</Style>" << '\n' << indent << indent << "</Style>\n"
<< indent << indent << "<Folder>" << '\n' << indent << indent << "<Folder>\n"
<< indent << indent << indent << "<name>Points</name>\n"; << indent << indent << indent << "<name>Points</name>\n";
return true; return true;
@@ -239,12 +239,12 @@ bool Kml_Printer::print_position(const Pvt_Solution* position, bool print_averag
positions_printed = true; positions_printed = true;
double speed_over_ground = position->get_speed_over_ground(); // expressed in m/s const double speed_over_ground = position->get_speed_over_ground(); // expressed in m/s
double course_over_ground = position->get_course_over_ground(); // expressed in deg const double course_over_ground = position->get_course_over_ground(); // expressed in deg
double hdop = position->get_hdop(); const double hdop = position->get_hdop();
double vdop = position->get_vdop(); const double vdop = position->get_vdop();
double pdop = position->get_pdop(); const double pdop = position->get_pdop();
std::string utc_time = to_iso_extended_string(position->get_position_UTC_time()); std::string utc_time = to_iso_extended_string(position->get_position_UTC_time());
if (utc_time.length() < 23) if (utc_time.length() < 23)
{ {
@@ -269,30 +269,30 @@ bool Kml_Printer::print_position(const Pvt_Solution* position, bool print_averag
if (kml_file.is_open() && tmp_file.is_open()) if (kml_file.is_open() && tmp_file.is_open())
{ {
point_id++; point_id++;
kml_file << indent << indent << indent << "<Placemark>" << '\n' kml_file << indent << indent << indent << "<Placemark>\n"
<< indent << indent << indent << indent << "<name>" << point_id << "</name>" << '\n' << indent << indent << indent << indent << "<name>" << point_id << "</name>\n"
<< indent << indent << indent << indent << "<snippet/>" << '\n' << indent << indent << indent << indent << "<snippet/>\n"
<< indent << indent << indent << indent << "<description><![CDATA[" << '\n' << indent << indent << indent << indent << "<description><![CDATA[\n"
<< indent << indent << indent << indent << indent << "<table>" << '\n' << indent << indent << indent << indent << indent << "<table>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>Time:</td><td>" << utc_time << "</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>Time:</td><td>" << utc_time << "</td></tr>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>Longitude:</td><td>" << longitude << "</td><td>deg</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>Longitude:</td><td>" << longitude << "</td><td>deg</td></tr>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>Latitude:</td><td>" << latitude << "</td><td>deg</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>Latitude:</td><td>" << latitude << "</td><td>deg</td></tr>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>Altitude:</td><td>" << height << "</td><td>m</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>Altitude:</td><td>" << height << "</td><td>m</td></tr>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>Speed:</td><td>" << speed_over_ground << "</td><td>m/s</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>Speed:</td><td>" << speed_over_ground << "</td><td>m/s</td></tr>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>Course:</td><td>" << course_over_ground << "</td><td>deg</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>Course:</td><td>" << course_over_ground << "</td><td>deg</td></tr>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>HDOP:</td><td>" << hdop << "</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>HDOP:</td><td>" << hdop << "</td></tr>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>VDOP:</td><td>" << vdop << "</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>VDOP:</td><td>" << vdop << "</td></tr>\n"
<< indent << indent << indent << indent << indent << indent << "<tr><td>PDOP:</td><td>" << pdop << "</td></tr>" << '\n' << indent << indent << indent << indent << indent << indent << "<tr><td>PDOP:</td><td>" << pdop << "</td></tr>\n"
<< indent << indent << indent << indent << indent << "</table>" << '\n' << indent << indent << indent << indent << indent << "</table>\n"
<< indent << indent << indent << indent << "]]></description>" << '\n' << indent << indent << indent << indent << "]]></description>\n"
<< indent << indent << indent << indent << "<TimeStamp>" << '\n' << indent << indent << indent << indent << "<TimeStamp>\n"
<< indent << indent << indent << indent << indent << "<when>" << utc_time << "</when>" << '\n' << indent << indent << indent << indent << indent << "<when>" << utc_time << "</when>\n"
<< indent << indent << indent << indent << "</TimeStamp>" << '\n' << indent << indent << indent << indent << "</TimeStamp>\n"
<< indent << indent << indent << indent << "<styleUrl>#track</styleUrl>" << '\n' << indent << indent << indent << indent << "<styleUrl>#track</styleUrl>\n"
<< indent << indent << indent << indent << "<Point>" << '\n' << indent << indent << indent << indent << "<Point>\n"
<< indent << indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << '\n' << indent << indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>\n"
<< indent << indent << indent << indent << indent << "<coordinates>" << longitude << "," << latitude << "," << height << "</coordinates>" << '\n' << indent << indent << indent << indent << indent << "<coordinates>" << longitude << "," << latitude << "," << height << "</coordinates>\n"
<< indent << indent << indent << indent << "</Point>" << '\n' << indent << indent << indent << indent << "</Point>\n"
<< indent << indent << indent << "</Placemark>\n"; << indent << indent << indent << "</Placemark>\n";
tmp_file << indent << indent << indent << indent << indent tmp_file << indent << indent << indent << indent << indent
@@ -311,23 +311,23 @@ bool Kml_Printer::close_file()
tmp_file.close(); tmp_file.close();
kml_file << indent << indent << "</Folder>" kml_file << indent << indent << "</Folder>"
<< indent << indent << "<Placemark>" << '\n' << indent << indent << "<Placemark>\n"
<< indent << indent << indent << "<name>Path</name>" << '\n' << indent << indent << indent << "<name>Path</name>\n"
<< indent << indent << indent << "<styleUrl>#yellowLineGreenPoly</styleUrl>" << '\n' << indent << indent << indent << "<styleUrl>#yellowLineGreenPoly</styleUrl>\n"
<< indent << indent << indent << "<LineString>" << '\n' << indent << indent << indent << "<LineString>\n"
<< indent << indent << indent << indent << "<extrude>0</extrude>" << '\n' << indent << indent << indent << indent << "<extrude>0</extrude>\n"
<< indent << indent << indent << indent << "<tessellate>1</tessellate>" << '\n' << indent << indent << indent << indent << "<tessellate>1</tessellate>\n"
<< indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << '\n' << indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>\n"
<< indent << indent << indent << indent << "<coordinates>\n"; << indent << indent << indent << indent << "<coordinates>\n";
// Copy the contents of tmp_file into kml_file // Copy the contents of tmp_file into kml_file
std::ifstream src(tmp_file_str, std::ios::binary); std::ifstream src(tmp_file_str, std::ios::binary);
kml_file << src.rdbuf(); kml_file << src.rdbuf();
kml_file << indent << indent << indent << indent << "</coordinates>" << '\n' kml_file << indent << indent << indent << indent << "</coordinates>\n"
<< indent << indent << indent << "</LineString>" << '\n' << indent << indent << indent << "</LineString>\n"
<< indent << indent << "</Placemark>" << '\n' << indent << indent << "</Placemark>\n"
<< indent << "</Document>" << '\n' << indent << "</Document>\n"
<< "</kml>"; << "</kml>";
kml_file.close(); kml_file.close();

46
src/algorithms/PVT/libs/nmea_printer.cc
View File

@@ -126,7 +126,7 @@ Nmea_Printer::Nmea_Printer(const std::string& filename, bool flag_nmea_output_fi
Nmea_Printer::~Nmea_Printer() Nmea_Printer::~Nmea_Printer()
{ {
DLOG(INFO) << "NMEA printer destructor called."; DLOG(INFO) << "NMEA printer destructor called.";
auto pos = nmea_file_descriptor.tellp(); const auto pos = nmea_file_descriptor.tellp();
try try
{ {
if (nmea_file_descriptor.is_open()) if (nmea_file_descriptor.is_open())
@@ -170,11 +170,11 @@ int Nmea_Printer::init_serial(const std::string& serial_device)
// clang-format off // clang-format off
struct termios options{}; struct termios options{};
// clang-format on // clang-format on
int64_t BAUD; const int64_t BAUD = B9600; // BAUD = B38400;
int64_t DATABITS; const int64_t DATABITS = CS8;
int64_t STOPBITS; const int64_t STOPBITS = 0;
int64_t PARITYON; const int64_t PARITYON = 0;
int64_t PARITY; const int64_t PARITY = 0;
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC); fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC);
if (fd == -1) if (fd == -1)
@@ -188,13 +188,6 @@ int Nmea_Printer::init_serial(const std::string& serial_device)
} }
tcgetattr(fd, &options); // read serial port options tcgetattr(fd, &options); // read serial port options
BAUD = B9600;
// BAUD = B38400;
DATABITS = CS8;
STOPBITS = 0;
PARITYON = 0;
PARITY = 0;
options.c_cflag = BAUD | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD; options.c_cflag = BAUD | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD;
// enable receiver, set 8 bit data, ignore control lines // enable receiver, set 8 bit data, ignore control lines
// options.c_cflag |= (CLOCAL | CREAD | CS8); // options.c_cflag |= (CLOCAL | CREAD | CS8);
@@ -217,11 +210,6 @@ void Nmea_Printer::close_serial()
bool Nmea_Printer::Print_Nmea_Line(const Rtklib_Solver* pvt_data, bool print_average_values) bool Nmea_Printer::Print_Nmea_Line(const Rtklib_Solver* pvt_data, bool print_average_values)
{ {
std::string GPRMC;
std::string GPGGA;
std::string GPGSA;
std::string GPGSV;
// set the new PVT data // set the new PVT data
d_PVT_data = pvt_data; d_PVT_data = pvt_data;
print_avg_pos = print_average_values; print_avg_pos = print_average_values;
@@ -229,13 +217,13 @@ bool Nmea_Printer::Print_Nmea_Line(const Rtklib_Solver* pvt_data, bool print_ave
// generate the NMEA sentences // generate the NMEA sentences
// GPRMC // GPRMC
GPRMC = get_GPRMC(); const std::string GPRMC = get_GPRMC();
// GPGGA (Global Positioning System Fixed Data) // GPGGA (Global Positioning System Fixed Data)
GPGGA = get_GPGGA(); const std::string GPGGA = get_GPGGA();
// GPGSA // GPGSA
GPGSA = get_GPGSA(); const std::string GPGSA = get_GPGSA();
// GPGSV // GPGSV
GPGSV = get_GPGSV(); const std::string GPGSV = get_GPGSV();
// write to log file // write to log file
if (d_flag_nmea_output_file) if (d_flag_nmea_output_file)
@@ -311,7 +299,7 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
north = true; north = true;
} }
int deg = static_cast<int>(lat); const int deg = static_cast<int>(lat);
double mins = lat - static_cast<double>(deg); double mins = lat - static_cast<double>(deg);
mins *= 60.0; mins *= 60.0;
std::ostringstream out_string; std::ostringstream out_string;
@@ -348,7 +336,7 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
{ {
east = true; east = true;
} }
int deg = static_cast<int>(longitude); const int deg = static_cast<int>(longitude);
double mins = longitude - static_cast<double>(deg); double mins = longitude - static_cast<double>(deg);
mins *= 60.0; mins *= 60.0;
std::ostringstream out_string; std::ostringstream out_string;
@@ -378,11 +366,11 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
// UTC Time: hhmmss.sss // UTC Time: hhmmss.sss
std::stringstream sentence_str; std::stringstream sentence_str;
boost::posix_time::time_duration td = d_position_UTC_time.time_of_day(); const boost::posix_time::time_duration td = d_position_UTC_time.time_of_day();
int utc_hours = td.hours(); const int utc_hours = td.hours();
int utc_mins = td.minutes(); const int utc_mins = td.minutes();
int utc_seconds = td.seconds(); const int utc_seconds = td.seconds();
auto utc_milliseconds = static_cast<int>(td.total_milliseconds() - td.total_seconds() * 1000); const auto utc_milliseconds = static_cast<int>(td.total_milliseconds() - td.total_seconds() * 1000);
if (utc_hours < 10) if (utc_hours < 10)
{ {

839
src/algorithms/PVT/libs/rinex_printer.cc
View File

File diff suppressed because it is too large Load Diff

1274
src/algorithms/PVT/libs/rtcm.cc
View File

File diff suppressed because it is too large Load Diff

46
src/algorithms/PVT/libs/rtcm_printer.cc
View File

@@ -60,8 +60,8 @@ namespace errorlib = boost::system;
Rtcm_Printer::Rtcm_Printer(const std::string& filename, bool flag_rtcm_file_dump, bool flag_rtcm_server, bool flag_rtcm_tty_port, uint16_t rtcm_tcp_port, uint16_t rtcm_station_id, const std::string& rtcm_dump_devname, bool time_tag_name, const std::string& base_path) Rtcm_Printer::Rtcm_Printer(const std::string& filename, bool flag_rtcm_file_dump, bool flag_rtcm_server, bool flag_rtcm_tty_port, uint16_t rtcm_tcp_port, uint16_t rtcm_station_id, const std::string& rtcm_dump_devname, bool time_tag_name, const std::string& base_path)
{ {
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time(); const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt); const tm timeinfo = boost::posix_time::to_tm(pt);
d_rtcm_file_dump = flag_rtcm_file_dump; d_rtcm_file_dump = flag_rtcm_file_dump;
rtcm_base_path = base_path; rtcm_base_path = base_path;
if (d_rtcm_file_dump) if (d_rtcm_file_dump)
@@ -200,8 +200,7 @@ Rtcm_Printer::~Rtcm_Printer()
} }
if (rtcm_file_descriptor.is_open()) if (rtcm_file_descriptor.is_open())
{ {
int64_t pos; const auto pos = rtcm_file_descriptor.tellp();
pos = rtcm_file_descriptor.tellp();
try try
{ {
rtcm_file_descriptor.close(); rtcm_file_descriptor.close();
@@ -232,7 +231,7 @@ Rtcm_Printer::~Rtcm_Printer()
bool Rtcm_Printer::Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables) bool Rtcm_Printer::Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables)
{ {
std::string m1001 = rtcm->print_MT1001(gps_eph, obs_time, observables, station_id); const std::string m1001 = rtcm->print_MT1001(gps_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1001); Rtcm_Printer::Print_Message(m1001);
return true; return true;
} }
@@ -240,7 +239,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1001(const Gps_Ephemeris& gps_eph, double obs_ti
bool Rtcm_Printer::Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables) bool Rtcm_Printer::Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables)
{ {
std::string m1002 = rtcm->print_MT1002(gps_eph, obs_time, observables, station_id); const std::string m1002 = rtcm->print_MT1002(gps_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1002); Rtcm_Printer::Print_Message(m1002);
return true; return true;
} }
@@ -248,7 +247,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1002(const Gps_Ephemeris& gps_eph, double obs_ti
bool Rtcm_Printer::Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables) bool Rtcm_Printer::Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables)
{ {
std::string m1003 = rtcm->print_MT1003(gps_eph, cnav_eph, obs_time, observables, station_id); const std::string m1003 = rtcm->print_MT1003(gps_eph, cnav_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1003); Rtcm_Printer::Print_Message(m1003);
return true; return true;
} }
@@ -256,7 +255,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1003(const Gps_Ephemeris& gps_eph, const Gps_CNA
bool Rtcm_Printer::Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables) bool Rtcm_Printer::Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNAV_Ephemeris& cnav_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables)
{ {
std::string m1003 = rtcm->print_MT1004(gps_eph, cnav_eph, obs_time, observables, station_id); const std::string m1003 = rtcm->print_MT1004(gps_eph, cnav_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1003); Rtcm_Printer::Print_Message(m1003);
return true; return true;
} }
@@ -264,7 +263,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1004(const Gps_Ephemeris& gps_eph, const Gps_CNA
bool Rtcm_Printer::Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables) bool Rtcm_Printer::Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables)
{ {
std::string m1009 = rtcm->print_MT1009(glonass_gnav_eph, obs_time, observables, station_id); const std::string m1009 = rtcm->print_MT1009(glonass_gnav_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1009); Rtcm_Printer::Print_Message(m1009);
return true; return true;
} }
@@ -272,7 +271,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1009(const Glonass_Gnav_Ephemeris& glonass_gnav_
bool Rtcm_Printer::Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables) bool Rtcm_Printer::Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables)
{ {
std::string m1010 = rtcm->print_MT1010(glonass_gnav_eph, obs_time, observables, station_id); const std::string m1010 = rtcm->print_MT1010(glonass_gnav_eph, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1010); Rtcm_Printer::Print_Message(m1010);
return true; return true;
} }
@@ -280,7 +279,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1010(const Glonass_Gnav_Ephemeris& glonass_gnav_
bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables) bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables)
{ {
std::string m1011 = rtcm->print_MT1011(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id); const std::string m1011 = rtcm->print_MT1011(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1011); Rtcm_Printer::Print_Message(m1011);
return true; return true;
} }
@@ -288,7 +287,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1011(const Glonass_Gnav_Ephemeris& glonass_gnav_
bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables) bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_ephL1, const Glonass_Gnav_Ephemeris& glonass_gnav_ephL2, double obs_time, const std::map<int32_t, Gnss_Synchro>& observables)
{ {
std::string m1012 = rtcm->print_MT1012(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id); const std::string m1012 = rtcm->print_MT1012(glonass_gnav_ephL1, glonass_gnav_ephL2, obs_time, observables, station_id);
Rtcm_Printer::Print_Message(m1012); Rtcm_Printer::Print_Message(m1012);
return true; return true;
} }
@@ -296,7 +295,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1012(const Glonass_Gnav_Ephemeris& glonass_gnav_
bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris& gps_eph) bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris& gps_eph)
{ {
std::string m1019 = rtcm->print_MT1019(gps_eph); const std::string m1019 = rtcm->print_MT1019(gps_eph);
Rtcm_Printer::Print_Message(m1019); Rtcm_Printer::Print_Message(m1019);
return true; return true;
} }
@@ -304,7 +303,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1019(const Gps_Ephemeris& gps_eph)
bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model) bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model)
{ {
std::string m1020 = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model); const std::string m1020 = rtcm->print_MT1020(glonass_gnav_eph, glonass_gnav_utc_model);
Rtcm_Printer::Print_Message(m1020); Rtcm_Printer::Print_Message(m1020);
return true; return true;
} }
@@ -312,7 +311,7 @@ bool Rtcm_Printer::Print_Rtcm_MT1020(const Glonass_Gnav_Ephemeris& glonass_gnav_
bool Rtcm_Printer::Print_Rtcm_MT1045(const Galileo_Ephemeris& gal_eph) bool Rtcm_Printer::Print_Rtcm_MT1045(const Galileo_Ephemeris& gal_eph)
{ {
std::string m1045 = rtcm->print_MT1045(gal_eph); const std::string m1045 = rtcm->print_MT1045(gal_eph);
Rtcm_Printer::Print_Message(m1045); Rtcm_Printer::Print_Message(m1045);
return true; return true;
} }
@@ -378,11 +377,11 @@ int Rtcm_Printer::init_serial(const std::string& serial_device)
// clang-format off // clang-format off
struct termios options{}; struct termios options{};
// clang-format on // clang-format on
int64_t BAUD; const int64_t BAUD = B9600; // BAUD = B38400;
int64_t DATABITS; const int64_t DATABITS = CS8;
int64_t STOPBITS; const int64_t STOPBITS = 0;
int64_t PARITYON; const int64_t PARITYON = 0;
int64_t PARITY; const int64_t PARITY = 0;
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC); fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC);
if (fd == -1) if (fd == -1)
@@ -396,13 +395,6 @@ int Rtcm_Printer::init_serial(const std::string& serial_device)
} }
tcgetattr(fd, &options); // read serial port options tcgetattr(fd, &options); // read serial port options
BAUD = B9600;
// BAUD = B38400;
DATABITS = CS8;
STOPBITS = 0;
PARITYON = 0;
PARITY = 0;
options.c_cflag = BAUD | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD; options.c_cflag = BAUD | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD;
// enable receiver, set 8 bit data, ignore control lines // enable receiver, set 8 bit data, ignore control lines
// options.c_cflag |= (CLOCAL | CREAD | CS8); // options.c_cflag |= (CLOCAL | CREAD | CS8);

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

@@ -61,15 +61,15 @@ namespace errorlib = boost::system;
#endif #endif
Rtklib_Solver::Rtklib_Solver(int nchannels, const std::string &dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, const rtk_t &rtk) Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, int nchannels, const std::string &dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat)
{ {
// init empty ephemeris for all the available GNSS channels // init empty ephemeris for all the available GNSS channels
rtk_ = rtk;
d_nchannels = nchannels; d_nchannels = nchannels;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file; d_flag_dump_enabled = flag_dump_to_file;
d_flag_dump_mat_enabled = flag_dump_to_mat; d_flag_dump_mat_enabled = flag_dump_to_mat;
this->set_averaging_flag(false); this->set_averaging_flag(false);
rtk_ = rtk;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true) if (d_flag_dump_enabled == true)
@@ -96,7 +96,7 @@ Rtklib_Solver::~Rtklib_Solver()
DLOG(INFO) << "Rtklib_Solver destructor called."; DLOG(INFO) << "Rtklib_Solver destructor called.";
if (d_dump_file.is_open() == true) if (d_dump_file.is_open() == true)
{ {
auto pos = d_dump_file.tellp(); const auto pos = d_dump_file.tellp();
try try
{ {
d_dump_file.close(); d_dump_file.close();
@@ -129,19 +129,18 @@ Rtklib_Solver::~Rtklib_Solver()
} }
bool Rtklib_Solver::save_matfile() bool Rtklib_Solver::save_matfile() const
{ {
// READ DUMP FILE // READ DUMP FILE
std::string dump_filename = d_dump_filename; const std::string dump_filename = d_dump_filename;
std::ifstream::pos_type size; const int32_t number_of_double_vars = 21;
int32_t number_of_double_vars = 21; const int32_t number_of_uint32_vars = 2;
int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3;
int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2;
int32_t number_of_float_vars = 2; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars +
int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + sizeof(uint32_t) * number_of_uint32_vars +
sizeof(uint32_t) * number_of_uint32_vars + sizeof(uint8_t) * number_of_uint8_vars +
sizeof(uint8_t) * number_of_uint8_vars + sizeof(float) * number_of_float_vars;
sizeof(float) * number_of_float_vars;
std::ifstream dump_file; std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try try
@@ -158,7 +157,7 @@ bool Rtklib_Solver::save_matfile()
if (dump_file.is_open()) if (dump_file.is_open())
{ {
std::cout << "Generating .mat file for " << dump_filename << '\n'; std::cout << "Generating .mat file for " << dump_filename << '\n';
size = dump_file.tellg(); const std::ifstream::pos_type size = dump_file.tellg();
num_epoch = static_cast<int64_t>(size) / static_cast<int64_t>(epoch_size_bytes); num_epoch = static_cast<int64_t>(size) / static_cast<int64_t>(epoch_size_bytes);
dump_file.seekg(0, std::ios::beg); dump_file.seekg(0, std::ios::beg);
} }
@@ -433,7 +432,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{ {
case 'G': case 'G':
{ {
std::string sig_(gnss_observables_iter->second.Signal); const std::string sig_(gnss_observables_iter->second.Signal);
if (sig_ == "1C") if (sig_ == "1C")
{ {
band1 = true; band1 = true;
@@ -462,7 +461,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{ {
case 'E': case 'E':
{ {
std::string sig_(gnss_observables_iter->second.Signal); const std::string sig_(gnss_observables_iter->second.Signal);
// Galileo E1 // Galileo E1
if (sig_ == "1B") if (sig_ == "1B")
{ {
@@ -473,7 +472,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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); 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', {}, {}, {}, {}, {}, {}}; obsd_t newobs{};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
galileo_ephemeris_iter->second.WN_5, galileo_ephemeris_iter->second.WN_5,
@@ -512,7 +511,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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); 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
auto default_code_ = static_cast<unsigned char>(CODE_NONE); const auto default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_}, {default_code_, default_code_, default_code_},
{}, {0.0, 0.0, 0.0}, {}}; {}, {0.0, 0.0, 0.0}, {}};
@@ -543,7 +542,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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, this->is_pre_2009()); eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second, this->is_pre_2009());
// 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', {}, {}, {}, {}, {}, {}}; obsd_t newobs{};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
gps_ephemeris_iter->second.i_GPS_week, gps_ephemeris_iter->second.i_GPS_week,
@@ -589,7 +588,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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); 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
auto default_code_ = static_cast<unsigned char>(CODE_NONE); const auto default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_}, {default_code_, default_code_, default_code_},
{}, {0.0, 0.0, 0.0}, {}}; {}, {0.0, 0.0, 0.0}, {}};
@@ -636,7 +635,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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); 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
auto default_code_ = static_cast<unsigned char>(CODE_NONE); const auto default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_}, {default_code_, default_code_, default_code_},
{}, {0.0, 0.0, 0.0}, {}}; {}, {0.0, 0.0, 0.0}, {}};
@@ -656,7 +655,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
} }
case 'R': // TODO This should be using rtk lib nomenclature case 'R': // TODO This should be using rtk lib nomenclature
{ {
std::string sig_(gnss_observables_iter->second.Signal); const std::string sig_(gnss_observables_iter->second.Signal);
// GLONASS GNAV L1 // GLONASS GNAV L1
if (sig_ == "1G") if (sig_ == "1G")
{ {
@@ -667,7 +666,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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); 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', {}, {}, {}, {}, {}, {}}; obsd_t newobs{};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
glonass_gnav_ephemeris_iter->second.d_WN, glonass_gnav_ephemeris_iter->second.d_WN,
@@ -705,7 +704,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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); 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', {}, {}, {}, {}, {}, {}}; obsd_t newobs{};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
glonass_gnav_ephemeris_iter->second.d_WN, glonass_gnav_ephemeris_iter->second.d_WN,
@@ -724,7 +723,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{ {
// BEIDOU B1I // BEIDOU B1I
// - find the ephemeris for the current BEIDOU SV observation. The SV PRN ID is the map key // - find the ephemeris for the current BEIDOU SV observation. The SV PRN ID is the map key
std::string sig_(gnss_observables_iter->second.Signal); const std::string sig_(gnss_observables_iter->second.Signal);
if (sig_ == "B1") if (sig_ == "B1")
{ {
beidou_ephemeris_iter = beidou_dnav_ephemeris_map.find(gnss_observables_iter->second.PRN); beidou_ephemeris_iter = beidou_dnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
@@ -733,7 +732,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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(beidou_ephemeris_iter->second); eph_data[valid_obs] = eph_to_rtklib(beidou_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', {}, {}, {}, {}, {}, {}}; obsd_t newobs{};
obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
beidou_ephemeris_iter->second.i_BEIDOU_week + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, beidou_ephemeris_iter->second.i_BEIDOU_week + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET,
@@ -770,7 +769,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
// 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(beidou_ephemeris_iter->second); eph_data[valid_obs] = eph_to_rtklib(beidou_ephemeris_iter->second);
// convert observation from GNSS-SDR class to RTKLIB structure // convert observation from GNSS-SDR class to RTKLIB structure
auto default_code_ = static_cast<unsigned char>(CODE_NONE); const auto default_code_ = static_cast<unsigned char>(CODE_NONE);
obsd_t newobs = {{0, 0}, '0', '0', {}, {}, obsd_t newobs = {{0, 0}, '0', '0', {}, {},
{default_code_, default_code_, default_code_}, {default_code_, default_code_, default_code_},
{}, {0.0, 0.0, 0.0}, {}}; {}, {0.0, 0.0, 0.0}, {}};
@@ -981,12 +980,11 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
DLOG(INFO) << "RTKLIB Position at RX TOW = " << gnss_observables_map.begin()->second.RX_time 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[0] << ", " << rx_position_and_time[1] << ", " << rx_position_and_time[2] << ", " << rx_position_and_time[4]; << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time[0] << ", " << rx_position_and_time[1] << ", " << rx_position_and_time[2] << ", " << rx_position_and_time[4];
boost::posix_time::ptime p_time;
// gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); // Corrected RX Time (Non integer multiply of 1 ms of granularity) // gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); // Corrected RX Time (Non integer multiply of 1 ms of granularity)
// Uncorrected RX Time (integer multiply of 1 ms and the same observables time reported in RTCM and RINEX) // Uncorrected RX Time (integer multiply of 1 ms and the same observables time reported in RTCM and RINEX)
gtime_t rtklib_time = timeadd(pvt_sol.time, rx_position_and_time[3]); // uncorrected rx time const gtime_t rtklib_time = timeadd(pvt_sol.time, rx_position_and_time[3]); // uncorrected rx time
gtime_t rtklib_utc_time = gpst2utc(rtklib_time); const gtime_t rtklib_utc_time = gpst2utc(rtklib_time);
p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); boost::posix_time::ptime p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
p_time += boost::posix_time::microseconds(static_cast<long>(round(rtklib_utc_time.sec * 1e6))); // NOLINT(google-runtime-int) p_time += boost::posix_time::microseconds(static_cast<long>(round(rtklib_utc_time.sec * 1e6))); // NOLINT(google-runtime-int)
this->set_position_UTC_time(p_time); this->set_position_UTC_time(p_time);
@@ -1048,7 +1046,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
this->set_rx_vel({enuv[0], enuv[1], enuv[2]}); this->set_rx_vel({enuv[0], enuv[1], enuv[2]});
double clock_drift_ppm = pvt_sol.dtr[5] / SPEED_OF_LIGHT_M_S * 1e6; const double clock_drift_ppm = pvt_sol.dtr[5] / SPEED_OF_LIGHT_M_S * 1e6;
this->set_clock_drift_ppm(clock_drift_ppm); this->set_clock_drift_ppm(clock_drift_ppm);
// User clock drift [ppm] // User clock drift [ppm]

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

@@ -68,7 +68,7 @@
class Rtklib_Solver : public Pvt_Solution class Rtklib_Solver : public Pvt_Solution
{ {
public: public:
Rtklib_Solver(int nchannels, const std::string& dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, const rtk_t& rtk); Rtklib_Solver(const rtk_t& rtk, int nchannels, const std::string& dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat);
~Rtklib_Solver(); ~Rtklib_Solver();
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging); bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging);
@@ -107,7 +107,7 @@ public:
std::map<int, Beidou_Dnav_Almanac> beidou_dnav_almanac_map; std::map<int, Beidou_Dnav_Almanac> beidou_dnav_almanac_map;
private: private:
bool save_matfile(); bool save_matfile() const;
std::array<obsd_t, MAXOBS> obs_data{}; std::array<obsd_t, MAXOBS> obs_data{};
std::array<double, 4> dop_{}; std::array<double, 4> dop_{};

26
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@@ -210,7 +210,7 @@ void pcps_acquisition::set_local_code(std::complex<float>* code)
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
if (d_acq_parameters.bit_transition_flag) if (d_acq_parameters.bit_transition_flag)
{ {
int32_t offset = d_fft_size / 2; const int32_t offset = d_fft_size / 2;
std::fill_n(d_fft_if->get_inbuf(), offset, gr_complex(0.0, 0.0)); std::fill_n(d_fft_if->get_inbuf(), offset, gr_complex(0.0, 0.0));
memcpy(d_fft_if->get_inbuf() + offset, code, sizeof(gr_complex) * offset); memcpy(d_fft_if->get_inbuf() + offset, code, sizeof(gr_complex) * offset);
} }
@@ -309,7 +309,7 @@ void pcps_acquisition::init()
if (d_dump) if (d_dump)
{ {
uint32_t effective_fft_size = (d_acq_parameters.bit_transition_flag ? (d_fft_size / 2) : d_fft_size); const uint32_t effective_fft_size = (d_acq_parameters.bit_transition_flag ? (d_fft_size / 2) : d_fft_size);
d_grid = arma::fmat(effective_fft_size, d_num_doppler_bins, arma::fill::zeros); d_grid = arma::fmat(effective_fft_size, d_num_doppler_bins, arma::fill::zeros);
d_narrow_grid = arma::fmat(effective_fft_size, d_num_doppler_bins_step2, arma::fill::zeros); d_narrow_grid = arma::fmat(effective_fft_size, d_num_doppler_bins_step2, arma::fill::zeros);
} }
@@ -320,7 +320,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs()
{ {
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
int32_t doppler = -static_cast<int32_t>(d_acq_parameters.doppler_max) + d_doppler_center + d_doppler_step * doppler_index; const int32_t doppler = -static_cast<int32_t>(d_acq_parameters.doppler_max) + d_doppler_center + d_doppler_step * doppler_index;
update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], static_cast<float>(d_doppler_bias + doppler)); update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], static_cast<float>(d_doppler_bias + doppler));
} }
} }
@@ -330,7 +330,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs_step2()
{ {
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++) for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++)
{ {
float doppler = (static_cast<float>(doppler_index) - static_cast<float>(floor(d_num_doppler_bins_step2 / 2.0))) * d_acq_parameters.doppler_step2; const float doppler = (static_cast<float>(doppler_index) - static_cast<float>(floor(d_num_doppler_bins_step2 / 2.0))) * d_acq_parameters.doppler_step2;
update_local_carrier(d_grid_doppler_wipeoffs_step_two[doppler_index], d_doppler_center_step_two + doppler); update_local_carrier(d_grid_doppler_wipeoffs_step_two[doppler_index], d_doppler_center_step_two + doppler);
} }
} }
@@ -620,7 +620,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
// Initialize acquisition algorithm // Initialize acquisition algorithm
int32_t doppler = 0; int32_t doppler = 0;
uint32_t indext = 0U; uint32_t indext = 0U;
int32_t effective_fft_size = (d_acq_parameters.bit_transition_flag ? d_fft_size / 2 : d_fft_size); const int32_t effective_fft_size = (d_acq_parameters.bit_transition_flag ? d_fft_size / 2 : d_fft_size);
if (d_cshort) if (d_cshort)
{ {
volk_gnsssdr_16ic_convert_32fc(d_data_buffer.data(), d_data_buffer_sc.data(), d_consumed_samples); volk_gnsssdr_16ic_convert_32fc(d_data_buffer.data(), d_data_buffer_sc.data(), d_consumed_samples);
@@ -666,7 +666,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
d_ifft->execute(); d_ifft->execute();
// Compute squared magnitude (and accumulate in case of non-coherent integration) // Compute squared magnitude (and accumulate in case of non-coherent integration)
size_t offset = (d_acq_parameters.bit_transition_flag ? effective_fft_size : 0); const size_t offset = (d_acq_parameters.bit_transition_flag ? effective_fft_size : 0);
if (d_num_noncoherent_integrations_counter == 1) if (d_num_noncoherent_integrations_counter == 1)
{ {
volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index].data(), d_ifft->get_outbuf() + offset, effective_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index].data(), d_ifft->get_outbuf() + offset, effective_fft_size);
@@ -724,7 +724,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
size_t offset = (d_acq_parameters.bit_transition_flag ? effective_fft_size : 0); const size_t offset = (d_acq_parameters.bit_transition_flag ? effective_fft_size : 0);
if (d_num_noncoherent_integrations_counter == 1) if (d_num_noncoherent_integrations_counter == 1)
{ {
volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index].data(), d_ifft->get_outbuf() + offset, effective_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index].data(), d_ifft->get_outbuf() + offset, effective_fft_size);
@@ -811,7 +811,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
} }
d_state = 0; d_state = 0;
d_active = false; d_active = false;
bool was_step_two = d_step_two; const bool was_step_two = d_step_two;
d_step_two = false; d_step_two = false;
if (was_step_two) if (was_step_two)
{ {
@@ -849,7 +849,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
else else
{ {
d_state = 0; // Negative acquisition d_state = 0; // Negative acquisition
bool was_step_two = d_step_two; const bool was_step_two = d_step_two;
d_step_two = false; d_step_two = false;
if (was_step_two) if (was_step_two)
{ {
@@ -884,17 +884,17 @@ bool pcps_acquisition::start()
void pcps_acquisition::calculate_threshold() void pcps_acquisition::calculate_threshold()
{ {
float pfa = (d_step_two ? d_acq_parameters.pfa2 : d_acq_parameters.pfa); const float pfa = (d_step_two ? d_acq_parameters.pfa2 : d_acq_parameters.pfa);
if (pfa <= 0.0) if (pfa <= 0.0)
{ {
return; return;
} }
auto effective_fft_size = static_cast<int>(d_acq_parameters.bit_transition_flag ? (d_fft_size / 2) : d_fft_size); const auto effective_fft_size = static_cast<int>(d_acq_parameters.bit_transition_flag ? (d_fft_size / 2) : d_fft_size);
int num_doppler_bins = (d_step_two ? d_num_doppler_bins_step2 : d_num_doppler_bins); const int num_doppler_bins = (d_step_two ? d_num_doppler_bins_step2 : d_num_doppler_bins);
int num_bins = effective_fft_size * num_doppler_bins; const int num_bins = effective_fft_size * num_doppler_bins;
d_threshold = static_cast<float>(2.0 * boost::math::gamma_p_inv(2.0 * d_acq_parameters.max_dwells, std::pow(1.0 - pfa, 1.0 / static_cast<float>(num_bins)))); d_threshold = static_cast<float>(2.0 * boost::math::gamma_p_inv(2.0 * d_acq_parameters.max_dwells, std::pow(1.0 - pfa, 1.0 / static_cast<float>(num_bins))));
} }

6
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc
View File

@@ -169,7 +169,7 @@ void pcps_acquisition_fpga::acquisition_core(uint32_t num_doppler_bins, uint32_t
float secondpeak = 0.0; float secondpeak = 0.0;
uint32_t total_block_exp; uint32_t total_block_exp;
uint64_t initial_sample; uint64_t initial_sample;
int32_t doppler;
d_acquisition_fpga->set_doppler_sweep(num_doppler_bins, doppler_step, doppler_min); d_acquisition_fpga->set_doppler_sweep(num_doppler_bins, doppler_step, doppler_min);
d_acquisition_fpga->run_acquisition(); d_acquisition_fpga->run_acquisition();
d_acquisition_fpga->read_acquisition_results(&indext, d_acquisition_fpga->read_acquisition_results(&indext,
@@ -180,7 +180,7 @@ void pcps_acquisition_fpga::acquisition_core(uint32_t num_doppler_bins, uint32_t
&d_doppler_index, &d_doppler_index,
&total_block_exp); &total_block_exp);
doppler = static_cast<int32_t>(doppler_min) + doppler_step * (d_doppler_index - 1); const auto doppler = static_cast<int32_t>(doppler_min) + doppler_step * (d_doppler_index - 1);
if (total_block_exp > d_total_block_exp) if (total_block_exp > d_total_block_exp)
{ {
@@ -276,7 +276,7 @@ void pcps_acquisition_fpga::set_active(bool active)
d_state = 0; // Positive acquisition d_state = 0; // Positive acquisition
break; break;
} }
num_second_acq = num_second_acq + 1; num_second_acq += 1;
} }
d_acquisition_fpga->close_device(); d_acquisition_fpga->close_device();
if (d_test_statistics <= d_threshold) if (d_test_statistics <= d_threshold)

29
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
View File

@@ -158,7 +158,7 @@ hybrid_observables_gs::~hybrid_observables_gs()
DLOG(INFO) << "Observables block destructor called."; DLOG(INFO) << "Observables block destructor called.";
if (d_dump_file.is_open()) if (d_dump_file.is_open())
{ {
auto pos = d_dump_file.tellp(); const auto pos = d_dump_file.tellp();
try try
{ {
d_dump_file.close(); d_dump_file.close();
@@ -196,10 +196,9 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
try try
{ {
if (pmt::any_ref(msg).type() == typeid(double)) if (pmt::any_ref(msg).type().hash_code() == d_double_type_hash_code)
{ {
double new_rx_clock_offset_s; const auto new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg));
new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg));
d_T_rx_TOW_ms = d_T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0)); d_T_rx_TOW_ms = d_T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0));
// align the receiver clock to integer multiple of 20 ms // align the receiver clock to integer multiple of 20 ms
if (d_T_rx_TOW_ms % 20) if (d_T_rx_TOW_ms % 20)
@@ -227,8 +226,8 @@ int32_t hybrid_observables_gs::save_matfile() const
// READ DUMP FILE // READ DUMP FILE
std::string dump_filename = d_dump_filename; std::string dump_filename = d_dump_filename;
std::ifstream::pos_type size; std::ifstream::pos_type size;
int32_t number_of_double_vars = 7; const int32_t number_of_double_vars = 7;
int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels_out; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels_out;
std::ifstream dump_file; std::ifstream dump_file;
std::cout << "Generating .mat file for " << dump_filename << '\n'; std::cout << "Generating .mat file for " << dump_filename << '\n';
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
@@ -416,11 +415,11 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, uint3
interpolated_obs = d_gnss_synchro_history->get(ch, nearest_element); interpolated_obs = d_gnss_synchro_history->get(ch, nearest_element);
// 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1) // 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1)
double T_rx_s = static_cast<double>(rx_clock) / static_cast<double>(interpolated_obs.fs); const double T_rx_s = static_cast<double>(rx_clock) / static_cast<double>(interpolated_obs.fs);
double time_factor = (T_rx_s - d_gnss_synchro_history->get(ch, t1_idx).RX_time) / const double time_factor = (T_rx_s - d_gnss_synchro_history->get(ch, t1_idx).RX_time) /
(d_gnss_synchro_history->get(ch, t2_idx).RX_time - (d_gnss_synchro_history->get(ch, t2_idx).RX_time -
d_gnss_synchro_history->get(ch, t1_idx).RX_time); d_gnss_synchro_history->get(ch, t1_idx).RX_time);
// CARRIER PHASE INTERPOLATION // CARRIER PHASE INTERPOLATION
interpolated_obs.Carrier_phase_rads = d_gnss_synchro_history->get(ch, t1_idx).Carrier_phase_rads + (d_gnss_synchro_history->get(ch, t2_idx).Carrier_phase_rads - d_gnss_synchro_history->get(ch, t1_idx).Carrier_phase_rads) * time_factor; interpolated_obs.Carrier_phase_rads = d_gnss_synchro_history->get(ch, t1_idx).Carrier_phase_rads + (d_gnss_synchro_history->get(ch, t2_idx).Carrier_phase_rads - d_gnss_synchro_history->get(ch, t1_idx).Carrier_phase_rads) * time_factor;
@@ -517,8 +516,8 @@ void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data) con
// std::cout.precision(17); // std::cout.precision(17);
// std::cout << " d_T_rx_TOW_ms: " << static_cast<double>(d_T_rx_TOW_ms) << '\n'; // std::cout << " d_T_rx_TOW_ms: " << static_cast<double>(d_T_rx_TOW_ms) << '\n';
std::vector<Gnss_Synchro>::iterator it; std::vector<Gnss_Synchro>::iterator it;
auto current_T_rx_TOW_ms = static_cast<double>(d_T_rx_TOW_ms); const auto current_T_rx_TOW_ms = static_cast<double>(d_T_rx_TOW_ms);
double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0; const double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0;
for (it = data.begin(); it != data.end(); it++) for (it = data.begin(); it != data.end(); it++)
{ {
if (it->Flag_valid_word) if (it->Flag_valid_word)
@@ -591,8 +590,8 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
{ {
// 2. Compute the smoothed pseudorange for this channel // 2. Compute the smoothed pseudorange for this channel
// Hatch filter algorithm (https://insidegnss.com/can-you-list-all-the-properties-of-the-carrier-smoothing-filter/) // Hatch filter algorithm (https://insidegnss.com/can-you-list-all-the-properties-of-the-carrier-smoothing-filter/)
double r_sm = d_channel_last_pseudorange_smooth[it->Channel_ID]; const double r_sm = d_channel_last_pseudorange_smooth[it->Channel_ID];
double factor = ((d_smooth_filter_M - 1.0) / d_smooth_filter_M); const double factor = ((d_smooth_filter_M - 1.0) / d_smooth_filter_M);
it->Pseudorange_m = factor * r_sm + (1.0 / d_smooth_filter_M) * it->Pseudorange_m + wavelength_m * (factor / TWO_PI) * (it->Carrier_phase_rads - d_channel_last_carrier_phase_rads[it->Channel_ID]); it->Pseudorange_m = factor * r_sm + (1.0 / d_smooth_filter_M) * it->Pseudorange_m + wavelength_m * (factor / TWO_PI) * (it->Carrier_phase_rads - d_channel_last_carrier_phase_rads[it->Channel_ID]);
} }
d_channel_last_pseudorange_smooth[it->Channel_ID] = it->Pseudorange_m; d_channel_last_pseudorange_smooth[it->Channel_ID] = it->Pseudorange_m;
@@ -704,7 +703,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
{ {
for (uint32_t n = 0; n < d_nchannels_out; n++) for (uint32_t n = 0; n < d_nchannels_out; n++)
{ {
std::shared_ptr<Gnss_Synchro> gnss_synchro_sptr = std::make_shared<Gnss_Synchro>(epoch_data[n]); const std::shared_ptr<Gnss_Synchro> gnss_synchro_sptr = std::make_shared<Gnss_Synchro>(epoch_data[n]);
// publish valid gnss_synchro to the gnss_flowgraph channel status monitor // publish valid gnss_synchro to the gnss_flowgraph channel status monitor
this->message_port_pub(pmt::mp("status"), pmt::make_any(gnss_synchro_sptr)); this->message_port_pub(pmt::mp("status"), pmt::make_any(gnss_synchro_sptr));
} }

4
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
View File

@@ -27,11 +27,13 @@
#include <boost/circular_buffer.hpp> // for boost::circular_buffer #include <boost/circular_buffer.hpp> // for boost::circular_buffer
#include <gnuradio/block.h> // for block #include <gnuradio/block.h> // for block
#include <gnuradio/types.h> // for gr_vector_int #include <gnuradio/types.h> // for gr_vector_int
#include <cstddef> // for size_t
#include <cstdint> // for int32_t #include <cstdint> // for int32_t
#include <fstream> // for std::ofstream #include <fstream> // for std::ofstream
#include <map> // for std::map #include <map> // for std::map
#include <memory> // for std::shared, std:unique_ptr #include <memory> // for std::shared, std:unique_ptr
#include <string> // for std::string #include <string> // for std::string
#include <typeinfo> // for typeid
#include <vector> // for std::vector #include <vector> // for std::vector
#if GNURADIO_USES_STD_POINTERS #if GNURADIO_USES_STD_POINTERS
#else #else
@@ -68,6 +70,8 @@ private:
explicit hybrid_observables_gs(const Obs_Conf& conf_); explicit hybrid_observables_gs(const Obs_Conf& conf_);
const size_t d_double_type_hash_code = typeid(double).hash_code();
void msg_handler_pvt_to_observables(const pmt::pmt_t& msg); void msg_handler_pvt_to_observables(const pmt::pmt_t& msg);
double compute_T_rx_s(const Gnss_Synchro& a) const; double compute_T_rx_s(const Gnss_Synchro& a) const;
bool interp_trk_obs(Gnss_Synchro& interpolated_obs, uint32_t ch, uint64_t rx_clock) const; bool interp_trk_obs(Gnss_Synchro& interpolated_obs, uint32_t ch, uint64_t rx_clock) const;

11
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc
View File

@@ -241,7 +241,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
if (d_nav.have_new_ephemeris() == true) if (d_nav.have_new_ephemeris() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Beidou_Dnav_Ephemeris> tmp_obj = std::make_shared<Beidou_Dnav_Ephemeris>(d_nav.get_ephemeris()); const std::shared_ptr<Beidou_Dnav_Ephemeris> tmp_obj = std::make_shared<Beidou_Dnav_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite; LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite;
std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n';
@@ -249,7 +249,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
if (d_nav.have_new_utc_model() == true) if (d_nav.have_new_utc_model() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Beidou_Dnav_Utc_Model> tmp_obj = std::make_shared<Beidou_Dnav_Utc_Model>(d_nav.get_utc_model()); const std::shared_ptr<Beidou_Dnav_Utc_Model> tmp_obj = std::make_shared<Beidou_Dnav_Utc_Model>(d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "BEIDOU DNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite; LOG(INFO) << "BEIDOU DNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite;
std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV utc model message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV utc model message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
@@ -257,7 +257,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
if (d_nav.have_new_iono() == true) if (d_nav.have_new_iono() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Beidou_Dnav_Iono> tmp_obj = std::make_shared<Beidou_Dnav_Iono>(d_nav.get_iono()); const std::shared_ptr<Beidou_Dnav_Iono> tmp_obj = std::make_shared<Beidou_Dnav_Iono>(d_nav.get_iono());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel << " from satellite " << d_satellite; LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel << " from satellite " << d_satellite;
std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_YELLOW << "New BEIDOU B1I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
@@ -362,6 +362,7 @@ void beidou_b1i_telemetry_decoder_gs::set_channel(int32_t channel)
} }
} }
void beidou_b1i_telemetry_decoder_gs::reset() void beidou_b1i_telemetry_decoder_gs::reset()
{ {
d_last_valid_preamble = d_sample_counter; d_last_valid_preamble = d_sample_counter;
@@ -371,6 +372,7 @@ void beidou_b1i_telemetry_decoder_gs::reset()
DLOG(INFO) << "Beidou B1I Telemetry decoder reset for satellite " << d_satellite; DLOG(INFO) << "Beidou B1I Telemetry decoder reset for satellite " << d_satellite;
} }
int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
@@ -427,7 +429,6 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite; DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I 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; d_stat = 2;
// ******* SAMPLES TO SYMBOLS ******* // ******* SAMPLES TO SYMBOLS *******
@@ -539,7 +540,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
// Reporting sow as gps time of week // Reporting sow as gps time of week
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.get_SOW() + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0); d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.get_SOW() + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0);
// check TOW update consistency // check TOW update consistency
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; const uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// compute new TOW // compute new TOW
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms;
flag_SOW_set = true; flag_SOW_set = true;

8
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc
View File

@@ -242,7 +242,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
if (d_nav.have_new_ephemeris() == true) if (d_nav.have_new_ephemeris() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Beidou_Dnav_Ephemeris> tmp_obj = const std::shared_ptr<Beidou_Dnav_Ephemeris> tmp_obj =
std::make_shared<Beidou_Dnav_Ephemeris>(d_nav.get_ephemeris()); std::make_shared<Beidou_Dnav_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel" LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel"
@@ -253,7 +253,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
if (d_nav.have_new_utc_model() == true) if (d_nav.have_new_utc_model() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Beidou_Dnav_Utc_Model> tmp_obj = const std::shared_ptr<Beidou_Dnav_Utc_Model> tmp_obj =
std::make_shared<Beidou_Dnav_Utc_Model>(d_nav.get_utc_model()); std::make_shared<Beidou_Dnav_Utc_Model>(d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "BEIDOU DNAV UTC Model have been received in channel" LOG(INFO) << "BEIDOU DNAV UTC Model have been received in channel"
@@ -265,7 +265,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
if (d_nav.have_new_iono() == true) if (d_nav.have_new_iono() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Beidou_Dnav_Iono> tmp_obj = const std::shared_ptr<Beidou_Dnav_Iono> tmp_obj =
std::make_shared<Beidou_Dnav_Iono>(d_nav.get_iono()); std::make_shared<Beidou_Dnav_Iono>(d_nav.get_iono());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel
@@ -570,7 +570,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
// Reporting sow as gps time of week // Reporting sow as gps time of week
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.get_SOW() + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0); d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.get_SOW() + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0);
// check TOW update consistency // check TOW update consistency
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; const uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// compute new TOW // compute new TOW
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms; d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms;
flag_SOW_set = true; flag_SOW_set = true;

14
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc
View File

@@ -114,7 +114,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
d_codelength = 0; d_codelength = 0;
d_datalength = 0; d_datalength = 0;
d_max_symbols_without_valid_frame = 0; d_max_symbols_without_valid_frame = 0;
std::cout << "Galileo unified telemetry decoder error: Unknown frame type \n"; std::cout << "Galileo unified telemetry decoder error: Unknown frame type\n";
} }
d_page_part_symbols.reserve(d_frame_length_symbols); d_page_part_symbols.reserve(d_frame_length_symbols);
@@ -166,8 +166,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
d_symbol_history.set_capacity(d_required_symbols + 1); d_symbol_history.set_capacity(d_required_symbols + 1);
// vars for Viterbi decoder // vars for Viterbi decoder
int32_t max_states = 1U << static_cast<uint32_t>(d_mm); // 2^d_mm const int32_t max_states = 1U << static_cast<uint32_t>(d_mm); // 2^d_mm
std::array<int32_t, 2> g_encoder{{121, 91}}; // Polynomial G1 and G2 std::array<int32_t, 2> g_encoder{{121, 91}}; // Polynomial G1 and G2
d_out0.reserve(max_states); d_out0.reserve(max_states);
d_out1.reserve(max_states); d_out1.reserve(max_states);
d_state0.reserve(max_states); d_state0.reserve(max_states);
@@ -274,21 +274,21 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
if (d_inav_nav.have_new_ephemeris() == true) if (d_inav_nav.have_new_ephemeris() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_inav_nav.get_ephemeris()); const std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_inav_nav.get_ephemeris());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << '\n'; std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
} }
if (d_inav_nav.have_new_iono_and_GST() == true) if (d_inav_nav.have_new_iono_and_GST() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(d_inav_nav.get_iono()); const std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(d_inav_nav.get_iono());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << '\n'; std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
} }
if (d_inav_nav.have_new_utc_model() == true) if (d_inav_nav.have_new_utc_model() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_inav_nav.get_utc_model()); const std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_inav_nav.get_utc_model());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << '\n'; std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
d_delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G_10 + 604800 * (std::fmod(static_cast<float>(d_inav_nav.get_Galileo_week() - tmp_obj->WN_0G_10), 64.0))); d_delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G_10 + 604800 * (std::fmod(static_cast<float>(d_inav_nav.get_Galileo_week() - tmp_obj->WN_0G_10), 64.0)));
@@ -296,7 +296,7 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
} }
if (d_inav_nav.have_new_almanac() == true) if (d_inav_nav.have_new_almanac() == true)
{ {
std::shared_ptr<Galileo_Almanac_Helper> tmp_obj = std::make_shared<Galileo_Almanac_Helper>(d_inav_nav.get_almanac()); const std::shared_ptr<Galileo_Almanac_Helper> tmp_obj = std::make_shared<Galileo_Almanac_Helper>(d_inav_nav.get_almanac());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
// debug // debug
std::cout << "Galileo E1 I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << '\n'; std::cout << "Galileo E1 I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << '\n';

10
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc
View File

@@ -179,7 +179,7 @@ void glonass_l1_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
d_nav.set_rf_link(d_satellite.get_rf_link()); d_nav.set_rf_link(d_satellite.get_rf_link());
std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(d_nav.get_ephemeris()); const std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite; LOG(INFO) << "GLONASS GNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite;
std::cout << "New GLONASS L1 GNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << '\n'; std::cout << "New GLONASS L1 GNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << '\n';
@@ -187,15 +187,15 @@ void glonass_l1_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
if (d_nav.have_new_utc_model() == true) if (d_nav.have_new_utc_model() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Glonass_Gnav_Utc_Model> tmp_obj = std::make_shared<Glonass_Gnav_Utc_Model>(d_nav.get_utc_model()); const std::shared_ptr<Glonass_Gnav_Utc_Model> tmp_obj = std::make_shared<Glonass_Gnav_Utc_Model>(d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite; LOG(INFO) << "GLONASS GNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite;
std::cout << "New GLONASS L1 GNAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << '\n'; std::cout << "New GLONASS L1 GNAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << '\n';
} }
if (d_nav.have_new_almanac() == true) if (d_nav.have_new_almanac() == true)
{ {
uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number(); const uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number();
std::shared_ptr<Glonass_Gnav_Almanac> const std::shared_ptr<Glonass_Gnav_Almanac>
tmp_obj = std::make_shared<Glonass_Gnav_Almanac>(d_nav.get_almanac(slot_nbr)); tmp_obj = std::make_shared<Glonass_Gnav_Almanac>(d_nav.get_almanac(slot_nbr));
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV Almanac have been received in channel" << d_channel << " in slot number " << slot_nbr; LOG(INFO) << "GLONASS GNAV Almanac have been received in channel" << d_channel << " in slot number " << slot_nbr;
@@ -326,7 +326,7 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
{ {
// NEW GLONASS string received // NEW GLONASS string received
// 0. fetch the symbols into an array // 0. fetch the symbols into an array
int32_t string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; const int32_t string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble;
std::array<double, GLONASS_GNAV_DATA_SYMBOLS> string_symbols{}; std::array<double, GLONASS_GNAV_DATA_SYMBOLS> string_symbols{};
// ******* SYMBOL TO BIT ******* // ******* SYMBOL TO BIT *******

10
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc
View File

@@ -179,7 +179,7 @@ void glonass_l2_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
d_nav.set_rf_link(d_satellite.get_rf_link()); d_nav.set_rf_link(d_satellite.get_rf_link());
std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(d_nav.get_ephemeris()); const std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite; LOG(INFO) << "GLONASS GNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite;
std::cout << TEXT_CYAN << "New GLONASS L2 GNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_CYAN << "New GLONASS L2 GNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n';
@@ -187,15 +187,15 @@ void glonass_l2_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
if (d_nav.have_new_utc_model() == true) if (d_nav.have_new_utc_model() == true)
{ {
// get object for this SV (mandatory) // get object for this SV (mandatory)
std::shared_ptr<Glonass_Gnav_Utc_Model> tmp_obj = std::make_shared<Glonass_Gnav_Utc_Model>(d_nav.get_utc_model()); const std::shared_ptr<Glonass_Gnav_Utc_Model> tmp_obj = std::make_shared<Glonass_Gnav_Utc_Model>(d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite; LOG(INFO) << "GLONASS GNAV UTC Model have been received in channel" << d_channel << " from satellite " << d_satellite;
std::cout << TEXT_CYAN << "New GLONASS L2 GNAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_CYAN << "New GLONASS L2 GNAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
} }
if (d_nav.have_new_almanac() == true) if (d_nav.have_new_almanac() == true)
{ {
uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number(); const uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number();
std::shared_ptr<Glonass_Gnav_Almanac> tmp_obj = std::make_shared<Glonass_Gnav_Almanac>(d_nav.get_almanac(slot_nbr)); const std::shared_ptr<Glonass_Gnav_Almanac> tmp_obj = std::make_shared<Glonass_Gnav_Almanac>(d_nav.get_almanac(slot_nbr));
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV Almanac have been received in channel" << d_channel << " in slot number " << slot_nbr; LOG(INFO) << "GLONASS GNAV Almanac have been received in channel" << d_channel << " in slot number " << slot_nbr;
std::cout << TEXT_CYAN << "New GLONASS L2 GNAV almanac received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_CYAN << "New GLONASS L2 GNAV almanac received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n';
@@ -325,7 +325,7 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
{ {
// NEW GLONASS string received // NEW GLONASS string received
// 0. fetch the symbols into an array // 0. fetch the symbols into an array
int32_t string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; const int32_t string_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble;
std::array<double, GLONASS_GNAV_DATA_SYMBOLS> string_symbols{}; std::array<double, GLONASS_GNAV_DATA_SYMBOLS> string_symbols{};
// ******* SYMBOL TO BIT ******* // ******* SYMBOL TO BIT *******

29
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc
View File

@@ -135,29 +135,20 @@ gps_l1_ca_telemetry_decoder_gs::~gps_l1_ca_telemetry_decoder_gs()
bool gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(uint32_t gpsword) bool gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(uint32_t gpsword)
{ {
uint32_t d1;
uint32_t d2;
uint32_t d3;
uint32_t d4;
uint32_t d5;
uint32_t d6;
uint32_t d7;
uint32_t t;
uint32_t parity;
// XOR as many bits in parallel as possible. The magic constants pick // XOR as many bits in parallel as possible. The magic constants pick
// up bits which are to be XOR'ed together to implement the GPS parity // up bits which are to be XOR'ed together to implement the GPS parity
// check algorithm described in IS-GPS-200K. This avoids lengthy shift- // check algorithm described in IS-GPS-200K. This avoids lengthy shift-
// and-xor loops. // and-xor loops.
d1 = gpsword & 0xFBFFBF00U; const uint32_t d1 = gpsword & 0xFBFFBF00U;
d2 = my_rotl::rotl(gpsword, 1U) & 0x07FFBF01U; const uint32_t d2 = my_rotl::rotl(gpsword, 1U) & 0x07FFBF01U;
d3 = my_rotl::rotl(gpsword, 2U) & 0xFC0F8100U; const uint32_t d3 = my_rotl::rotl(gpsword, 2U) & 0xFC0F8100U;
d4 = my_rotl::rotl(gpsword, 3U) & 0xF81FFE02U; const uint32_t d4 = my_rotl::rotl(gpsword, 3U) & 0xF81FFE02U;
d5 = my_rotl::rotl(gpsword, 4U) & 0xFC00000EU; const uint32_t d5 = my_rotl::rotl(gpsword, 4U) & 0xFC00000EU;
d6 = my_rotl::rotl(gpsword, 5U) & 0x07F00001U; const uint32_t d6 = my_rotl::rotl(gpsword, 5U) & 0x07F00001U;
d7 = my_rotl::rotl(gpsword, 6U) & 0x00003000U; const uint32_t d7 = my_rotl::rotl(gpsword, 6U) & 0x00003000U;
t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7; const uint32_t t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;
// Now XOR the 5 6-bit fields together to produce the 6-bit final result. // Now XOR the 5 6-bit fields together to produce the 6-bit final result.
parity = t ^ my_rotl::rotl(t, 6U) ^ my_rotl::rotl(t, 12U) ^ my_rotl::rotl(t, 18U) ^ my_rotl::rotl(t, 24U); uint32_t parity = t ^ my_rotl::rotl(t, 6U) ^ my_rotl::rotl(t, 12U) ^ my_rotl::rotl(t, 18U) ^ my_rotl::rotl(t, 24U);
parity = parity & 0x3FU; parity = parity & 0x3FU;
if (parity == (gpsword & 0x3FU)) if (parity == (gpsword & 0x3FU))
{ {
@@ -269,7 +260,7 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe()
// NEW GPS SUBFRAME HAS ARRIVED! // NEW GPS SUBFRAME HAS ARRIVED!
if (subframe_synchro_confirmation) if (subframe_synchro_confirmation)
{ {
int32_t subframe_ID = d_nav.subframe_decoder(subframe.data()); // decode the subframe const int32_t subframe_ID = d_nav.subframe_decoder(subframe.data()); // decode the subframe
if (subframe_ID > 0 and subframe_ID < 6) if (subframe_ID > 0 and subframe_ID < 6)
{ {
std::cout << "New GPS NAV message received in channel " << this->d_channel << ": " std::cout << "New GPS NAV message received in channel " << this->d_channel << ": "

8
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc
View File

@@ -150,7 +150,7 @@ int gps_l2c_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
uint32_t delay = 0; uint32_t delay = 0;
// add the symbol to the decoder // add the symbol to the decoder
uint8_t symbol_clip = static_cast<uint8_t>(in[0].Prompt_I > 0) * 255; const uint8_t symbol_clip = static_cast<uint8_t>(in[0].Prompt_I > 0) * 255;
flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay); flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay);
consume_each(1); // one by one consume_each(1); // one by one
@@ -198,20 +198,20 @@ int gps_l2c_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
if (d_CNAV_Message.have_new_ephemeris() == true) if (d_CNAV_Message.have_new_ephemeris() == true)
{ {
// get ephemeris object for this SV // get ephemeris object for this SV
std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(d_CNAV_Message.get_ephemeris()); const std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(d_CNAV_Message.get_ephemeris());
std::cout << TEXT_BLUE << "New GPS CNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_BLUE << "New GPS CNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
} }
if (d_CNAV_Message.have_new_iono() == true) if (d_CNAV_Message.have_new_iono() == true)
{ {
std::shared_ptr<Gps_CNAV_Iono> tmp_obj = std::make_shared<Gps_CNAV_Iono>(d_CNAV_Message.get_iono()); const std::shared_ptr<Gps_CNAV_Iono> tmp_obj = std::make_shared<Gps_CNAV_Iono>(d_CNAV_Message.get_iono());
std::cout << TEXT_BLUE << "New GPS CNAV message received in channel " << d_channel << ": iono model parameters from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_BLUE << "New GPS CNAV message received in channel " << d_channel << ": iono model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
} }
if (d_CNAV_Message.have_new_utc_model() == true) if (d_CNAV_Message.have_new_utc_model() == true)
{ {
std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(d_CNAV_Message.get_utc_model()); const std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(d_CNAV_Message.get_utc_model());
std::cout << TEXT_BLUE << "New GPS CNAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_BLUE << "New GPS CNAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
} }

8
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc
View File

@@ -190,20 +190,20 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u
if (d_CNAV_Message.have_new_ephemeris() == true) if (d_CNAV_Message.have_new_ephemeris() == true)
{ {
// get ephemeris object for this SV // get ephemeris object for this SV
std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(d_CNAV_Message.get_ephemeris()); const std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(d_CNAV_Message.get_ephemeris());
std::cout << TEXT_MAGENTA << "New GPS L5 CNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_MAGENTA << "New GPS L5 CNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
} }
if (d_CNAV_Message.have_new_iono() == true) if (d_CNAV_Message.have_new_iono() == true)
{ {
std::shared_ptr<Gps_CNAV_Iono> tmp_obj = std::make_shared<Gps_CNAV_Iono>(d_CNAV_Message.get_iono()); const std::shared_ptr<Gps_CNAV_Iono> tmp_obj = std::make_shared<Gps_CNAV_Iono>(d_CNAV_Message.get_iono());
std::cout << TEXT_MAGENTA << "New GPS L5 CNAV message received in channel " << d_channel << ": iono model parameters from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_MAGENTA << "New GPS L5 CNAV message received in channel " << d_channel << ": iono model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
} }
if (d_CNAV_Message.have_new_utc_model() == true) if (d_CNAV_Message.have_new_utc_model() == true)
{ {
std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(d_CNAV_Message.get_utc_model()); const std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(d_CNAV_Message.get_utc_model());
std::cout << TEXT_MAGENTA << "New GPS L5 CNAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n'; std::cout << TEXT_MAGENTA << "New GPS L5 CNAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << '\n';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
} }
@@ -217,7 +217,7 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u
// symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory // symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory
// check TOW update consistency // check TOW update consistency
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; const uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5I_SYMBOL_PERIOD_MS; d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5I_SYMBOL_PERIOD_MS;
if (last_d_TOW_at_current_symbol_ms != 0 and std::llabs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - static_cast<int64_t>(last_d_TOW_at_current_symbol_ms)) > static_cast<int64_t>(GPS_L5I_SYMBOL_PERIOD_MS)) if (last_d_TOW_at_current_symbol_ms != 0 and std::llabs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - static_cast<int64_t>(last_d_TOW_at_current_symbol_ms)) > static_cast<int64_t>(GPS_L5I_SYMBOL_PERIOD_MS))
{ {

28
src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_gs.cc
View File

@@ -199,7 +199,7 @@ void sbas_l1_telemetry_decoder_gs::Symbol_Aligner_And_Decoder::reset()
bool sbas_l1_telemetry_decoder_gs::Symbol_Aligner_And_Decoder::get_bits(const std::vector<double> &symbols, std::vector<int32_t> &bits) bool sbas_l1_telemetry_decoder_gs::Symbol_Aligner_And_Decoder::get_bits(const std::vector<double> &symbols, std::vector<int32_t> &bits)
{ {
const int32_t traceback_depth = 5 * d_KK; const int32_t traceback_depth = 5 * d_KK;
int32_t nbits_requested = symbols.size() / D_SYMBOLS_PER_BIT; const int32_t nbits_requested = symbols.size() / D_SYMBOLS_PER_BIT;
int32_t nbits_decoded; int32_t nbits_decoded;
// fill two vectors with the two possible symbol alignments // fill two vectors with the two possible symbol alignments
std::vector<double> symbols_vd1(symbols); // aligned symbol vector -> copy input symbol vector std::vector<double> symbols_vd1(symbols); // aligned symbol vector -> copy input symbol vector
@@ -213,8 +213,8 @@ bool sbas_l1_telemetry_decoder_gs::Symbol_Aligner_And_Decoder::get_bits(const st
std::vector<int32_t> bits_vd1(nbits_requested); std::vector<int32_t> bits_vd1(nbits_requested);
std::vector<int32_t> bits_vd2(nbits_requested); std::vector<int32_t> bits_vd2(nbits_requested);
// decode // decode
float metric_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1.data(), nbits_requested, nbits_decoded); const float metric_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1.data(), nbits_requested, nbits_decoded);
float metric_vd2 = d_vd2->decode_continuous(symbols_vd2.data(), traceback_depth, bits_vd2.data(), nbits_requested, nbits_decoded); const float metric_vd2 = d_vd2->decode_continuous(symbols_vd2.data(), traceback_depth, bits_vd2.data(), nbits_requested, nbits_decoded);
// choose the bits with the better metric // choose the bits with the better metric
for (int32_t i = 0; i < nbits_decoded; i++) for (int32_t i = 0; i < nbits_decoded; i++)
{ {
@@ -242,8 +242,8 @@ void sbas_l1_telemetry_decoder_gs::Frame_Detector::reset()
void sbas_l1_telemetry_decoder_gs::Frame_Detector::get_frame_candidates(const std::vector<int32_t> &bits, std::vector<std::pair<int32_t, std::vector<int32_t>>> &msg_candidates) void sbas_l1_telemetry_decoder_gs::Frame_Detector::get_frame_candidates(const std::vector<int32_t> &bits, std::vector<std::pair<int32_t, std::vector<int32_t>>> &msg_candidates)
{ {
std::stringstream ss; std::stringstream ss;
uint32_t sbas_msg_length = 250; const uint32_t sbas_msg_length = 250;
std::vector<std::vector<int32_t>> preambles = {{0, 1, 0, 1, 0, 0, 1, 1}, const std::vector<std::vector<int32_t>> preambles = {{0, 1, 0, 1, 0, 0, 1, 1},
{1, 0, 0, 1, 1, 0, 1, 0}, {1, 0, 0, 1, 1, 0, 1, 0},
{1, 1, 0, 0, 0, 1, 1, 0}}; {1, 1, 0, 0, 0, 1, 1, 0}};
VLOG(FLOW) << "get_frame_candidates(): " VLOG(FLOW) << "get_frame_candidates(): "
@@ -322,7 +322,7 @@ void sbas_l1_telemetry_decoder_gs::Crc_Verifier::get_valid_frames(const std::vec
// verify CRC // verify CRC
d_checksum_agent.reset(0); d_checksum_agent.reset(0);
d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size()); d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size());
uint32_t crc = d_checksum_agent.checksum(); const uint32_t crc = d_checksum_agent.checksum();
VLOG(SAMP_SYNC) << "candidate " << candidate_it - msg_candidates.begin() VLOG(SAMP_SYNC) << "candidate " << candidate_it - msg_candidates.begin()
<< ": final crc remainder= " << std::hex << crc << ": final crc remainder= " << std::hex << crc
<< std::setfill(' ') << std::resetiosflags(std::ios::hex); << std::setfill(' ') << std::resetiosflags(std::ios::hex);
@@ -354,8 +354,8 @@ void sbas_l1_telemetry_decoder_gs::Crc_Verifier::zerropad_back_and_convert_to_by
VLOG(LMORE) << "zerropad_back_and_convert_to_bytes():" << byte; VLOG(LMORE) << "zerropad_back_and_convert_to_bytes():" << byte;
for (auto candidate_bit_it = msg_candidate.cbegin(); candidate_bit_it < msg_candidate.cend(); ++candidate_bit_it) for (auto candidate_bit_it = msg_candidate.cbegin(); candidate_bit_it < msg_candidate.cend(); ++candidate_bit_it)
{ {
int32_t idx_bit = candidate_bit_it - msg_candidate.begin(); const int32_t idx_bit = candidate_bit_it - msg_candidate.begin();
int32_t bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte); const int32_t bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
byte |= static_cast<uint8_t>(*candidate_bit_it) << bit_pos_in_current_byte; byte |= static_cast<uint8_t>(*candidate_bit_it) << bit_pos_in_current_byte;
ss << *candidate_bit_it; ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte - 1) if (idx_bit % bits_per_byte == bits_per_byte - 1)
@@ -382,7 +382,7 @@ void sbas_l1_telemetry_decoder_gs::Crc_Verifier::zerropad_front_and_convert_to_b
VLOG(LMORE) << "zerropad_front_and_convert_to_bytes():" << byte; VLOG(LMORE) << "zerropad_front_and_convert_to_bytes():" << byte;
for (auto candidate_bit_it = msg_candidate.cbegin(); candidate_bit_it < msg_candidate.cend(); ++candidate_bit_it) for (auto candidate_bit_it = msg_candidate.cbegin(); candidate_bit_it < msg_candidate.cend(); ++candidate_bit_it)
{ {
int32_t bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte); const int32_t bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
byte |= static_cast<uint8_t>(*candidate_bit_it) << bit_pos_in_current_byte; byte |= static_cast<uint8_t>(*candidate_bit_it) << bit_pos_in_current_byte;
ss << *candidate_bit_it; ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte - 1) if (idx_bit % bits_per_byte == bits_per_byte - 1)
@@ -417,7 +417,7 @@ int sbas_l1_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
d_sample_buf.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue d_sample_buf.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue
// store the time stamp of the first sample in the processed sample block // store the time stamp of the first sample in the processed sample block
double sample_stamp = static_cast<double>(in[0].Tracking_sample_counter) / static_cast<double>(in[0].fs); const double sample_stamp = static_cast<double>(in[0].Tracking_sample_counter) / static_cast<double>(in[0].fs);
// decode only if enough samples in buffer // decode only if enough samples in buffer
if (d_sample_buf.size() >= d_block_size) if (d_sample_buf.size() >= d_block_size)
@@ -425,12 +425,12 @@ int sbas_l1_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
// align correlation samples in pairs // align correlation samples in pairs
// and obtain the symbols by summing the paired correlation samples // and obtain the symbols by summing the paired correlation samples
std::vector<double> symbols; std::vector<double> symbols;
bool sample_alignment = d_sample_aligner.get_symbols(d_sample_buf, symbols); const bool sample_alignment = d_sample_aligner.get_symbols(d_sample_buf, symbols);
// align symbols in pairs // align symbols in pairs
// and obtain the bits by decoding the symbol pairs // and obtain the bits by decoding the symbol pairs
std::vector<int32_t> bits; std::vector<int32_t> bits;
bool symbol_alignment = d_symbol_aligner_and_decoder.get_bits(symbols, bits); const bool symbol_alignment = d_symbol_aligner_and_decoder.get_bits(symbols, bits);
// search for preambles // search for preambles
// and extract the corresponding message candidates // and extract the corresponding message candidates
@@ -447,9 +447,9 @@ int sbas_l1_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
// std::vector<Sbas_Raw_Msg> sbas_raw_msgs; // std::vector<Sbas_Raw_Msg> sbas_raw_msgs;
for (const auto &valid_msg : valid_msgs) for (const auto &valid_msg : valid_msgs)
{ {
int32_t message_sample_offset = const int32_t message_sample_offset =
(sample_alignment ? 0 : -1) + D_SAMPLES_PER_SYMBOL * (symbol_alignment ? -1 : 0) + D_SAMPLES_PER_SYMBOL * D_SYMBOLS_PER_BIT * valid_msg.first; (sample_alignment ? 0 : -1) + D_SAMPLES_PER_SYMBOL * (symbol_alignment ? -1 : 0) + D_SAMPLES_PER_SYMBOL * D_SYMBOLS_PER_BIT * valid_msg.first;
double message_sample_stamp = sample_stamp + static_cast<double>(message_sample_offset) / 1000.0; const double message_sample_stamp = sample_stamp + static_cast<double>(message_sample_offset) / 1000.0;
VLOG(EVENT) << "message_sample_stamp=" << message_sample_stamp VLOG(EVENT) << "message_sample_stamp=" << message_sample_stamp
<< " (sample_stamp=" << sample_stamp << " (sample_stamp=" << sample_stamp
<< " sample_alignment=" << sample_alignment << " sample_alignment=" << sample_alignment

34
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@@ -560,11 +560,9 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg)
{ {
try try
{ {
if (pmt::any_ref(msg).type() == typeid(int)) if (pmt::any_ref(msg).type().hash_code() == int_type_hash_code)
{ {
int tlm_event; const int tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
if (tlm_event == 1) if (tlm_event == 1)
{ {
DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel;
@@ -624,7 +622,7 @@ void dll_pll_veml_tracking::start_tracking()
{ {
if (d_trk_parameters.track_pilot) if (d_trk_parameters.track_pilot)
{ {
std::array<char, 3> pilot_signal = {{'1', 'C', '\0'}}; const std::array<char, 3> pilot_signal = {{'1', 'C', '\0'}};
galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN);
galileo_e1_code_gen_sinboc11_float(d_data_code, Signal_, d_acquisition_gnss_synchro->PRN); galileo_e1_code_gen_sinboc11_float(d_data_code, Signal_, d_acquisition_gnss_synchro->PRN);
d_Prompt_Data[0] = gr_complex(0.0, 0.0); d_Prompt_Data[0] = gr_complex(0.0, 0.0);
@@ -638,7 +636,7 @@ void dll_pll_veml_tracking::start_tracking()
else if (d_systemName == "Galileo" and d_signal_type == "5X") else if (d_systemName == "Galileo" and d_signal_type == "5X")
{ {
volk_gnsssdr::vector<gr_complex> aux_code(d_code_length_chips); volk_gnsssdr::vector<gr_complex> aux_code(d_code_length_chips);
std::array<char, 3> signal_type_ = {{'5', 'X', '\0'}}; const std::array<char, 3> signal_type_ = {{'5', 'X', '\0'}};
galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, signal_type_); galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, signal_type_);
if (d_trk_parameters.track_pilot) if (d_trk_parameters.track_pilot)
{ {
@@ -867,7 +865,7 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status(double coh_integration_
d_Prompt_buffer[d_cn0_estimation_counter % d_trk_parameters.cn0_samples] = d_P_accu; d_Prompt_buffer[d_cn0_estimation_counter % d_trk_parameters.cn0_samples] = d_P_accu;
d_cn0_estimation_counter++; d_cn0_estimation_counter++;
// Code lock indicator // Code lock indicator
float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator(d_Prompt_buffer.data(), d_trk_parameters.cn0_samples, static_cast<float>(coh_integration_time_s)); const float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator(d_Prompt_buffer.data(), d_trk_parameters.cn0_samples, static_cast<float>(coh_integration_time_s));
d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw);
// Carrier lock indicator // Carrier lock indicator
d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1));
@@ -1360,10 +1358,10 @@ int32_t dll_pll_veml_tracking::save_matfile() const
{ {
// READ DUMP FILE // READ DUMP FILE
std::ifstream::pos_type size; std::ifstream::pos_type size;
int32_t number_of_double_vars = 1; const int32_t number_of_double_vars = 1;
int32_t number_of_float_vars = 19; const int32_t number_of_float_vars = 19;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t); sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file; std::ifstream dump_file;
std::string dump_filename_ = d_dump_filename; std::string dump_filename_ = d_dump_filename;
// add channel number to the filename // add channel number to the filename
@@ -1631,21 +1629,21 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
case 1: // Pull-in case 1: // Pull-in
{ {
// Signal alignment (skip samples until the incoming signal is aligned with local replica) // Signal alignment (skip samples until the incoming signal is aligned with local replica)
int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); const int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp);
double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / d_trk_parameters.fs_in; const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / d_trk_parameters.fs_in;
double delta_trk_to_acq_prn_start_samples = static_cast<double>(acq_trk_diff_samples) - d_acq_code_phase_samples; const double delta_trk_to_acq_prn_start_samples = static_cast<double>(acq_trk_diff_samples) - d_acq_code_phase_samples;
d_code_freq_chips = d_code_chip_rate; d_code_freq_chips = d_code_chip_rate;
d_code_phase_step_chips = d_code_freq_chips / d_trk_parameters.fs_in; d_code_phase_step_chips = d_code_freq_chips / d_trk_parameters.fs_in;
d_code_phase_rate_step_chips = 0.0; d_code_phase_rate_step_chips = 0.0;
double T_chip_mod_seconds = 1.0 / d_code_freq_chips; const double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
double T_prn_mod_seconds = T_chip_mod_seconds * static_cast<double>(d_code_length_chips); const double T_prn_mod_seconds = T_chip_mod_seconds * static_cast<double>(d_code_length_chips);
double T_prn_mod_samples = T_prn_mod_seconds * d_trk_parameters.fs_in; const double T_prn_mod_samples = T_prn_mod_seconds * d_trk_parameters.fs_in;
d_acq_code_phase_samples = T_prn_mod_samples - std::fmod(delta_trk_to_acq_prn_start_samples, T_prn_mod_samples); d_acq_code_phase_samples = T_prn_mod_samples - std::fmod(delta_trk_to_acq_prn_start_samples, T_prn_mod_samples);
d_current_prn_length_samples = round(T_prn_mod_samples); d_current_prn_length_samples = round(T_prn_mod_samples);
int32_t samples_offset = round(d_acq_code_phase_samples); const int32_t samples_offset = round(d_acq_code_phase_samples);
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(samples_offset); d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(samples_offset);
d_state = 2; d_state = 2;
d_sample_counter += samples_offset; // count for the processed samples d_sample_counter += samples_offset; // count for the processed samples

10
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h
View File

@@ -32,10 +32,12 @@
#include <gnuradio/types.h> // for gr_vector_int, gr_vector... #include <gnuradio/types.h> // for gr_vector_int, gr_vector...
#include <pmt/pmt.h> // for pmt_t #include <pmt/pmt.h> // for pmt_t
#include <volk_gnsssdr/volk_gnsssdr_alloc.h> // for volk_gnsssdr::vector #include <volk_gnsssdr/volk_gnsssdr_alloc.h> // for volk_gnsssdr::vector
#include <cstddef> // for size_t
#include <cstdint> // for int32_t #include <cstdint> // for int32_t
#include <fstream> // for string, ofstream #include <fstream> // for ofstream
#include <string> #include <string> // for string
#include <utility> // for pair #include <typeinfo> // for typeid
#include <utility> // for pair
#if GNURADIO_USES_STD_POINTERS #if GNURADIO_USES_STD_POINTERS
#include <memory> #include <memory>
#else #else
@@ -113,6 +115,8 @@ private:
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history; boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer; boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
const size_t int_type_hash_code = typeid(int).hash_code();
double d_signal_carrier_freq; double d_signal_carrier_freq;
double d_code_period; double d_code_period;
double d_code_chip_rate; double d_code_chip_rate;

44
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc
View File

@@ -466,9 +466,9 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &
} }
} }
// create multicorrelator class // create multicorrelator class
std::string device_name = d_trk_parameters.device_name; const std::string device_name = d_trk_parameters.device_name;
uint32_t dev_file_num = d_trk_parameters.dev_file_num; const uint32_t dev_file_num = d_trk_parameters.dev_file_num;
uint32_t num_prev_assigned_ch = d_trk_parameters.num_prev_assigned_ch; const uint32_t num_prev_assigned_ch = d_trk_parameters.num_prev_assigned_ch;
int32_t *ca_codes = d_trk_parameters.ca_codes; int32_t *ca_codes = d_trk_parameters.ca_codes;
int32_t *data_codes = d_trk_parameters.data_codes; int32_t *data_codes = d_trk_parameters.data_codes;
d_multicorrelator_fpga = std::make_shared<Fpga_Multicorrelator_8sc>(d_n_correlator_taps, device_name, dev_file_num, num_prev_assigned_ch, ca_codes, data_codes, d_code_length_chips, d_trk_parameters.track_pilot, d_code_samples_per_chip); d_multicorrelator_fpga = std::make_shared<Fpga_Multicorrelator_8sc>(d_n_correlator_taps, device_name, dev_file_num, num_prev_assigned_ch, ca_codes, data_codes, d_code_length_chips, d_trk_parameters.track_pilot, d_code_samples_per_chip);
@@ -489,11 +489,9 @@ void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t &
{ {
try try
{ {
if (pmt::any_ref(msg).type() == typeid(int)) if (pmt::any_ref(msg).type().hash_code() == int_type_hash_code)
{ {
int tlm_event; const int tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
if (tlm_event == 1) if (tlm_event == 1)
{ {
DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel;
@@ -623,7 +621,7 @@ bool dll_pll_veml_tracking_fpga::cn0_and_tracking_lock_status(double coh_integra
d_Prompt_buffer[d_cn0_estimation_counter % d_trk_parameters.cn0_samples] = d_P_accu; d_Prompt_buffer[d_cn0_estimation_counter % d_trk_parameters.cn0_samples] = d_P_accu;
d_cn0_estimation_counter++; d_cn0_estimation_counter++;
// Code lock indicator // Code lock indicator
float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator(d_Prompt_buffer.data(), d_trk_parameters.cn0_samples, static_cast<float>(coh_integration_time_s)); const float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator(d_Prompt_buffer.data(), d_trk_parameters.cn0_samples, static_cast<float>(coh_integration_time_s));
d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw);
// Carrier lock indicator // Carrier lock indicator
d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1));
@@ -765,10 +763,10 @@ void dll_pll_veml_tracking_fpga::run_dll_pll()
if (d_dll_filt_history.full()) if (d_dll_filt_history.full())
{ {
float avg_code_error_chips_s = static_cast<float>(std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0)) / static_cast<float>(d_dll_filt_history.capacity()); const float avg_code_error_chips_s = static_cast<float>(std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0)) / static_cast<float>(d_dll_filt_history.capacity());
if (fabs(avg_code_error_chips_s) > 1.0) if (fabs(avg_code_error_chips_s) > 1.0)
{ {
float carrier_doppler_error_hz = static_cast<float>(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast<float>(d_code_chip_rate); const float carrier_doppler_error_hz = static_cast<float>(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast<float>(d_code_chip_rate);
LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN); LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN);
d_carrier_loop_filter.initialize(static_cast<float>(d_carrier_doppler_hz) - carrier_doppler_error_hz); d_carrier_loop_filter.initialize(static_cast<float>(d_carrier_doppler_hz) - carrier_doppler_error_hz);
d_corrected_doppler = true; d_corrected_doppler = true;
@@ -824,7 +822,7 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars()
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
d_T_prn_samples = d_T_prn_seconds * d_trk_parameters.fs_in; d_T_prn_samples = d_T_prn_seconds * d_trk_parameters.fs_in;
d_K_blk_samples = d_T_prn_samples * d_current_fpga_integration_period + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function d_K_blk_samples = d_T_prn_samples * d_current_fpga_integration_period + d_rem_code_phase_samples; // initially d_rem_code_phase_samples is zero. It is updated at the end of this function
auto actual_blk_length = static_cast<int32_t>(std::floor(d_K_blk_samples)); const auto actual_blk_length = static_cast<int32_t>(std::floor(d_K_blk_samples));
d_next_integration_length_samples = actual_blk_length; d_next_integration_length_samples = actual_blk_length;
// ################## PLL COMMANDS ################################################# // ################## PLL COMMANDS #################################################
@@ -1119,10 +1117,10 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() const
{ {
// READ DUMP FILE // READ DUMP FILE
std::ifstream::pos_type size; std::ifstream::pos_type size;
int32_t number_of_double_vars = 1; const int32_t number_of_double_vars = 1;
int32_t number_of_float_vars = 19; const int32_t number_of_float_vars = 19;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t); sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file; std::ifstream dump_file;
std::string dump_filename_ = d_dump_filename; std::string dump_filename_ = d_dump_filename;
// add channel number to the filename // add channel number to the filename
@@ -1546,7 +1544,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
uint64_t absolute_samples_offset; uint64_t absolute_samples_offset;
d_multicorrelator_fpga->lock_channel(); d_multicorrelator_fpga->lock_channel();
uint64_t counter_value = d_multicorrelator_fpga->read_sample_counter(); const uint64_t counter_value = d_multicorrelator_fpga->read_sample_counter();
if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples)) if (counter_value > (d_acq_sample_stamp + d_acq_code_phase_samples))
{ {
// Signal alignment (skip samples until the incoming signal is aligned with local replica) // Signal alignment (skip samples until the incoming signal is aligned with local replica)
@@ -1554,7 +1552,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / d_trk_parameters.fs_in; acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / d_trk_parameters.fs_in;
delta_trk_to_acq_prn_start_samples = static_cast<double>(acq_trk_diff_samples) - d_acq_code_phase_samples; delta_trk_to_acq_prn_start_samples = static_cast<double>(acq_trk_diff_samples) - d_acq_code_phase_samples;
uint32_t num_frames = ceil((delta_trk_to_acq_prn_start_samples) / d_current_integration_length_samples); const uint32_t num_frames = ceil((delta_trk_to_acq_prn_start_samples) / d_current_integration_length_samples);
absolute_samples_offset = static_cast<uint64_t>(d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_current_integration_length_samples); absolute_samples_offset = static_cast<uint64_t>(d_acq_code_phase_samples + d_acq_sample_stamp + num_frames * d_current_integration_length_samples);
} }
else else
@@ -1572,14 +1570,14 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
d_sample_counter_next = d_sample_counter; d_sample_counter_next = d_sample_counter;
// Doppler effect Fd = (C / (C + Vr)) * F // Doppler effect Fd = (C / (C + Vr)) * F
double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq; const double radial_velocity = (d_signal_carrier_freq + d_acq_carrier_doppler_hz) / d_signal_carrier_freq;
// new chip and PRN sequence periods based on acq Doppler // new chip and PRN sequence periods based on acq Doppler
d_code_freq_chips = radial_velocity * d_code_chip_rate; d_code_freq_chips = radial_velocity * d_code_chip_rate;
d_code_phase_step_chips = d_code_freq_chips / d_trk_parameters.fs_in; d_code_phase_step_chips = d_code_freq_chips / d_trk_parameters.fs_in;
d_acq_code_phase_samples = absolute_samples_offset; d_acq_code_phase_samples = absolute_samples_offset;
int32_t samples_offset = round(d_acq_code_phase_samples); const int32_t samples_offset = round(d_acq_code_phase_samples);
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(samples_offset); d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(samples_offset);
d_state = 2; d_state = 2;
@@ -1893,8 +1891,8 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
// this must be computed for the secondary prn code // this must be computed for the secondary prn code
if (d_secondary) if (d_secondary)
{ {
uint32_t next_prn_length = d_current_integration_length_samples / d_fpga_integration_period; const uint32_t next_prn_length = d_current_integration_length_samples / d_fpga_integration_period;
uint32_t first_prn_length = d_current_integration_length_samples - next_prn_length * (d_fpga_integration_period - 1); const uint32_t first_prn_length = d_current_integration_length_samples - next_prn_length * (d_fpga_integration_period - 1);
d_multicorrelator_fpga->update_prn_code_length(first_prn_length, next_prn_length); d_multicorrelator_fpga->update_prn_code_length(first_prn_length, next_prn_length);
} }
@@ -1946,8 +1944,8 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
// this must be computed for the secondary prn code // this must be computed for the secondary prn code
if (d_secondary) if (d_secondary)
{ {
uint32_t next_prn_length = d_current_integration_length_samples / d_fpga_integration_period; const uint32_t next_prn_length = d_current_integration_length_samples / d_fpga_integration_period;
uint32_t first_prn_length = d_current_integration_length_samples - next_prn_length * (d_fpga_integration_period - 1); const uint32_t first_prn_length = d_current_integration_length_samples - next_prn_length * (d_fpga_integration_period - 1);
d_multicorrelator_fpga->update_prn_code_length(first_prn_length, next_prn_length); d_multicorrelator_fpga->update_prn_code_length(first_prn_length, next_prn_length);
} }

8
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h
View File

@@ -31,11 +31,13 @@
#include <gnuradio/types.h> // for gr_vector_int, gr_vector... #include <gnuradio/types.h> // for gr_vector_int, gr_vector...
#include <pmt/pmt.h> // for pmt_t #include <pmt/pmt.h> // for pmt_t
#include <volk_gnsssdr/volk_gnsssdr_alloc.h> // for volk_gnsssdr::vector #include <volk_gnsssdr/volk_gnsssdr_alloc.h> // for volk_gnsssdr::vector
#include <cstddef> // for size_t
#include <cstdint> // for int32_t #include <cstdint> // for int32_t
#include <fstream> // for string, ofstream #include <fstream> // for string, ofstream
#include <memory> // for std::shared_ptr #include <memory> // for std::shared_ptr
#include <string> #include <string> // for string
#include <utility> // for pair #include <typeinfo> // for typeid
#include <utility> // for pair
#if GNURADIO_USES_STD_POINTERS #if GNURADIO_USES_STD_POINTERS
#else #else
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
@@ -153,6 +155,8 @@ private:
boost::mutex d_mutex; boost::mutex d_mutex;
const size_t int_type_hash_code = typeid(int).hash_code();
double d_signal_carrier_freq; double d_signal_carrier_freq;
double d_code_period; double d_code_period;
double d_code_chip_rate; double d_code_chip_rate;

17
src/core/libs/channel_status_msg_receiver.cc
View File

@@ -23,7 +23,9 @@
#include <glog/logging.h> #include <glog/logging.h>
#include <gnuradio/gr_complex.h> #include <gnuradio/gr_complex.h>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <cstddef>
#include <cstdint> #include <cstdint>
#include <typeinfo>
#include <utility> #include <utility>
#if HAS_GENERIC_LAMBDA #if HAS_GENERIC_LAMBDA
@@ -60,11 +62,11 @@ void channel_status_msg_receiver::msg_handler_events(const pmt::pmt_t& msg)
gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_events function called by the scheduler gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_events function called by the scheduler
try try
{ {
// ************* Gnss_Synchro received ***************** const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gnss_Synchro>)) // ****************** Gnss_Synchro received ************************
if (msg_type_hash_code == typeid(std::shared_ptr<Gnss_Synchro>).hash_code())
{ {
std::shared_ptr<Gnss_Synchro> gnss_synchro_obj; const std::shared_ptr<Gnss_Synchro> gnss_synchro_obj = boost::any_cast<std::shared_ptr<Gnss_Synchro>>(pmt::any_ref(msg));
gnss_synchro_obj = boost::any_cast<std::shared_ptr<Gnss_Synchro>>(pmt::any_ref(msg));
if (gnss_synchro_obj->Flag_valid_pseudorange == true) if (gnss_synchro_obj->Flag_valid_pseudorange == true)
{ {
d_channel_status_map[gnss_synchro_obj->Channel_ID] = gnss_synchro_obj; d_channel_status_map[gnss_synchro_obj->Channel_ID] = gnss_synchro_obj;
@@ -81,11 +83,10 @@ void channel_status_msg_receiver::msg_handler_events(const pmt::pmt_t& msg)
// } // }
// std::cout << "-------- \n" << '\n'; // std::cout << "-------- \n" << '\n';
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Monitor_Pvt>)) else if (msg_type_hash_code == typeid(std::shared_ptr<Monitor_Pvt>).hash_code())
{ {
// ************* Monitor_Pvt received ***************** // ***************** Monitor_Pvt received ******************
std::shared_ptr<Monitor_Pvt> monitor_pvt_obj; const std::shared_ptr<Monitor_Pvt> monitor_pvt_obj = boost::any_cast<std::shared_ptr<Monitor_Pvt>>(pmt::any_ref(msg));
monitor_pvt_obj = boost::any_cast<std::shared_ptr<Monitor_Pvt>>(pmt::any_ref(msg));
d_pvt_status = *monitor_pvt_obj.get(); d_pvt_status = *monitor_pvt_obj.get();
// std::cout << "-------- \n" << '\n'; // std::cout << "-------- \n" << '\n';

112
src/core/receiver/control_thread.cc
View File

@@ -26,8 +26,6 @@
#endif #endif
#include "control_thread.h" #include "control_thread.h"
#include "channel_event.h"
#include "command_event.h"
#include "concurrent_map.h" #include "concurrent_map.h"
#include "configuration_interface.h" #include "configuration_interface.h"
#include "file_configuration.h" #include "file_configuration.h"
@@ -128,9 +126,9 @@ void ControlThread::init()
agnss_ref_location_ = Agnss_Ref_Location(); agnss_ref_location_ = Agnss_Ref_Location();
agnss_ref_time_ = Agnss_Ref_Time(); agnss_ref_time_ = Agnss_Ref_Time();
std::string empty_string = ""; const std::string empty_string;
std::string ref_location_str = configuration_->property("GNSS-SDR.AGNSS_ref_location", empty_string); const std::string ref_location_str = configuration_->property("GNSS-SDR.AGNSS_ref_location", empty_string);
std::string ref_time_str = configuration_->property("GNSS-SDR.AGNSS_ref_utc_time", empty_string); const std::string ref_time_str = configuration_->property("GNSS-SDR.AGNSS_ref_utc_time", empty_string);
if (ref_location_str != empty_string) if (ref_location_str != empty_string)
{ {
std::vector<double> vect; std::vector<double> vect;
@@ -171,7 +169,9 @@ void ControlThread::init()
else else
{ {
// fill agnss_ref_time_ // fill agnss_ref_time_
struct tm tm = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, nullptr}; struct tm tm
{
};
if (strptime(ref_time_str.c_str(), "%d/%m/%Y %H:%M:%S", &tm) != nullptr) if (strptime(ref_time_str.c_str(), "%d/%m/%Y %H:%M:%S", &tm) != nullptr)
{ {
agnss_ref_time_.d_tv_sec = timegm(&tm); agnss_ref_time_.d_tv_sec = timegm(&tm);
@@ -219,7 +219,7 @@ void ControlThread::telecommand_listener()
{ {
if (telecommand_enabled_) if (telecommand_enabled_)
{ {
int tcp_cmd_port = configuration_->property("GNSS-SDR.telecommand_tcp_port", 3333); const int tcp_cmd_port = configuration_->property("GNSS-SDR.telecommand_tcp_port", 3333);
cmd_interface_.run_cmd_server(tcp_cmd_port); cmd_interface_.run_cmd_server(tcp_cmd_port);
} }
} }
@@ -230,21 +230,20 @@ void ControlThread::event_dispatcher(bool &valid_event, pmt::pmt_t &msg)
if (valid_event) if (valid_event)
{ {
processed_control_messages_++; processed_control_messages_++;
if (pmt::any_ref(msg).type() == typeid(channel_event_sptr)) const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
if (msg_type_hash_code == channel_event_type_hash_code_)
{ {
if (receiver_on_standby_ == false) if (receiver_on_standby_ == false)
{ {
channel_event_sptr new_event; const channel_event_sptr new_event = boost::any_cast<channel_event_sptr>(pmt::any_ref(msg));
new_event = boost::any_cast<channel_event_sptr>(pmt::any_ref(msg));
DLOG(INFO) << "New channel event rx from ch id: " << new_event->channel_id DLOG(INFO) << "New channel event rx from ch id: " << new_event->channel_id
<< " what: " << new_event->event_type; << " what: " << new_event->event_type;
flowgraph_->apply_action(new_event->channel_id, new_event->event_type); flowgraph_->apply_action(new_event->channel_id, new_event->event_type);
} }
} }
else if (pmt::any_ref(msg).type() == typeid(command_event_sptr)) else if (msg_type_hash_code == command_event_type_hash_code_)
{ {
command_event_sptr new_event; const command_event_sptr new_event = boost::any_cast<command_event_sptr>(pmt::any_ref(msg));
new_event = boost::any_cast<command_event_sptr>(pmt::any_ref(msg));
DLOG(INFO) << "New command event rx from ch id: " << new_event->command_id DLOG(INFO) << "New command event rx from ch id: " << new_event->command_id
<< " what: " << new_event->event_type; << " what: " << new_event->event_type;
@@ -445,7 +444,7 @@ bool ControlThread::read_assistance_from_XML()
gps_eph_iter++) gps_eph_iter++)
{ {
std::cout << "From XML file: Read NAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_eph_iter->second.i_satellite_PRN) << '\n'; std::cout << "From XML file: Read NAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_eph_iter->second.i_satellite_PRN) << '\n';
std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(gps_eph_iter->second); const std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(gps_eph_iter->second);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
ret = true; ret = true;
@@ -453,7 +452,7 @@ bool ControlThread::read_assistance_from_XML()
if (supl_client_acquisition_.load_utc_xml(utc_xml_filename) == true) if (supl_client_acquisition_.load_utc_xml(utc_xml_filename) == true)
{ {
std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(supl_client_acquisition_.gps_utc); const std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(supl_client_acquisition_.gps_utc);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
std::cout << "From XML file: Read GPS UTC model parameters.\n"; std::cout << "From XML file: Read GPS UTC model parameters.\n";
ret = true; ret = true;
@@ -461,7 +460,7 @@ bool ControlThread::read_assistance_from_XML()
if (supl_client_acquisition_.load_iono_xml(iono_xml_filename) == true) if (supl_client_acquisition_.load_iono_xml(iono_xml_filename) == true)
{ {
std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(supl_client_acquisition_.gps_iono); const std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(supl_client_acquisition_.gps_iono);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
std::cout << "From XML file: Read GPS ionosphere model parameters.\n"; std::cout << "From XML file: Read GPS ionosphere model parameters.\n";
ret = true; ret = true;
@@ -475,7 +474,7 @@ bool ControlThread::read_assistance_from_XML()
gps_alm_iter++) gps_alm_iter++)
{ {
std::cout << "From XML file: Read GPS almanac for satellite " << Gnss_Satellite("GPS", gps_alm_iter->second.i_satellite_PRN) << '\n'; std::cout << "From XML file: Read GPS almanac for satellite " << Gnss_Satellite("GPS", gps_alm_iter->second.i_satellite_PRN) << '\n';
std::shared_ptr<Gps_Almanac> tmp_obj = std::make_shared<Gps_Almanac>(gps_alm_iter->second); const std::shared_ptr<Gps_Almanac> tmp_obj = std::make_shared<Gps_Almanac>(gps_alm_iter->second);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
ret = true; ret = true;
@@ -492,7 +491,7 @@ bool ControlThread::read_assistance_from_XML()
gal_eph_iter++) gal_eph_iter++)
{ {
std::cout << "From XML file: Read ephemeris for satellite " << Gnss_Satellite("Galileo", gal_eph_iter->second.i_satellite_PRN) << '\n'; std::cout << "From XML file: Read ephemeris for satellite " << Gnss_Satellite("Galileo", gal_eph_iter->second.i_satellite_PRN) << '\n';
std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(gal_eph_iter->second); const std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(gal_eph_iter->second);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
ret = true; ret = true;
@@ -500,7 +499,7 @@ bool ControlThread::read_assistance_from_XML()
if (supl_client_acquisition_.load_gal_iono_xml(gal_iono_xml_filename) == true) if (supl_client_acquisition_.load_gal_iono_xml(gal_iono_xml_filename) == true)
{ {
std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(supl_client_acquisition_.gal_iono); const std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(supl_client_acquisition_.gal_iono);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
std::cout << "From XML file: Read Galileo ionosphere model parameters.\n"; std::cout << "From XML file: Read Galileo ionosphere model parameters.\n";
ret = true; ret = true;
@@ -508,7 +507,7 @@ bool ControlThread::read_assistance_from_XML()
if (supl_client_acquisition_.load_gal_utc_xml(gal_utc_xml_filename) == true) if (supl_client_acquisition_.load_gal_utc_xml(gal_utc_xml_filename) == true)
{ {
std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(supl_client_acquisition_.gal_utc); const std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(supl_client_acquisition_.gal_utc);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
std::cout << "From XML file: Read Galileo UTC model parameters.\n"; std::cout << "From XML file: Read Galileo UTC model parameters.\n";
ret = true; ret = true;
@@ -522,7 +521,7 @@ bool ControlThread::read_assistance_from_XML()
gal_alm_iter++) gal_alm_iter++)
{ {
std::cout << "From XML file: Read Galileo almanac for satellite " << Gnss_Satellite("Galileo", gal_alm_iter->second.i_satellite_PRN) << '\n'; std::cout << "From XML file: Read Galileo almanac for satellite " << Gnss_Satellite("Galileo", gal_alm_iter->second.i_satellite_PRN) << '\n';
std::shared_ptr<Galileo_Almanac> tmp_obj = std::make_shared<Galileo_Almanac>(gal_alm_iter->second); const std::shared_ptr<Galileo_Almanac> tmp_obj = std::make_shared<Galileo_Almanac>(gal_alm_iter->second);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
ret = true; ret = true;
@@ -539,7 +538,7 @@ bool ControlThread::read_assistance_from_XML()
gps_cnav_eph_iter++) gps_cnav_eph_iter++)
{ {
std::cout << "From XML file: Read CNAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_cnav_eph_iter->second.i_satellite_PRN) << '\n'; std::cout << "From XML file: Read CNAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_cnav_eph_iter->second.i_satellite_PRN) << '\n';
std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(gps_cnav_eph_iter->second); const std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(gps_cnav_eph_iter->second);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
ret = true; ret = true;
@@ -547,7 +546,7 @@ bool ControlThread::read_assistance_from_XML()
if (supl_client_acquisition_.load_cnav_utc_xml(cnav_utc_xml_filename) == true) if (supl_client_acquisition_.load_cnav_utc_xml(cnav_utc_xml_filename) == true)
{ {
std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(supl_client_acquisition_.gps_cnav_utc); const std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(supl_client_acquisition_.gps_cnav_utc);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
std::cout << "From XML file: Read GPS CNAV UTC model parameters.\n"; std::cout << "From XML file: Read GPS CNAV UTC model parameters.\n";
ret = true; ret = true;
@@ -564,7 +563,7 @@ bool ControlThread::read_assistance_from_XML()
glo_gnav_eph_iter++) glo_gnav_eph_iter++)
{ {
std::cout << "From XML file: Read GLONASS GNAV ephemeris for satellite " << Gnss_Satellite("GLONASS", glo_gnav_eph_iter->second.i_satellite_PRN) << '\n'; std::cout << "From XML file: Read GLONASS GNAV ephemeris for satellite " << Gnss_Satellite("GLONASS", glo_gnav_eph_iter->second.i_satellite_PRN) << '\n';
std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(glo_gnav_eph_iter->second); const std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(glo_gnav_eph_iter->second);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
ret = true; ret = true;
@@ -572,7 +571,7 @@ bool ControlThread::read_assistance_from_XML()
if (supl_client_acquisition_.load_glo_utc_xml(glo_utc_xml_filename) == true) if (supl_client_acquisition_.load_glo_utc_xml(glo_utc_xml_filename) == true)
{ {
std::shared_ptr<Glonass_Gnav_Utc_Model> tmp_obj = std::make_shared<Glonass_Gnav_Utc_Model>(supl_client_acquisition_.glo_gnav_utc); const std::shared_ptr<Glonass_Gnav_Utc_Model> tmp_obj = std::make_shared<Glonass_Gnav_Utc_Model>(supl_client_acquisition_.glo_gnav_utc);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
std::cout << "From XML file: Read GLONASS UTC model parameters.\n"; std::cout << "From XML file: Read GLONASS UTC model parameters.\n";
ret = true; ret = true;
@@ -586,14 +585,14 @@ bool ControlThread::read_assistance_from_XML()
} }
// Only look for {ref time, ref location} if SUPL is enabled // Only look for {ref time, ref location} if SUPL is enabled
bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false); const bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false);
if (enable_gps_supl_assistance == true) if (enable_gps_supl_assistance == true)
{ {
// Try to read Ref Time from XML // Try to read Ref Time from XML
if (supl_client_acquisition_.load_ref_time_xml(ref_time_xml_filename) == true) if (supl_client_acquisition_.load_ref_time_xml(ref_time_xml_filename) == true)
{ {
LOG(INFO) << "SUPL: Read XML Ref Time"; LOG(INFO) << "SUPL: Read XML Ref Time";
std::shared_ptr<Agnss_Ref_Time> tmp_obj = std::make_shared<Agnss_Ref_Time>(supl_client_acquisition_.gps_time); const std::shared_ptr<Agnss_Ref_Time> tmp_obj = std::make_shared<Agnss_Ref_Time>(supl_client_acquisition_.gps_time);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
else else
@@ -605,7 +604,7 @@ bool ControlThread::read_assistance_from_XML()
if (supl_client_acquisition_.load_ref_location_xml(ref_location_xml_filename) == true) if (supl_client_acquisition_.load_ref_location_xml(ref_location_xml_filename) == true)
{ {
LOG(INFO) << "SUPL: Read XML Ref Location"; LOG(INFO) << "SUPL: Read XML Ref Location";
std::shared_ptr<Agnss_Ref_Location> tmp_obj = std::make_shared<Agnss_Ref_Location>(supl_client_acquisition_.gps_ref_loc); const std::shared_ptr<Agnss_Ref_Location> tmp_obj = std::make_shared<Agnss_Ref_Location>(supl_client_acquisition_.gps_ref_loc);
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
else else
@@ -622,13 +621,13 @@ void ControlThread::assist_GNSS()
{ {
// ######### GNSS Assistance ################################# // ######### GNSS Assistance #################################
// GNSS Assistance configuration // GNSS Assistance configuration
bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false); const bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false);
bool enable_agnss_xml = configuration_->property("GNSS-SDR.AGNSS_XML_enabled", false); const bool enable_agnss_xml = configuration_->property("GNSS-SDR.AGNSS_XML_enabled", false);
if ((enable_gps_supl_assistance == true) and (enable_agnss_xml == false)) if ((enable_gps_supl_assistance == true) and (enable_agnss_xml == false))
{ {
std::cout << "SUPL RRLP GPS assistance enabled!\n"; std::cout << "SUPL RRLP GPS assistance enabled!\n";
std::string default_acq_server = "supl.google.com"; const std::string default_acq_server("supl.google.com");
std::string default_eph_server = "supl.google.com"; const std::string default_eph_server("supl.google.com");
supl_client_ephemeris_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_server", default_acq_server); supl_client_ephemeris_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_server", default_acq_server);
supl_client_acquisition_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_acquisition_server", default_eph_server); supl_client_acquisition_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_acquisition_server", default_eph_server);
supl_client_ephemeris_.server_port = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_port", 7275); supl_client_ephemeris_.server_port = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_port", 7275);
@@ -636,10 +635,10 @@ void ControlThread::assist_GNSS()
supl_mcc_ = configuration_->property("GNSS-SDR.SUPL_MCC", 244); supl_mcc_ = configuration_->property("GNSS-SDR.SUPL_MCC", 244);
supl_mns_ = configuration_->property("GNSS-SDR.SUPL_MNC ", 5); supl_mns_ = configuration_->property("GNSS-SDR.SUPL_MNC ", 5);
std::string default_lac = "0x59e2"; const std::string default_lac("0x59e2");
std::string default_ci = "0x31b0"; const std::string default_ci("0x31b0");
std::string supl_lac_s = configuration_->property("GNSS-SDR.SUPL_LAC", default_lac); const std::string supl_lac_s = configuration_->property("GNSS-SDR.SUPL_LAC", default_lac);
std::string supl_ci_s = configuration_->property("GNSS-SDR.SUPL_CI", default_ci); const std::string supl_ci_s = configuration_->property("GNSS-SDR.SUPL_CI", default_ci);
try try
{ {
supl_lac_ = std::stoi(supl_lac_s, nullptr, 0); supl_lac_ = std::stoi(supl_lac_s, nullptr, 0);
@@ -669,7 +668,7 @@ void ControlThread::assist_GNSS()
supl_ci_ = 0x31b0; supl_ci_ = 0x31b0;
} }
bool SUPL_read_gps_assistance_xml = configuration_->property("GNSS-SDR.SUPL_read_gps_assistance_xml", false); const bool SUPL_read_gps_assistance_xml = configuration_->property("GNSS-SDR.SUPL_read_gps_assistance_xml", false);
if (SUPL_read_gps_assistance_xml == true) if (SUPL_read_gps_assistance_xml == true)
{ {
// Read assistance from file // Read assistance from file
@@ -682,10 +681,9 @@ void ControlThread::assist_GNSS()
else else
{ {
// Request ephemeris from SUPL server // Request ephemeris from SUPL server
int error;
supl_client_ephemeris_.request = 1; supl_client_ephemeris_.request = 1;
std::cout << "SUPL: Try to read GPS ephemeris data from SUPL server...\n"; std::cout << "SUPL: Try to read GPS ephemeris data from SUPL server...\n";
error = supl_client_ephemeris_.get_assistance(supl_mcc_, supl_mns_, supl_lac_, supl_ci_); int error = supl_client_ephemeris_.get_assistance(supl_mcc_, supl_mns_, supl_lac_, supl_ci_);
if (error == 0) if (error == 0)
{ {
std::map<int, Gps_Ephemeris>::const_iterator gps_eph_iter; std::map<int, Gps_Ephemeris>::const_iterator gps_eph_iter;
@@ -748,7 +746,7 @@ void ControlThread::assist_GNSS()
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
} }
// Save iono and UTC model data to xml file // Save iono and UTC model data to xml file
std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename_); const std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename_);
if (supl_client_ephemeris_.save_iono_xml(iono_xml_filename, supl_client_ephemeris_.gps_iono) == true) if (supl_client_ephemeris_.save_iono_xml(iono_xml_filename, supl_client_ephemeris_.gps_iono) == true)
{ {
std::cout << "SUPL: Iono data file created\n"; std::cout << "SUPL: Iono data file created\n";
@@ -757,7 +755,7 @@ void ControlThread::assist_GNSS()
{ {
std::cout << "SUPL: Failed to create Iono data file\n"; std::cout << "SUPL: Failed to create Iono data file\n";
} }
std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model_xml", utc_default_xml_filename_); const std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model_xml", utc_default_xml_filename_);
if (supl_client_ephemeris_.save_utc_xml(utc_xml_filename, supl_client_ephemeris_.gps_utc) == true) if (supl_client_ephemeris_.save_utc_xml(utc_xml_filename, supl_client_ephemeris_.gps_utc) == true)
{ {
std::cout << "SUPL: UTC model data file created\n"; std::cout << "SUPL: UTC model data file created\n";
@@ -909,7 +907,7 @@ void ControlThread::apply_action(unsigned int what)
std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, const std::array<float, 3> &LLH) std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, const std::array<float, 3> &LLH)
{ {
// 1. Compute rx ECEF position from LLH WGS84 // 1. Compute rx ECEF position from LLH WGS84
arma::vec LLH_rad = arma::vec{degtorad(LLH[0]), degtorad(LLH[1]), LLH[2]}; const arma::vec LLH_rad = arma::vec{degtorad(LLH[0]), degtorad(LLH[1]), LLH[2]};
arma::mat C_tmp = arma::zeros(3, 3); arma::mat C_tmp = arma::zeros(3, 3);
arma::vec r_eb_e = arma::zeros(3, 1); arma::vec r_eb_e = arma::zeros(3, 1);
arma::vec v_eb_e = arma::zeros(3, 1); arma::vec v_eb_e = arma::zeros(3, 1);
@@ -927,15 +925,17 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
std::vector<unsigned int> visible_gps; std::vector<unsigned int> visible_gps;
std::vector<unsigned int> visible_gal; std::vector<unsigned int> visible_gal;
std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt(); std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt();
struct tm tstruct = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, nullptr}; struct tm tstruct
{
};
char buf[80]; char buf[80];
tstruct = *gmtime(&rx_utc_time); tstruct = *gmtime(&rx_utc_time);
strftime(buf, sizeof(buf), "%d/%m/%Y %H:%M:%S ", &tstruct); strftime(buf, sizeof(buf), "%d/%m/%Y %H:%M:%S ", &tstruct);
std::string str_time = std::string(buf); const std::string str_time = std::string(buf);
std::cout << "Get visible satellites at " << str_time std::cout << "Get visible satellites at " << str_time
<< "UTC, assuming RX position " << LLH[0] << " [deg], " << LLH[1] << " [deg], " << LLH[2] << " [m]\n"; << "UTC, assuming RX position " << LLH[0] << " [deg], " << LLH[1] << " [deg], " << LLH[2] << " [m]\n";
std::map<int, Gps_Ephemeris> gps_eph_map = pvt_ptr->get_gps_ephemeris(); const std::map<int, Gps_Ephemeris> gps_eph_map = pvt_ptr->get_gps_ephemeris();
for (auto &it : gps_eph_map) for (auto &it : gps_eph_map)
{ {
eph_t rtklib_eph = eph_to_rtklib(it.second, pre_2009_file_); eph_t rtklib_eph = eph_to_rtklib(it.second, pre_2009_file_);
@@ -947,8 +947,8 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
double Az; double Az;
double El; double El;
double dist_m; double dist_m;
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]}; const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e; const arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx); topocent(&Az, &El, &dist_m, r_eb_e, dx);
// push sat // push sat
if (El > 0) if (El > 0)
@@ -960,7 +960,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
} }
} }
std::map<int, Galileo_Ephemeris> gal_eph_map = pvt_ptr->get_galileo_ephemeris(); const std::map<int, Galileo_Ephemeris> gal_eph_map = pvt_ptr->get_galileo_ephemeris();
for (auto &it : gal_eph_map) for (auto &it : gal_eph_map)
{ {
eph_t rtklib_eph = eph_to_rtklib(it.second); eph_t rtklib_eph = eph_to_rtklib(it.second);
@@ -972,8 +972,8 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
double Az; double Az;
double El; double El;
double dist_m; double dist_m;
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]}; const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e; const arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx); topocent(&Az, &El, &dist_m, r_eb_e, dx);
// push sat // push sat
if (El > 0) if (El > 0)
@@ -985,7 +985,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
} }
} }
std::map<int, Gps_Almanac> gps_alm_map = pvt_ptr->get_gps_almanac(); const std::map<int, Gps_Almanac> gps_alm_map = pvt_ptr->get_gps_almanac();
for (auto &it : gps_alm_map) for (auto &it : gps_alm_map)
{ {
alm_t rtklib_alm = alm_to_rtklib(it.second); alm_t rtklib_alm = alm_to_rtklib(it.second);
@@ -998,8 +998,8 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
double Az; double Az;
double El; double El;
double dist_m; double dist_m;
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]}; const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e; const arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx); topocent(&Az, &El, &dist_m, r_eb_e, dx);
// push sat // push sat
std::vector<unsigned int>::iterator it2; std::vector<unsigned int>::iterator it2;
@@ -1015,7 +1015,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
} }
} }
std::map<int, Galileo_Almanac> gal_alm_map = pvt_ptr->get_galileo_almanac(); const std::map<int, Galileo_Almanac> gal_alm_map = pvt_ptr->get_galileo_almanac();
for (auto &it : gal_alm_map) for (auto &it : gal_alm_map)
{ {
alm_t rtklib_alm = alm_to_rtklib(it.second); alm_t rtklib_alm = alm_to_rtklib(it.second);
@@ -1028,8 +1028,8 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
double Az; double Az;
double El; double El;
double dist_m; double dist_m;
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]}; const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e; const arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx); topocent(&Az, &El, &dist_m, r_eb_e, dx);
// push sat // push sat
std::vector<unsigned int>::iterator it2; std::vector<unsigned int>::iterator it2;

19
src/core/receiver/control_thread.h
View File

@@ -26,16 +26,20 @@
#include "agnss_ref_location.h" // for Agnss_Ref_Location #include "agnss_ref_location.h" // for Agnss_Ref_Location
#include "agnss_ref_time.h" // for Agnss_Ref_Time #include "agnss_ref_time.h" // for Agnss_Ref_Time
#include "channel_event.h" // for channel_event_sptr
#include "command_event.h" // for command_event_sptr
#include "concurrent_queue.h" // for Concurrent_Queue #include "concurrent_queue.h" // for Concurrent_Queue
#include "gnss_sdr_supl_client.h" // for Gnss_Sdr_Supl_Client #include "gnss_sdr_supl_client.h" // for Gnss_Sdr_Supl_Client
#include "tcp_cmd_interface.h" // for TcpCmdInterface #include "tcp_cmd_interface.h" // for TcpCmdInterface
#include <pmt/pmt.h> #include <pmt/pmt.h>
#include <array> // for array #include <array> // for array
#include <memory> // for shared_ptr #include <cstddef> // for size_t
#include <string> // for string #include <memory> // for shared_ptr
#include <thread> // for std::thread #include <string> // for string
#include <utility> // for pair #include <thread> // for std::thread
#include <vector> // for vector #include <typeinfo> // for std::type_info, typeid
#include <utility> // for pair
#include <vector> // for vector
#ifdef ENABLE_FPGA #ifdef ENABLE_FPGA
#include <boost/thread.hpp> // for boost::thread #include <boost/thread.hpp> // for boost::thread
@@ -162,6 +166,9 @@ private:
const std::string gal_almanac_default_xml_filename_ = "./gal_almanac.xml"; const std::string gal_almanac_default_xml_filename_ = "./gal_almanac.xml";
const std::string gps_almanac_default_xml_filename_ = "./gps_almanac.xml"; const std::string gps_almanac_default_xml_filename_ = "./gps_almanac.xml";
const size_t channel_event_type_hash_code_ = typeid(channel_event_sptr).hash_code();
const size_t command_event_type_hash_code_ = typeid(command_event_sptr).hash_code();
std::shared_ptr<ConfigurationInterface> configuration_; std::shared_ptr<ConfigurationInterface> configuration_;
std::shared_ptr<Concurrent_Queue<pmt::pmt_t>> control_queue_; std::shared_ptr<Concurrent_Queue<pmt::pmt_t>> control_queue_;
std::shared_ptr<GNSSFlowgraph> flowgraph_; std::shared_ptr<GNSSFlowgraph> flowgraph_;

2
src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc
View File

@@ -163,7 +163,7 @@ void NmeaPrinterTest::conf()
TEST_F(NmeaPrinterTest, PrintLine) TEST_F(NmeaPrinterTest, PrintLine)
{ {
std::string filename("nmea_test.nmea"); std::string filename("nmea_test.nmea");
std::shared_ptr<Rtklib_Solver> pvt_solution = std::make_shared<Rtklib_Solver>(12, "filename", false, false, rtk); std::shared_ptr<Rtklib_Solver> pvt_solution = std::make_shared<Rtklib_Solver>(rtk, 12, "filename", false, false);
boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));

2
src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc
View File

@@ -386,7 +386,7 @@ TEST(RTKLibSolverTest, test1)
bool save_to_mat = false; bool save_to_mat = false;
rtk_t rtk = configure_rtklib_options(); rtk_t rtk = configure_rtklib_options();
auto d_ls_pvt = std::make_unique<Rtklib_Solver>(nchannels, dump_filename, flag_dump_to_file, save_to_mat, rtk); auto d_ls_pvt = std::make_unique<Rtklib_Solver>(rtk, nchannels, dump_filename, flag_dump_to_file, save_to_mat);
d_ls_pvt->set_averaging_depth(1); d_ls_pvt->set_averaging_depth(1);
// load ephemeris // load ephemeris