mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-18 21:23:02 +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
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
33 changed files with 1566 additions and 1575 deletions
Show Stats 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 <sys/ipc.h> // for IPC_CREAT
#include <sys/msg.h> // for msgctl
#include <typeinfo> // for std::type_info, typeid
#include <utility> // for pair
#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
// 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_pre_2009_file(conf_.pre_2009_file);
// internal PVT solver, mainly used to estimate the receiver clock
rtk_t internal_rtk = rtk;
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_pre_2009_file(conf_.pre_2009_file);
}
else
{
// 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_pre_2009_file(conf_.pre_2009_file);
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();
}
@ -1050,12 +1071,12 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
{
try
{
// ************* GPS telemetry *****************
if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Ephemeris>))
const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
// ************************* GPS telemetry *************************
if (msg_type_hash_code == d_gps_ephemeris_sptr_type_hash_code)
{
// ### GPS EPHEMERIS ###
std::shared_ptr<Gps_Ephemeris> gps_eph;
gps_eph = boost::any_cast<std::shared_ptr<Gps_Ephemeris>>(pmt::any_ref(msg));
const std::shared_ptr<Gps_Ephemeris> gps_eph = boost::any_cast<std::shared_ptr<Gps_Ephemeris>>(pmt::any_ref(msg));
DLOG(INFO) << "Ephemeris record has arrived from SAT ID "
<< gps_eph->i_satellite_PRN << " (Block "
<< 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;
}
}
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 ###
std::shared_ptr<Gps_Iono> gps_iono;
gps_iono = boost::any_cast<std::shared_ptr<Gps_Iono>>(pmt::any_ref(msg));
const std::shared_ptr<Gps_Iono> gps_iono = boost::any_cast<std::shared_ptr<Gps_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->gps_iono = *gps_iono;
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 ";
}
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 ###
std::shared_ptr<Gps_Utc_Model> gps_utc_model;
gps_utc_model = boost::any_cast<std::shared_ptr<Gps_Utc_Model>>(pmt::any_ref(msg));
const std::shared_ptr<Gps_Utc_Model> 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;
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 ";
}
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 ###
std::shared_ptr<Gps_CNAV_Ephemeris> gps_cnav_ephemeris;
gps_cnav_ephemeris = boost::any_cast<std::shared_ptr<Gps_CNAV_Ephemeris>>(pmt::any_ref(msg));
const std::shared_ptr<Gps_CNAV_Ephemeris> 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
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 ";
}
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 ###
std::shared_ptr<Gps_CNAV_Iono> gps_cnav_iono;
gps_cnav_iono = boost::any_cast<std::shared_ptr<Gps_CNAV_Iono>>(pmt::any_ref(msg));
const std::shared_ptr<Gps_CNAV_Iono> 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;
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 ";
}
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 ###
std::shared_ptr<Gps_CNAV_Utc_Model> gps_cnav_utc_model;
gps_cnav_utc_model = boost::any_cast<std::shared_ptr<Gps_CNAV_Utc_Model>>(pmt::any_ref(msg));
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));
d_internal_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 ";
}
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 ###
std::shared_ptr<Gps_Almanac> gps_almanac;
gps_almanac = boost::any_cast<std::shared_ptr<Gps_Almanac>>(pmt::any_ref(msg));
const std::shared_ptr<Gps_Almanac> 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;
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 ";
}
// **************** Galileo telemetry ********************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Ephemeris>))
// *********************** Galileo telemetry ***********************
else if (msg_type_hash_code == d_galileo_ephemeris_sptr_type_hash_code)
{
// ### Galileo EPHEMERIS ###
std::shared_ptr<Galileo_Ephemeris> galileo_eph;
galileo_eph = boost::any_cast<std::shared_ptr<Galileo_Ephemeris>>(pmt::any_ref(msg));
const std::shared_ptr<Galileo_Ephemeris> galileo_eph = boost::any_cast<std::shared_ptr<Galileo_Ephemeris>>(pmt::any_ref(msg));
// insert new ephemeris record
DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_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;
}
}
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 ###
std::shared_ptr<Galileo_Iono> galileo_iono;
galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
const std::shared_ptr<Galileo_Iono> galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
d_internal_pvt_solver->galileo_iono = *galileo_iono;
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 ";
}
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 ###
std::shared_ptr<Galileo_Utc_Model> galileo_utc_model;
galileo_utc_model = boost::any_cast<std::shared_ptr<Galileo_Utc_Model>>(pmt::any_ref(msg));
const std::shared_ptr<Galileo_Utc_Model> 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;
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 ";
}
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 ###
std::shared_ptr<Galileo_Almanac_Helper> galileo_almanac_helper;
galileo_almanac_helper = boost::any_cast<std::shared_ptr<Galileo_Almanac_Helper>>(pmt::any_ref(msg));
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 sv1 = galileo_almanac_helper->get_almanac(1);
Galileo_Almanac sv2 = galileo_almanac_helper->get_almanac(2);
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 ";
}
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 ###
std::shared_ptr<Galileo_Almanac> galileo_alm;
galileo_alm = boost::any_cast<std::shared_ptr<Galileo_Almanac>>(pmt::any_ref(msg));
const std::shared_ptr<Galileo_Almanac> galileo_alm = boost::any_cast<std::shared_ptr<Galileo_Almanac>>(pmt::any_ref(msg));
// update/insert new almanac record to the global almanac map
d_internal_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm;
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 **************************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>))
// **************** GLONASS GNAV Telemetry *************************
else if (msg_type_hash_code == d_glonass_gnav_ephemeris_sptr_type_hash_code)
{
// ### GLONASS GNAV EPHEMERIS ###
std::shared_ptr<Glonass_Gnav_Ephemeris> glonass_gnav_eph;
glonass_gnav_eph = boost::any_cast<std::shared_ptr<Glonass_Gnav_Ephemeris>>(pmt::any_ref(msg));
const std::shared_ptr<Glonass_Gnav_Ephemeris> 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,
// insert new ephemeris record
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;
}
}
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 ###
std::shared_ptr<Glonass_Gnav_Utc_Model> glonass_gnav_utc_model;
glonass_gnav_utc_model = boost::any_cast<std::shared_ptr<Glonass_Gnav_Utc_Model>>(pmt::any_ref(msg));
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));
d_internal_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model;
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 ";
}
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 ###
std::shared_ptr<Glonass_Gnav_Almanac> glonass_gnav_almanac;
glonass_gnav_almanac = boost::any_cast<std::shared_ptr<Glonass_Gnav_Almanac>>(pmt::any_ref(msg));
const std::shared_ptr<Glonass_Gnav_Almanac> 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;
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;
}
// ************* BeiDou telemetry *****************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Ephemeris>))
// *********************** BeiDou telemetry ************************
else if (msg_type_hash_code == d_beidou_dnav_ephemeris_sptr_type_hash_code)
{
// ### Beidou EPHEMERIS ###
std::shared_ptr<Beidou_Dnav_Ephemeris> bds_dnav_eph;
bds_dnav_eph = boost::any_cast<std::shared_ptr<Beidou_Dnav_Ephemeris>>(pmt::any_ref(msg));
const std::shared_ptr<Beidou_Dnav_Ephemeris> 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 "
<< bds_dnav_eph->i_satellite_PRN << " (Block "
<< 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;
}
}
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 ###
std::shared_ptr<Beidou_Dnav_Iono> bds_dnav_iono;
bds_dnav_iono = boost::any_cast<std::shared_ptr<Beidou_Dnav_Iono>>(pmt::any_ref(msg));
const std::shared_ptr<Beidou_Dnav_Iono> 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;
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 ";
}
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 ###
std::shared_ptr<Beidou_Dnav_Utc_Model> bds_dnav_utc_model;
bds_dnav_utc_model = boost::any_cast<std::shared_ptr<Beidou_Dnav_Utc_Model>>(pmt::any_ref(msg));
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));
d_internal_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model;
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 ";
}
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 ###
std::shared_ptr<Beidou_Dnav_Almanac> bds_dnav_almanac;
bds_dnav_almanac = boost::any_cast<std::shared_ptr<Beidou_Dnav_Almanac>>(pmt::any_ref(msg));
const std::shared_ptr<Beidou_Dnav_Almanac> 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;
if (d_enable_rx_clock_correction == true)
{
@ -1917,7 +1919,7 @@ void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset()
default:
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_channel_initialized.at(observables_iter->second.Channel_ID) = true;
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)
{
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);
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);
auto tmp_eph_iter_bds_dnav = d_internal_pvt_solver->beidou_dnav_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);
const auto tmp_eph_iter_gal = d_internal_pvt_solver->galileo_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);
const auto tmp_eph_iter_glo_gnav = d_internal_pvt_solver->glonass_gnav_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;
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"))
{
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())
{
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")))
{
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())
{
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")))
{
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())
{
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")))
{
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())
{
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")))
{
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
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 !!
{
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;
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 (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;
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 (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;
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 (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;
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 (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;
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 (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;
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 (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;
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 (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;
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 (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;
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 (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();
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 (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;
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 (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();
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 (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;
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 (system == "G")
@ -4177,7 +4179,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
// PVT MONITOR
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
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 <pmt/pmt.h> // for pmt_t
#include <chrono> // for system_clock
#include <cstddef> // for size_t
#include <cstdint> // for int32_t
#include <ctime> // for time_t
#include <map> // for map
@ -202,6 +203,26 @@ private:
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_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)
{
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt);
const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
const tm timeinfo = boost::posix_time::to_tm(pt);
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)
{
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt);
const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
const tm timeinfo = boost::posix_time::to_tm(pt);
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 << R"(<?xml version="1.0" encoding="UTF-8"?>)" << '\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 << "xmlns=\"http://www.topografix.com/GPX/1/1\"" << '\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:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << '\n'
<< indent << "<trk>" << '\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 << "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:gpxx=\"http://www.garmin.com/xmlschemas/GpxExtensions/v3\"\n"
<< indent << "xmlns:gpxtpx=\"http://www.garmin.com/xmlschemas/TrackPointExtension/v2\"\n"
<< indent << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">\n"
<< indent << "<trk>\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 << "<trkseg>\n";
return true;
}
@ -170,12 +170,12 @@ bool Gpx_Printer::print_position(const Pvt_Solution* position, bool print_averag
positions_printed = true;
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 speed_over_ground = position->get_speed_over_ground(); // expressed in m/s
const double course_over_ground = position->get_course_over_ground(); // expressed in deg
double hdop = position->get_hdop();
double vdop = position->get_vdop();
double pdop = position->get_pdop();
const double hdop = position->get_hdop();
const double vdop = position->get_vdop();
const double pdop = position->get_pdop();
std::string utc_time = to_iso_extended_string(position->get_position_UTC_time());
if (utc_time.length() < 23)
{
@ -216,8 +216,8 @@ bool Gpx_Printer::close_file()
{
if (gpx_file.is_open())
{
gpx_file << indent << indent << "</trkseg>" << '\n'
<< indent << "</trk>" << '\n'
gpx_file << indent << indent << "</trkseg>\n"
<< indent << "</trk>\n"
<< "</gpx>";
gpx_file.close();
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)
{
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt);
const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
const tm timeinfo = boost::posix_time::to_tm(pt);
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'
<< 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 << indent << "<name>GNSS Track</name>" << '\n'
<< indent << indent << "<description><![CDATA[" << '\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>https://gnss-sdr.org/</td></tr>" << '\n'
<< indent << indent << indent << "</table>" << '\n'
<< indent << indent << "]]></description>" << '\n'
<< indent << indent << "<!-- Normal track style -->" << '\n'
<< indent << indent << "<Style id=\"track_n\">" << '\n'
<< indent << indent << indent << "<IconStyle>" << '\n'
<< indent << indent << indent << indent << "<color>ff00ffff</color>" << '\n'
<< indent << indent << indent << indent << "<scale>0.3</scale>" << '\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 << "</Icon>" << '\n'
<< indent << indent << indent << "</IconStyle>" << '\n'
<< indent << indent << indent << "<LabelStyle>" << '\n'
<< indent << indent << indent << indent << "<scale>0</scale>" << '\n'
<< indent << indent << indent << "</LabelStyle>" << '\n'
<< indent << indent << "</Style>" << '\n'
<< indent << indent << "<!-- Highlighted track style -->" << '\n'
<< indent << indent << "<Style id=\"track_h\">" << '\n'
<< indent << indent << indent << "<IconStyle>" << '\n'
<< indent << indent << indent << indent << "<color>ff00ffff</color>" << '\n'
<< indent << indent << indent << indent << "<scale>1</scale>" << '\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 << "</Icon>" << '\n'
<< indent << indent << indent << "</IconStyle>" << '\n'
<< indent << indent << "</Style>" << '\n'
<< indent << indent << "<StyleMap id=\"track\">" << '\n'
<< indent << indent << indent << "<Pair>" << '\n'
<< indent << indent << indent << indent << "<key>normal</key>" << '\n'
<< indent << indent << indent << indent << "<styleUrl>#track_n</styleUrl>" << '\n'
<< indent << indent << indent << "</Pair>" << '\n'
<< indent << indent << indent << "<Pair>" << '\n'
<< indent << indent << indent << indent << "<key>highlight</key>" << '\n'
<< indent << indent << indent << indent << "<styleUrl>#track_h</styleUrl>" << '\n'
<< indent << indent << indent << "</Pair>" << '\n'
<< indent << indent << "</StyleMap>" << '\n'
<< indent << indent << "<Style id=\"yellowLineGreenPoly\">" << '\n'
<< indent << indent << indent << "<LineStyle>" << '\n'
<< indent << indent << indent << indent << "<color>7f00ffff</color>" << '\n'
<< indent << indent << indent << indent << "<width>1</width>" << '\n'
<< indent << indent << indent << "</LineStyle>" << '\n'
<< indent << indent << indent << "<PolyStyle>" << '\n'
<< indent << indent << indent << indent << "<color>7f00ff00</color>" << '\n'
<< indent << indent << indent << "</PolyStyle>" << '\n'
<< indent << indent << "</Style>" << '\n'
<< indent << indent << "<Folder>" << '\n'
<< indent << "<Document>\n"
<< indent << indent << "<name>GNSS Track</name>\n"
<< indent << indent << "<description><![CDATA[\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>https://gnss-sdr.org/</td></tr>\n"
<< indent << indent << indent << "</table>\n"
<< indent << indent << "]]></description>\n"
<< indent << indent << "<!-- Normal track style -->\n"
<< indent << indent << "<Style id=\"track_n\">\n"
<< indent << indent << indent << "<IconStyle>\n"
<< indent << indent << indent << indent << "<color>ff00ffff</color>\n"
<< indent << indent << indent << indent << "<scale>0.3</scale>\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 << "</Icon>\n"
<< indent << indent << indent << "</IconStyle>\n"
<< indent << indent << indent << "<LabelStyle>\n"
<< indent << indent << indent << indent << "<scale>0</scale>\n"
<< indent << indent << indent << "</LabelStyle>\n"
<< indent << indent << "</Style>\n"
<< indent << indent << "<!-- Highlighted track style -->\n"
<< indent << indent << "<Style id=\"track_h\">\n"
<< indent << indent << indent << "<IconStyle>\n"
<< indent << indent << indent << indent << "<color>ff00ffff</color>\n"
<< indent << indent << indent << indent << "<scale>1</scale>\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 << "</Icon>\n"
<< indent << indent << indent << "</IconStyle>\n"
<< indent << indent << "</Style>\n"
<< indent << indent << "<StyleMap id=\"track\">\n"
<< indent << indent << indent << "<Pair>\n"
<< indent << indent << indent << indent << "<key>normal</key>\n"
<< indent << indent << indent << indent << "<styleUrl>#track_n</styleUrl>\n"
<< indent << indent << indent << "</Pair>\n"
<< indent << indent << indent << "<Pair>\n"
<< indent << indent << indent << indent << "<key>highlight</key>\n"
<< indent << indent << indent << indent << "<styleUrl>#track_h</styleUrl>\n"
<< indent << indent << indent << "</Pair>\n"
<< indent << indent << "</StyleMap>\n"
<< indent << indent << "<Style id=\"yellowLineGreenPoly\">\n"
<< indent << indent << indent << "<LineStyle>\n"
<< indent << indent << indent << indent << "<color>7f00ffff</color>\n"
<< indent << indent << indent << indent << "<width>1</width>\n"
<< indent << indent << indent << "</LineStyle>\n"
<< indent << indent << indent << "<PolyStyle>\n"
<< indent << indent << indent << indent << "<color>7f00ff00</color>\n"
<< indent << indent << indent << "</PolyStyle>\n"
<< indent << indent << "</Style>\n"
<< indent << indent << "<Folder>\n"
<< indent << indent << indent << "<name>Points</name>\n";
return true;
@ -239,12 +239,12 @@ bool Kml_Printer::print_position(const Pvt_Solution* position, bool print_averag
positions_printed = true;
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 speed_over_ground = position->get_speed_over_ground(); // expressed in m/s
const double course_over_ground = position->get_course_over_ground(); // expressed in deg
double hdop = position->get_hdop();
double vdop = position->get_vdop();
double pdop = position->get_pdop();
const double hdop = position->get_hdop();
const double vdop = position->get_vdop();
const double pdop = position->get_pdop();
std::string utc_time = to_iso_extended_string(position->get_position_UTC_time());
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())
{
point_id++;
kml_file << indent << indent << indent << "<Placemark>" << '\n'
<< indent << indent << indent << indent << "<name>" << point_id << "</name>" << '\n'
<< indent << indent << indent << indent << "<snippet/>" << '\n'
<< indent << indent << indent << indent << "<description><![CDATA[" << '\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>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>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>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>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 << "</table>" << '\n'
<< indent << indent << indent << indent << "]]></description>" << '\n'
<< indent << indent << indent << indent << "<TimeStamp>" << '\n'
<< indent << indent << indent << indent << indent << "<when>" << utc_time << "</when>" << '\n'
<< indent << indent << indent << indent << "</TimeStamp>" << '\n'
<< indent << indent << indent << indent << "<styleUrl>#track</styleUrl>" << '\n'
<< indent << indent << indent << indent << "<Point>" << '\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 << "</Point>" << '\n'
kml_file << indent << indent << indent << "<Placemark>\n"
<< indent << indent << indent << indent << "<name>" << point_id << "</name>\n"
<< indent << indent << indent << indent << "<snippet/>\n"
<< indent << indent << indent << indent << "<description><![CDATA[\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>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>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>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>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 << "</table>\n"
<< indent << indent << indent << indent << "]]></description>\n"
<< indent << indent << indent << indent << "<TimeStamp>\n"
<< indent << indent << indent << indent << indent << "<when>" << utc_time << "</when>\n"
<< indent << indent << indent << indent << "</TimeStamp>\n"
<< indent << indent << indent << indent << "<styleUrl>#track</styleUrl>\n"
<< indent << indent << indent << indent << "<Point>\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 << "</Point>\n"
<< indent << indent << indent << "</Placemark>\n";
tmp_file << indent << indent << indent << indent << indent
@ -311,23 +311,23 @@ bool Kml_Printer::close_file()
tmp_file.close();
kml_file << indent << indent << "</Folder>"
<< indent << indent << "<Placemark>" << '\n'
<< indent << indent << indent << "<name>Path</name>" << '\n'
<< indent << indent << indent << "<styleUrl>#yellowLineGreenPoly</styleUrl>" << '\n'
<< indent << indent << indent << "<LineString>" << '\n'
<< indent << indent << indent << indent << "<extrude>0</extrude>" << '\n'
<< indent << indent << indent << indent << "<tessellate>1</tessellate>" << '\n'
<< indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << '\n'
<< indent << indent << "<Placemark>\n"
<< indent << indent << indent << "<name>Path</name>\n"
<< indent << indent << indent << "<styleUrl>#yellowLineGreenPoly</styleUrl>\n"
<< indent << indent << indent << "<LineString>\n"
<< indent << indent << indent << indent << "<extrude>0</extrude>\n"
<< indent << indent << indent << indent << "<tessellate>1</tessellate>\n"
<< indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>\n"
<< indent << indent << indent << indent << "<coordinates>\n";
// Copy the contents of tmp_file into kml_file
std::ifstream src(tmp_file_str, std::ios::binary);
kml_file << src.rdbuf();
kml_file << indent << indent << indent << indent << "</coordinates>" << '\n'
<< indent << indent << indent << "</LineString>" << '\n'
<< indent << indent << "</Placemark>" << '\n'
<< indent << "</Document>" << '\n'
kml_file << indent << indent << indent << indent << "</coordinates>\n"
<< indent << indent << indent << "</LineString>\n"
<< indent << indent << "</Placemark>\n"
<< indent << "</Document>\n"
<< "</kml>";
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()
{
DLOG(INFO) << "NMEA printer destructor called.";
auto pos = nmea_file_descriptor.tellp();
const auto pos = nmea_file_descriptor.tellp();
try
{
if (nmea_file_descriptor.is_open())
@ -170,11 +170,11 @@ int Nmea_Printer::init_serial(const std::string& serial_device)
// clang-format off
struct termios options{};
// clang-format on
int64_t BAUD;
int64_t DATABITS;
int64_t STOPBITS;
int64_t PARITYON;
int64_t PARITY;
const int64_t BAUD = B9600; // BAUD = B38400;
const int64_t DATABITS = CS8;
const int64_t STOPBITS = 0;
const int64_t PARITYON = 0;
const int64_t PARITY = 0;
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC);
if (fd == -1)
@ -188,13 +188,6 @@ int Nmea_Printer::init_serial(const std::string& serial_device)
}
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;
// enable receiver, set 8 bit data, ignore control lines
// 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)
{
std::string GPRMC;
std::string GPGGA;
std::string GPGSA;
std::string GPGSV;
// set the new PVT data
d_PVT_data = pvt_data;
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
// GPRMC
GPRMC = get_GPRMC();
const std::string GPRMC = get_GPRMC();
// GPGGA (Global Positioning System Fixed Data)
GPGGA = get_GPGGA();
const std::string GPGGA = get_GPGGA();
// GPGSA
GPGSA = get_GPGSA();
const std::string GPGSA = get_GPGSA();
// GPGSV
GPGSV = get_GPGSV();
const std::string GPGSV = get_GPGSV();
// write to log file
if (d_flag_nmea_output_file)
@ -311,7 +299,7 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
north = true;
}
int deg = static_cast<int>(lat);
const int deg = static_cast<int>(lat);
double mins = lat - static_cast<double>(deg);
mins *= 60.0;
std::ostringstream out_string;
@ -348,7 +336,7 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
{
east = true;
}
int deg = static_cast<int>(longitude);
const int deg = static_cast<int>(longitude);
double mins = longitude - static_cast<double>(deg);
mins *= 60.0;
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
std::stringstream sentence_str;
boost::posix_time::time_duration td = d_position_UTC_time.time_of_day();
int utc_hours = td.hours();
int utc_mins = td.minutes();
int utc_seconds = td.seconds();
auto utc_milliseconds = static_cast<int>(td.total_milliseconds() - td.total_seconds() * 1000);
const boost::posix_time::time_duration td = d_position_UTC_time.time_of_day();
const int utc_hours = td.hours();
const int utc_mins = td.minutes();
const int utc_seconds = td.seconds();
const auto utc_milliseconds = static_cast<int>(td.total_milliseconds() - td.total_seconds() * 1000);
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)
{
boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
tm timeinfo = boost::posix_time::to_tm(pt);
const boost::posix_time::ptime pt = boost::posix_time::second_clock::local_time();
const tm timeinfo = boost::posix_time::to_tm(pt);
d_rtcm_file_dump = flag_rtcm_file_dump;
rtcm_base_path = base_path;
if (d_rtcm_file_dump)
@ -200,8 +200,7 @@ Rtcm_Printer::~Rtcm_Printer()
}
if (rtcm_file_descriptor.is_open())
{
int64_t pos;
pos = rtcm_file_descriptor.tellp();
const auto pos = rtcm_file_descriptor.tellp();
try
{
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)
{
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);
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)
{
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);
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)
{
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);
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)
{
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);
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)
{
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);
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)
{
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);
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)
{
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);
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)
{
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);
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)
{
std::string m1019 = rtcm->print_MT1019(gps_eph);
const std::string m1019 = rtcm->print_MT1019(gps_eph);
Rtcm_Printer::Print_Message(m1019);
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)
{
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);
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)
{
std::string m1045 = rtcm->print_MT1045(gal_eph);
const std::string m1045 = rtcm->print_MT1045(gal_eph);
Rtcm_Printer::Print_Message(m1045);
return true;
}
@ -378,11 +377,11 @@ int Rtcm_Printer::init_serial(const std::string& serial_device)
// clang-format off
struct termios options{};
// clang-format on
int64_t BAUD;
int64_t DATABITS;
int64_t STOPBITS;
int64_t PARITYON;
int64_t PARITY;
const int64_t BAUD = B9600; // BAUD = B38400;
const int64_t DATABITS = CS8;
const int64_t STOPBITS = 0;
const int64_t PARITYON = 0;
const int64_t PARITY = 0;
fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_CLOEXEC);
if (fd == -1)
@ -396,13 +395,6 @@ int Rtcm_Printer::init_serial(const std::string& serial_device)
}
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;
// enable receiver, set 8 bit data, ignore control lines
// 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
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
rtk_ = rtk;
d_nchannels = nchannels;
d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file;
d_flag_dump_mat_enabled = flag_dump_to_mat;
this->set_averaging_flag(false);
rtk_ = rtk;
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
@ -96,7 +96,7 @@ Rtklib_Solver::~Rtklib_Solver()
DLOG(INFO) << "Rtklib_Solver destructor called.";
if (d_dump_file.is_open() == true)
{
auto pos = d_dump_file.tellp();
const auto pos = d_dump_file.tellp();
try
{
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
std::string dump_filename = d_dump_filename;
std::ifstream::pos_type size;
int32_t number_of_double_vars = 21;
int32_t number_of_uint32_vars = 2;
int32_t number_of_uint8_vars = 3;
int32_t number_of_float_vars = 2;
int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars +
sizeof(uint32_t) * number_of_uint32_vars +
sizeof(uint8_t) * number_of_uint8_vars +
sizeof(float) * number_of_float_vars;
const std::string dump_filename = d_dump_filename;
const int32_t number_of_double_vars = 21;
const int32_t number_of_uint32_vars = 2;
const int32_t number_of_uint8_vars = 3;
const int32_t number_of_float_vars = 2;
const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars +
sizeof(uint32_t) * number_of_uint32_vars +
sizeof(uint8_t) * number_of_uint8_vars +
sizeof(float) * number_of_float_vars;
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
@ -158,7 +157,7 @@ bool Rtklib_Solver::save_matfile()
if (dump_file.is_open())
{
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);
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':
{
std::string sig_(gnss_observables_iter->second.Signal);
const std::string sig_(gnss_observables_iter->second.Signal);
if (sig_ == "1C")
{
band1 = true;
@ -462,7 +461,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{
case 'E':
{
std::string sig_(gnss_observables_iter->second.Signal);
const std::string sig_(gnss_observables_iter->second.Signal);
// Galileo E1
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
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
// 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,
gnss_observables_iter->second,
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
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
// 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', {}, {},
{default_code_, default_code_, default_code_},
{}, {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
eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second, this->is_pre_2009());
// 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,
gnss_observables_iter->second,
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
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
// 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', {}, {},
{default_code_, default_code_, default_code_},
{}, {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
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
// 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', {}, {},
{default_code_, default_code_, default_code_},
{}, {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
{
std::string sig_(gnss_observables_iter->second.Signal);
const std::string sig_(gnss_observables_iter->second.Signal);
// GLONASS GNAV L1
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
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
// 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,
gnss_observables_iter->second,
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
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
// 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,
gnss_observables_iter->second,
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
// - 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")
{
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
eph_data[valid_obs] = eph_to_rtklib(beidou_ephemeris_iter->second);
// 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,
gnss_observables_iter->second,
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
eph_data[valid_obs] = eph_to_rtklib(beidou_ephemeris_iter->second);
// 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', {}, {},
{default_code_, default_code_, default_code_},
{}, {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
<< " 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)
// 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
gtime_t rtklib_utc_time = gpst2utc(rtklib_time);
p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
const gtime_t rtklib_time = timeadd(pvt_sol.time, rx_position_and_time[3]); // uncorrected rx time
const gtime_t rtklib_utc_time = gpst2utc(rtklib_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)
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]});
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);
// User clock drift [ppm]

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

@ -68,7 +68,7 @@
class Rtklib_Solver : public Pvt_Solution
{
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();
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;
private:
bool save_matfile();
bool save_matfile() const;
std::array<obsd_t, MAXOBS> obs_data{};
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
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));
memcpy(d_fft_if->get_inbuf() + offset, code, sizeof(gr_complex) * offset);
}
@ -309,7 +309,7 @@ void pcps_acquisition::init()
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_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++)
{
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));
}
}
@ -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++)
{
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);
}
}
@ -620,7 +620,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
// Initialize acquisition algorithm
int32_t doppler = 0;
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)
{
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();
// 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)
{
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
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)
{
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_active = false;
bool was_step_two = d_step_two;
const bool was_step_two = d_step_two;
d_step_two = false;
if (was_step_two)
{
@ -849,7 +849,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
else
{
d_state = 0; // Negative acquisition
bool was_step_two = d_step_two;
const bool was_step_two = d_step_two;
d_step_two = false;
if (was_step_two)
{
@ -884,17 +884,17 @@ bool pcps_acquisition::start()
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)
{
return;
}
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 auto effective_fft_size = static_cast<int>(d_acq_parameters.bit_transition_flag ? (d_fft_size / 2) : d_fft_size);
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))));
}

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;
uint32_t total_block_exp;
uint64_t initial_sample;
int32_t doppler;
d_acquisition_fpga->set_doppler_sweep(num_doppler_bins, doppler_step, doppler_min);
d_acquisition_fpga->run_acquisition();
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,
&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)
{
@ -276,7 +276,7 @@ void pcps_acquisition_fpga::set_active(bool active)
d_state = 0; // Positive acquisition
break;
}
num_second_acq = num_second_acq + 1;
num_second_acq += 1;
}
d_acquisition_fpga->close_device();
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.";
if (d_dump_file.is_open())
{
auto pos = d_dump_file.tellp();
const auto pos = d_dump_file.tellp();
try
{
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
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;
new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg));
const auto 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));
// align the receiver clock to integer multiple of 20 ms
if (d_T_rx_TOW_ms % 20)
@ -227,8 +226,8 @@ int32_t hybrid_observables_gs::save_matfile() const
// READ DUMP FILE
std::string dump_filename = d_dump_filename;
std::ifstream::pos_type size;
int32_t number_of_double_vars = 7;
int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels_out;
const int32_t number_of_double_vars = 7;
const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels_out;
std::ifstream dump_file;
std::cout << "Generating .mat file for " << dump_filename << '\n';
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);
// 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) /
(d_gnss_synchro_history->get(ch, t2_idx).RX_time -
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, t1_idx).RX_time);
// 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;
@ -517,8 +516,8 @@ void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data) con
// std::cout.precision(17);
// std::cout << " d_T_rx_TOW_ms: " << static_cast<double>(d_T_rx_TOW_ms) << '\n';
std::vector<Gnss_Synchro>::iterator it;
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 auto current_T_rx_TOW_ms = static_cast<double>(d_T_rx_TOW_ms);
const double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0;
for (it = data.begin(); it != data.end(); it++)
{
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
// 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];
double factor = ((d_smooth_filter_M - 1.0) / d_smooth_filter_M);
const double r_sm = d_channel_last_pseudorange_smooth[it->Channel_ID];
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]);
}
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++)
{
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
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 <gnuradio/block.h> // for block
#include <gnuradio/types.h> // for gr_vector_int
#include <cstddef> // for size_t
#include <cstdint> // for int32_t
#include <fstream> // for std::ofstream
#include <map> // for std::map
#include <memory> // for std::shared, std:unique_ptr
#include <string> // for std::string
#include <typeinfo> // for typeid
#include <vector> // for std::vector
#if GNURADIO_USES_STD_POINTERS
#else
@ -68,6 +70,8 @@ private:
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);
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;

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)
{
// 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));
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';
@ -249,7 +249,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
if (d_nav.have_new_utc_model() == true)
{
// 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));
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';
@ -257,7 +257,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
if (d_nav.have_new_iono() == true)
{
// 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));
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';
@ -362,6 +362,7 @@ void beidou_b1i_telemetry_decoder_gs::set_channel(int32_t channel)
}
}
void beidou_b1i_telemetry_decoder_gs::reset()
{
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;
}
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)
{
@ -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;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
d_stat = 2;
// ******* 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
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
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
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms;
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)
{
// 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());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
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)
{
// 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());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
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)
{
// 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());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
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
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
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
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms;
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_datalength = 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);
@ -166,8 +166,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
d_symbol_history.set_capacity(d_required_symbols + 1);
// vars for Viterbi decoder
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
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
d_out0.reserve(max_states);
d_out1.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)
{
// 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';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_inav_nav.have_new_iono_and_GST() == true)
{
// 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';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_inav_nav.have_new_utc_model() == true)
{
// 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';
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)));
@ -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)
{
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));
// debug
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)
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));
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';
@ -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)
{
// 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));
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';
}
if (d_nav.have_new_almanac() == true)
{
uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number();
std::shared_ptr<Glonass_Gnav_Almanac>
const uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number();
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));
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
// 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{};
// ******* 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)
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));
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';
@ -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)
{
// 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));
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';
}
if (d_nav.have_new_almanac() == true)
{
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 uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number();
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));
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';
@ -325,7 +325,7 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
{
// NEW GLONASS string received
// 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{};
// ******* 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)
{
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
// 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-
// and-xor loops.
d1 = gpsword & 0xFBFFBF00U;
d2 = my_rotl::rotl(gpsword, 1U) & 0x07FFBF01U;
d3 = my_rotl::rotl(gpsword, 2U) & 0xFC0F8100U;
d4 = my_rotl::rotl(gpsword, 3U) & 0xF81FFE02U;
d5 = my_rotl::rotl(gpsword, 4U) & 0xFC00000EU;
d6 = my_rotl::rotl(gpsword, 5U) & 0x07F00001U;
d7 = my_rotl::rotl(gpsword, 6U) & 0x00003000U;
t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;
const uint32_t d1 = gpsword & 0xFBFFBF00U;
const uint32_t d2 = my_rotl::rotl(gpsword, 1U) & 0x07FFBF01U;
const uint32_t d3 = my_rotl::rotl(gpsword, 2U) & 0xFC0F8100U;
const uint32_t d4 = my_rotl::rotl(gpsword, 3U) & 0xF81FFE02U;
const uint32_t d5 = my_rotl::rotl(gpsword, 4U) & 0xFC00000EU;
const uint32_t d6 = my_rotl::rotl(gpsword, 5U) & 0x07F00001U;
const uint32_t d7 = my_rotl::rotl(gpsword, 6U) & 0x00003000U;
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.
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;
if (parity == (gpsword & 0x3FU))
{
@ -269,7 +260,7 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe()
// NEW GPS SUBFRAME HAS ARRIVED!
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)
{
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;
// 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);
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)
{
// 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';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
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';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
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';
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)
{
// 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';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
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';
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
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';
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
// 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;
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)
{
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;
// fill two vectors with the two possible symbol alignments
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_vd2(nbits_requested);
// decode
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_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1.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
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)
{
std::stringstream ss;
uint32_t sbas_msg_length = 250;
std::vector<std::vector<int32_t>> preambles = {{0, 1, 0, 1, 0, 0, 1, 1},
const uint32_t sbas_msg_length = 250;
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, 1, 0, 0, 0, 1, 1, 0}};
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
d_checksum_agent.reset(0);
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()
<< ": final crc remainder= " << std::hex << crc
<< 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;
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();
int32_t bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
const int32_t idx_bit = candidate_bit_it - msg_candidate.begin();
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;
ss << *candidate_bit_it;
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;
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;
ss << *candidate_bit_it;
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
// 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
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
// and obtain the symbols by summing the paired correlation samples
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
// and obtain the bits by decoding the symbol pairs
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
// 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;
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;
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
<< " (sample_stamp=" << sample_stamp
<< " 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
{
if (pmt::any_ref(msg).type() == typeid(int))
if (pmt::any_ref(msg).type().hash_code() == int_type_hash_code)
{
int tlm_event;
tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
const int tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
if (tlm_event == 1)
{
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)
{
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_data_code, Signal_, d_acquisition_gnss_synchro->PRN);
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")
{
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_);
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_cn0_estimation_counter++;
// 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);
// Carrier lock indicator
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
std::ifstream::pos_type size;
int32_t number_of_double_vars = 1;
int32_t number_of_float_vars = 19;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
const int32_t number_of_double_vars = 1;
const int32_t number_of_float_vars = 19;
const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
std::string dump_filename_ = d_dump_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
{
// 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);
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 int64_t acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp);
const double acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / d_trk_parameters.fs_in;
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_phase_step_chips = d_code_freq_chips / d_trk_parameters.fs_in;
d_code_phase_rate_step_chips = 0.0;
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);
double T_prn_mod_samples = T_prn_mod_seconds * d_trk_parameters.fs_in;
const double T_chip_mod_seconds = 1.0 / d_code_freq_chips;
const double T_prn_mod_seconds = T_chip_mod_seconds * static_cast<double>(d_code_length_chips);
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_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_state = 2;
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 <pmt/pmt.h> // for pmt_t
#include <volk_gnsssdr/volk_gnsssdr_alloc.h> // for volk_gnsssdr::vector
#include <cstddef> // for size_t
#include <cstdint> // for int32_t
#include <fstream> // for string, ofstream
#include <string>
#include <utility> // for pair
#include <fstream> // for ofstream
#include <string> // for string
#include <typeinfo> // for typeid
#include <utility> // for pair
#if GNURADIO_USES_STD_POINTERS
#include <memory>
#else
@ -113,6 +115,8 @@ private:
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
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_code_period;
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
std::string device_name = d_trk_parameters.device_name;
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 std::string device_name = d_trk_parameters.device_name;
const uint32_t dev_file_num = d_trk_parameters.dev_file_num;
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 *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);
@ -489,11 +489,9 @@ void dll_pll_veml_tracking_fpga::msg_handler_telemetry_to_trk(const pmt::pmt_t &
{
try
{
if (pmt::any_ref(msg).type() == typeid(int))
if (pmt::any_ref(msg).type().hash_code() == int_type_hash_code)
{
int tlm_event;
tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
const int tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
if (tlm_event == 1)
{
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_cn0_estimation_counter++;
// 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);
// Carrier lock indicator
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())
{
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)
{
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);
d_carrier_loop_filter.initialize(static_cast<float>(d_carrier_doppler_hz) - carrier_doppler_error_hz);
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
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
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;
// ################## PLL COMMANDS #################################################
@ -1119,10 +1117,10 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile() const
{
// READ DUMP FILE
std::ifstream::pos_type size;
int32_t number_of_double_vars = 1;
int32_t number_of_float_vars = 19;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
const int32_t number_of_double_vars = 1;
const int32_t number_of_float_vars = 19;
const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
std::string dump_filename_ = d_dump_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;
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))
{
// 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;
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);
}
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;
// 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
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_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_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
if (d_secondary)
{
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 next_prn_length = d_current_integration_length_samples / d_fpga_integration_period;
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);
}
@ -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
if (d_secondary)
{
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 next_prn_length = d_current_integration_length_samples / d_fpga_integration_period;
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);
}

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 <pmt/pmt.h> // for pmt_t
#include <volk_gnsssdr/volk_gnsssdr_alloc.h> // for volk_gnsssdr::vector
#include <cstddef> // for size_t
#include <cstdint> // for int32_t
#include <fstream> // for string, ofstream
#include <memory> // for std::shared_ptr
#include <string>
#include <utility> // for pair
#include <string> // for string
#include <typeinfo> // for typeid
#include <utility> // for pair
#if GNURADIO_USES_STD_POINTERS
#else
#include <boost/shared_ptr.hpp>
@ -153,6 +155,8 @@ private:
boost::mutex d_mutex;
const size_t int_type_hash_code = typeid(int).hash_code();
double d_signal_carrier_freq;
double d_code_period;
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 <gnuradio/gr_complex.h>
#include <gnuradio/io_signature.h>
#include <cstddef>
#include <cstdint>
#include <typeinfo>
#include <utility>
#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
try
{
// ************* Gnss_Synchro received *****************
if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gnss_Synchro>))
const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
// ****************** Gnss_Synchro received ************************
if (msg_type_hash_code == typeid(std::shared_ptr<Gnss_Synchro>).hash_code())
{
std::shared_ptr<Gnss_Synchro> gnss_synchro_obj;
gnss_synchro_obj = boost::any_cast<std::shared_ptr<Gnss_Synchro>>(pmt::any_ref(msg));
const std::shared_ptr<Gnss_Synchro> gnss_synchro_obj = boost::any_cast<std::shared_ptr<Gnss_Synchro>>(pmt::any_ref(msg));
if (gnss_synchro_obj->Flag_valid_pseudorange == true)
{
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';
}
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 *****************
std::shared_ptr<Monitor_Pvt> monitor_pvt_obj;
monitor_pvt_obj = boost::any_cast<std::shared_ptr<Monitor_Pvt>>(pmt::any_ref(msg));
// ***************** Monitor_Pvt received ******************
const std::shared_ptr<Monitor_Pvt> monitor_pvt_obj = boost::any_cast<std::shared_ptr<Monitor_Pvt>>(pmt::any_ref(msg));
d_pvt_status = *monitor_pvt_obj.get();
// std::cout << "-------- \n" << '\n';

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

@ -26,8 +26,6 @@
#endif
#include "control_thread.h"
#include "channel_event.h"
#include "command_event.h"
#include "concurrent_map.h"
#include "configuration_interface.h"
#include "file_configuration.h"
@ -128,9 +126,9 @@ void ControlThread::init()
agnss_ref_location_ = Agnss_Ref_Location();
agnss_ref_time_ = Agnss_Ref_Time();
std::string empty_string = "";
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 empty_string;
const std::string ref_location_str = configuration_->property("GNSS-SDR.AGNSS_ref_location", empty_string);
const std::string ref_time_str = configuration_->property("GNSS-SDR.AGNSS_ref_utc_time", empty_string);
if (ref_location_str != empty_string)
{
std::vector<double> vect;
@ -171,7 +169,9 @@ void ControlThread::init()
else
{
// 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)
{
agnss_ref_time_.d_tv_sec = timegm(&tm);
@ -219,7 +219,7 @@ void ControlThread::telecommand_listener()
{
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);
}
}
@ -230,21 +230,20 @@ void ControlThread::event_dispatcher(bool &valid_event, pmt::pmt_t &msg)
if (valid_event)
{
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)
{
channel_event_sptr new_event;
new_event = boost::any_cast<channel_event_sptr>(pmt::any_ref(msg));
const channel_event_sptr 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
<< " what: " << 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;
new_event = boost::any_cast<command_event_sptr>(pmt::any_ref(msg));
const command_event_sptr 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
<< " what: " << new_event->event_type;
@ -445,7 +444,7 @@ bool ControlThread::read_assistance_from_XML()
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::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));
}
ret = true;
@ -453,7 +452,7 @@ bool ControlThread::read_assistance_from_XML()
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));
std::cout << "From XML file: Read GPS UTC model parameters.\n";
ret = true;
@ -461,7 +460,7 @@ bool ControlThread::read_assistance_from_XML()
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));
std::cout << "From XML file: Read GPS ionosphere model parameters.\n";
ret = true;
@ -475,7 +474,7 @@ bool ControlThread::read_assistance_from_XML()
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::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));
}
ret = true;
@ -492,7 +491,7 @@ bool ControlThread::read_assistance_from_XML()
gal_eph_iter++)
{
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));
}
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)
{
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));
std::cout << "From XML file: Read Galileo ionosphere model parameters.\n";
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)
{
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));
std::cout << "From XML file: Read Galileo UTC model parameters.\n";
ret = true;
@ -522,7 +521,7 @@ bool ControlThread::read_assistance_from_XML()
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::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));
}
ret = true;
@ -539,7 +538,7 @@ bool ControlThread::read_assistance_from_XML()
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::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));
}
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)
{
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));
std::cout << "From XML file: Read GPS CNAV UTC model parameters.\n";
ret = true;
@ -564,7 +563,7 @@ bool ControlThread::read_assistance_from_XML()
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::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));
}
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)
{
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));
std::cout << "From XML file: Read GLONASS UTC model parameters.\n";
ret = true;
@ -586,14 +585,14 @@ bool ControlThread::read_assistance_from_XML()
}
// 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)
{
// Try to read Ref Time from XML
if (supl_client_acquisition_.load_ref_time_xml(ref_time_xml_filename) == true)
{
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));
}
else
@ -605,7 +604,7 @@ bool ControlThread::read_assistance_from_XML()
if (supl_client_acquisition_.load_ref_location_xml(ref_location_xml_filename) == true)
{
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));
}
else
@ -622,13 +621,13 @@ void ControlThread::assist_GNSS()
{
// ######### GNSS Assistance #################################
// GNSS Assistance configuration
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_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_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))
{
std::cout << "SUPL RRLP GPS assistance enabled!\n";
std::string default_acq_server = "supl.google.com";
std::string default_eph_server = "supl.google.com";
const std::string default_acq_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_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);
@ -636,10 +635,10 @@ void ControlThread::assist_GNSS()
supl_mcc_ = configuration_->property("GNSS-SDR.SUPL_MCC", 244);
supl_mns_ = configuration_->property("GNSS-SDR.SUPL_MNC ", 5);
std::string default_lac = "0x59e2";
std::string default_ci = "0x31b0";
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 default_lac("0x59e2");
const std::string default_ci("0x31b0");
const std::string supl_lac_s = configuration_->property("GNSS-SDR.SUPL_LAC", default_lac);
const std::string supl_ci_s = configuration_->property("GNSS-SDR.SUPL_CI", default_ci);
try
{
supl_lac_ = std::stoi(supl_lac_s, nullptr, 0);
@ -669,7 +668,7 @@ void ControlThread::assist_GNSS()
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)
{
// Read assistance from file
@ -682,10 +681,9 @@ void ControlThread::assist_GNSS()
else
{
// Request ephemeris from SUPL server
int error;
supl_client_ephemeris_.request = 1;
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)
{
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));
}
// 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)
{
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::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)
{
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)
{
// 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::vec r_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_gal;
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];
tstruct = *gmtime(&rx_utc_time);
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
<< "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)
{
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 El;
double dist_m;
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 r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
const arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
// push sat
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)
{
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 El;
double dist_m;
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 r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
const arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
// push sat
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)
{
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 El;
double dist_m;
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 r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
const arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
// push sat
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)
{
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 El;
double dist_m;
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 r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
const arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
// push sat
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_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 "gnss_sdr_supl_client.h" // for Gnss_Sdr_Supl_Client
#include "tcp_cmd_interface.h" // for TcpCmdInterface
#include <pmt/pmt.h>
#include <array> // for array
#include <memory> // for shared_ptr
#include <string> // for string
#include <thread> // for std::thread
#include <utility> // for pair
#include <vector> // for vector
#include <array> // for array
#include <cstddef> // for size_t
#include <memory> // for shared_ptr
#include <string> // for string
#include <thread> // for std::thread
#include <typeinfo> // for std::type_info, typeid
#include <utility> // for pair
#include <vector> // for vector
#ifdef ENABLE_FPGA
#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 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<Concurrent_Queue<pmt::pmt_t>> control_queue_;
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)
{
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::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;
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);
// load ephemeris