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

Reorder PVT

Browse Source 
Add includes
Reorder CMake scripts
Put work at the end of the file
This commit is contained in:
Carles Fernandez
2019-02-26 01:38:48 +01:00
parent c3709567a2
commit 8e66577ee9
8 changed files with 304 additions and 302 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/PVT/adapters/CMakeLists.txt
View File

@@ -31,13 +31,11 @@ add_library(pvt_adapters ${PVT_ADAPTER_SOURCES} ${PVT_ADAPTER_HEADERS})
target_link_libraries(pvt_adapters target_link_libraries(pvt_adapters
PUBLIC PUBLIC
Armadillo::armadillo
Gnuradio::runtime
pvt_gr_blocks pvt_gr_blocks
pvt_libs
core_system_parameters core_system_parameters
PRIVATE PRIVATE
Boost::serialization gnss_sdr_flags
pvt_libs
) )
target_include_directories(pvt_adapters target_include_directories(pvt_adapters

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

@@ -33,16 +33,12 @@
#include "configuration_interface.h" #include "configuration_interface.h"
#include "gnss_sdr_flags.h" #include "gnss_sdr_flags.h"
#include "pvt_conf.h" #include "pvt_conf.h"
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/serialization/map.hpp>
#include <glog/logging.h> #include <glog/logging.h>
#if OLD_BOOST #if OLD_BOOST
#include <boost/math/common_factor_rt.hpp> #include <boost/math/common_factor_rt.hpp>
namespace bc = boost::math; namespace bc = boost::math;
#else #else
#include <boost/integer/common_factor_rt.hpp> #include <boost/integer/common_factor_rt.hpp>
#include <utility>
namespace bc = boost::integer; namespace bc = boost::integer;
#endif #endif
@@ -50,7 +46,7 @@ namespace bc = boost::integer;
using google::LogMessage; using google::LogMessage;
RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, Rtklib_Pvt::Rtklib_Pvt(ConfigurationInterface* configuration,
const std::string& role, const std::string& role,
unsigned int in_streams, unsigned int in_streams,
unsigned int out_streams) : role_(role), unsigned int out_streams) : role_(role),
@@ -724,13 +720,13 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
} }
RtklibPvt::~RtklibPvt() Rtklib_Pvt::~Rtklib_Pvt()
{ {
rtkfree(&rtk); rtkfree(&rtk);
} }
bool RtklibPvt::get_latest_PVT(double* longitude_deg, bool Rtklib_Pvt::get_latest_PVT(double* longitude_deg,
double* latitude_deg, double* latitude_deg,
double* height_m, double* height_m,
double* ground_speed_kmh, double* ground_speed_kmh,
@@ -746,37 +742,37 @@ bool RtklibPvt::get_latest_PVT(double* longitude_deg,
} }
void RtklibPvt::clear_ephemeris() void Rtklib_Pvt::clear_ephemeris()
{ {
pvt_->clear_ephemeris(); pvt_->clear_ephemeris();
} }
std::map<int, Gps_Ephemeris> RtklibPvt::get_gps_ephemeris() const std::map<int, Gps_Ephemeris> Rtklib_Pvt::get_gps_ephemeris() const
{ {
return pvt_->get_gps_ephemeris_map(); return pvt_->get_gps_ephemeris_map();
} }
std::map<int, Galileo_Ephemeris> RtklibPvt::get_galileo_ephemeris() const std::map<int, Galileo_Ephemeris> Rtklib_Pvt::get_galileo_ephemeris() const
{ {
return pvt_->get_galileo_ephemeris_map(); return pvt_->get_galileo_ephemeris_map();
} }
std::map<int, Gps_Almanac> RtklibPvt::get_gps_almanac() const std::map<int, Gps_Almanac> Rtklib_Pvt::get_gps_almanac() const
{ {
return pvt_->get_gps_almanac_map(); return pvt_->get_gps_almanac_map();
} }
std::map<int, Galileo_Almanac> RtklibPvt::get_galileo_almanac() const std::map<int, Galileo_Almanac> Rtklib_Pvt::get_galileo_almanac() const
{ {
return pvt_->get_galileo_almanac_map(); return pvt_->get_galileo_almanac_map();
} }
void RtklibPvt::connect(gr::top_block_sptr top_block) void Rtklib_Pvt::connect(gr::top_block_sptr top_block)
{ {
if (top_block) if (top_block)
{ /* top_block is not null */ { /* top_block is not null */
@@ -786,7 +782,7 @@ void RtklibPvt::connect(gr::top_block_sptr top_block)
} }
void RtklibPvt::disconnect(gr::top_block_sptr top_block) void Rtklib_Pvt::disconnect(gr::top_block_sptr top_block)
{ {
if (top_block) if (top_block)
{ /* top_block is not null */ { /* top_block is not null */
@@ -795,13 +791,13 @@ void RtklibPvt::disconnect(gr::top_block_sptr top_block)
} }
gr::basic_block_sptr RtklibPvt::get_left_block() gr::basic_block_sptr Rtklib_Pvt::get_left_block()
{ {
return pvt_; return pvt_;
} }
gr::basic_block_sptr RtklibPvt::get_right_block() gr::basic_block_sptr Rtklib_Pvt::get_right_block()
{ {
return nullptr; // this is a sink, nothing downstream return nullptr; // this is a sink, nothing downstream
} }

12
src/algorithms/PVT/adapters/rtklib_pvt.h
View File

@@ -32,8 +32,14 @@
#ifndef GNSS_SDR_RTKLIB_PVT_H_ #ifndef GNSS_SDR_RTKLIB_PVT_H_
#define GNSS_SDR_RTKLIB_PVT_H_ #define GNSS_SDR_RTKLIB_PVT_H_
#include "galileo_almanac.h"
#include "galileo_ephemeris.h"
#include "gps_almanac.h"
#include "gps_ephemeris.h"
#include "pvt_interface.h" #include "pvt_interface.h"
#include "rtklib.h"
#include "rtklib_pvt_cc.h" #include "rtklib_pvt_cc.h"
#include <map>
#include <string> #include <string>
@@ -42,15 +48,15 @@ class ConfigurationInterface;
/*! /*!
* \brief This class implements a PvtInterface for the RTKLIB PVT block * \brief This class implements a PvtInterface for the RTKLIB PVT block
*/ */
class RtklibPvt : public PvtInterface class Rtklib_Pvt : public PvtInterface
{ {
public: public:
RtklibPvt(ConfigurationInterface* configuration, Rtklib_Pvt(ConfigurationInterface* configuration,
const std::string& role, const std::string& role,
unsigned int in_streams, unsigned int in_streams,
unsigned int out_streams); unsigned int out_streams);
virtual ~RtklibPvt(); virtual ~Rtklib_Pvt();
inline std::string role() override inline std::string role() override
{ {

15
src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt
View File

@@ -30,15 +30,18 @@ add_library(pvt_gr_blocks ${PVT_GR_BLOCKS_SOURCES} ${PVT_GR_BLOCKS_HEADERS})
target_link_libraries(pvt_gr_blocks target_link_libraries(pvt_gr_blocks
PUBLIC PUBLIC
Boost::date_time
Gnuradio::runtime
pvt_libs pvt_libs
core_system_parameters
Boost::date_time
Gnuradio::pmt
Gnuradio::runtime
PRIVATE PRIVATE
algorithms_libs
Gflags::gflags Gflags::gflags
Glog::glog Glog::glog
Boost::filesystem Boost::filesystem
Boost::system Boost::system
core_system_parameters Boost::serialization
) )
if(ENABLE_CLANG_TIDY) if(ENABLE_CLANG_TIDY)
@@ -50,11 +53,6 @@ if(ENABLE_CLANG_TIDY)
endif() endif()
endif() endif()
target_include_directories(pvt_gr_blocks
PUBLIC
${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs
)
if(Boost_VERSION LESS 105800) if(Boost_VERSION LESS 105800)
target_compile_definitions(pvt_gr_blocks PRIVATE -DOLD_BOOST=1) target_compile_definitions(pvt_gr_blocks PRIVATE -DOLD_BOOST=1)
endif() endif()
@@ -62,5 +60,4 @@ endif()
set_property(TARGET pvt_gr_blocks set_property(TARGET pvt_gr_blocks
APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}> $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/src/core/system_parameters>
) )

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

@@ -30,7 +30,6 @@
#include "rtklib_pvt_cc.h" #include "rtklib_pvt_cc.h"
#include "display.h" #include "display.h"
#include "galileo_almanac.h"
#include "galileo_almanac_helper.h" #include "galileo_almanac_helper.h"
#include "gnss_sdr_create_directory.h" #include "gnss_sdr_create_directory.h"
#include "pvt_conf.h" #include "pvt_conf.h"
@@ -44,8 +43,8 @@
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <algorithm> #include <algorithm>
#include <exception> #include <exception>
#include <fstream>
#include <iostream> #include <iostream>
#include <map>
#include <stdexcept> #include <stdexcept>
#if OLD_BOOST #if OLD_BOOST
#include <boost/math/common_factor_rt.hpp> #include <boost/math/common_factor_rt.hpp>
@@ -68,267 +67,6 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t nchannels,
} }
void rtklib_pvt_cc::msg_handler_telemetry(const pmt::pmt_t& msg)
{
try
{
// ************* GPS telemetry *****************
if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Ephemeris>))
{
// ### GPS EPHEMERIS ###
std::shared_ptr<Gps_Ephemeris> gps_eph;
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] << ")"
<< "inserted with Toe=" << gps_eph->d_Toe << " and GPS Week="
<< gps_eph->i_GPS_week;
// update/insert new ephemeris record to the global ephemeris map
d_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>))
{
// ### GPS IONO ###
std::shared_ptr<Gps_Iono> gps_iono;
gps_iono = boost::any_cast<std::shared_ptr<Gps_Iono>>(pmt::any_ref(msg));
d_pvt_solver->gps_iono = *gps_iono;
DLOG(INFO) << "New IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Utc_Model>))
{
// ### 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));
d_pvt_solver->gps_utc_model = *gps_utc_model;
DLOG(INFO) << "New UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Ephemeris>))
{
// ### 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));
// update/insert new ephemeris record to the global ephemeris map
d_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris;
DLOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>))
{
// ### 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));
d_pvt_solver->gps_cnav_iono = *gps_cnav_iono;
DLOG(INFO) << "New CNAV IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Utc_Model>))
{
// ### 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));
d_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model;
DLOG(INFO) << "New CNAV UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Almanac>))
{
// ### GPS ALMANAC ###
std::shared_ptr<Gps_Almanac> gps_almanac;
gps_almanac = boost::any_cast<std::shared_ptr<Gps_Almanac>>(pmt::any_ref(msg));
d_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac;
DLOG(INFO) << "New GPS almanac record has arrived ";
}
// **************** Galileo telemetry ********************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Ephemeris>))
{
// ### Galileo EPHEMERIS ###
std::shared_ptr<Galileo_Ephemeris> galileo_eph;
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
<< " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris;
// update/insert new ephemeris record to the global ephemeris map
d_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>))
{
// ### Galileo IONO ###
std::shared_ptr<Galileo_Iono> galileo_iono;
galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
d_pvt_solver->galileo_iono = *galileo_iono;
DLOG(INFO) << "New IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Utc_Model>))
{
// ### 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));
d_pvt_solver->galileo_utc_model = *galileo_utc_model;
DLOG(INFO) << "New UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac_Helper>))
{
// ### 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));
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);
if (sv1.i_satellite_PRN != 0)
{
d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1;
}
if (sv2.i_satellite_PRN != 0)
{
d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2;
}
if (sv3.i_satellite_PRN != 0)
{
d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3;
}
DLOG(INFO) << "New Galileo Almanac data have arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac>))
{
// ### Galileo Almanac ###
std::shared_ptr<Galileo_Almanac> galileo_alm;
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_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm;
}
// **************** GLONASS GNAV Telemetry **************************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>))
{
// ### 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));
// 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
<< ", Week Number =" << glonass_gnav_eph->d_WN
<< " and Ephemeris IOD in UTC = " << glonass_gnav_eph->compute_GLONASS_time(glonass_gnav_eph->d_t_b)
<< " from SV = " << glonass_gnav_eph->i_satellite_slot_number;
// update/insert new ephemeris record to the global ephemeris map
d_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>))
{
// ### 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));
d_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model;
DLOG(INFO) << "New GLONASS GNAV UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Almanac>))
{
// ### 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));
d_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac;
DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "
<< ", 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 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));
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] << ")"
<< "inserted with Toe=" << bds_dnav_eph->d_Toe << " and BDS Week="
<< bds_dnav_eph->i_BEIDOU_week;
// update/insert new ephemeris record to the global ephemeris map
d_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>))
{
// ### 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));
d_pvt_solver->beidou_dnav_iono = *bds_dnav_iono;
DLOG(INFO) << "New BeiDou DNAV IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Utc_Model>))
{
// ### GPS 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));
d_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model;
DLOG(INFO) << "New BeiDou DNAV UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Almanac>))
{
// ### GPS 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));
d_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac;
DLOG(INFO) << "New BeiDou DNAV almanac record has arrived ";
}
else
{
LOG(WARNING) << "msg_handler_telemetry unknown object type!";
}
}
catch (boost::bad_any_cast& e)
{
LOG(WARNING) << "msg_handler_telemetry Bad any cast!";
}
}
std::map<int, Gps_Ephemeris> rtklib_pvt_cc::get_gps_ephemeris_map() const
{
return d_pvt_solver->gps_ephemeris_map;
}
std::map<int, Gps_Almanac> rtklib_pvt_cc::get_gps_almanac_map() const
{
return d_pvt_solver->gps_almanac_map;
}
std::map<int, Galileo_Ephemeris> rtklib_pvt_cc::get_galileo_ephemeris_map() const
{
return d_pvt_solver->galileo_ephemeris_map;
}
std::map<int, Galileo_Almanac> rtklib_pvt_cc::get_galileo_almanac_map() const
{
return d_pvt_solver->galileo_almanac_map;
}
std::map<int, Beidou_Dnav_Ephemeris> rtklib_pvt_cc::get_beidou_dnav_ephemeris_map() const
{
return d_pvt_solver->beidou_dnav_ephemeris_map;
}
std::map<int, Beidou_Dnav_Almanac> rtklib_pvt_cc::get_beidou_dnav_almanac_map() const
{
return d_pvt_solver->beidou_dnav_almanac_map;
}
void rtklib_pvt_cc::clear_ephemeris()
{
d_pvt_solver->gps_ephemeris_map.clear();
d_pvt_solver->gps_almanac_map.clear();
d_pvt_solver->galileo_ephemeris_map.clear();
d_pvt_solver->galileo_almanac_map.clear();
d_pvt_solver->beidou_dnav_ephemeris_map.clear();
d_pvt_solver->beidou_dnav_almanac_map.clear();
}
rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels,
const Pvt_Conf& conf_, const Pvt_Conf& conf_,
const rtk_t& rtk) : gr::sync_block("rtklib_pvt_cc", const rtk_t& rtk) : gr::sync_block("rtklib_pvt_cc",
@@ -563,9 +301,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels,
xml_base_path = xml_base_path + boost::filesystem::path::preferred_separator; xml_base_path = xml_base_path + boost::filesystem::path::preferred_separator;
} }
d_rx_time = 0.0; d_rx_time = 0.0;
d_last_status_print_seg = 0; d_last_status_print_seg = 0;
// PVT MONITOR // PVT MONITOR
@@ -1147,6 +883,267 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
} }
void rtklib_pvt_cc::msg_handler_telemetry(const pmt::pmt_t& msg)
{
try
{
// ************* GPS telemetry *****************
if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Ephemeris>))
{
// ### GPS EPHEMERIS ###
std::shared_ptr<Gps_Ephemeris> gps_eph;
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] << ")"
<< "inserted with Toe=" << gps_eph->d_Toe << " and GPS Week="
<< gps_eph->i_GPS_week;
// update/insert new ephemeris record to the global ephemeris map
d_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>))
{
// ### GPS IONO ###
std::shared_ptr<Gps_Iono> gps_iono;
gps_iono = boost::any_cast<std::shared_ptr<Gps_Iono>>(pmt::any_ref(msg));
d_pvt_solver->gps_iono = *gps_iono;
DLOG(INFO) << "New IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Utc_Model>))
{
// ### 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));
d_pvt_solver->gps_utc_model = *gps_utc_model;
DLOG(INFO) << "New UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Ephemeris>))
{
// ### 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));
// update/insert new ephemeris record to the global ephemeris map
d_pvt_solver->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris;
DLOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>))
{
// ### 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));
d_pvt_solver->gps_cnav_iono = *gps_cnav_iono;
DLOG(INFO) << "New CNAV IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Utc_Model>))
{
// ### 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));
d_pvt_solver->gps_cnav_utc_model = *gps_cnav_utc_model;
DLOG(INFO) << "New CNAV UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_Almanac>))
{
// ### GPS ALMANAC ###
std::shared_ptr<Gps_Almanac> gps_almanac;
gps_almanac = boost::any_cast<std::shared_ptr<Gps_Almanac>>(pmt::any_ref(msg));
d_pvt_solver->gps_almanac_map[gps_almanac->i_satellite_PRN] = *gps_almanac;
DLOG(INFO) << "New GPS almanac record has arrived ";
}
// **************** Galileo telemetry ********************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Ephemeris>))
{
// ### Galileo EPHEMERIS ###
std::shared_ptr<Galileo_Ephemeris> galileo_eph;
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
<< " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris;
// update/insert new ephemeris record to the global ephemeris map
d_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>))
{
// ### Galileo IONO ###
std::shared_ptr<Galileo_Iono> galileo_iono;
galileo_iono = boost::any_cast<std::shared_ptr<Galileo_Iono>>(pmt::any_ref(msg));
d_pvt_solver->galileo_iono = *galileo_iono;
DLOG(INFO) << "New IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Utc_Model>))
{
// ### 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));
d_pvt_solver->galileo_utc_model = *galileo_utc_model;
DLOG(INFO) << "New UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac_Helper>))
{
// ### 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));
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);
if (sv1.i_satellite_PRN != 0)
{
d_pvt_solver->galileo_almanac_map[sv1.i_satellite_PRN] = sv1;
}
if (sv2.i_satellite_PRN != 0)
{
d_pvt_solver->galileo_almanac_map[sv2.i_satellite_PRN] = sv2;
}
if (sv3.i_satellite_PRN != 0)
{
d_pvt_solver->galileo_almanac_map[sv3.i_satellite_PRN] = sv3;
}
DLOG(INFO) << "New Galileo Almanac data have arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Galileo_Almanac>))
{
// ### Galileo Almanac ###
std::shared_ptr<Galileo_Almanac> galileo_alm;
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_pvt_solver->galileo_almanac_map[galileo_alm->i_satellite_PRN] = *galileo_alm;
}
// **************** GLONASS GNAV Telemetry **************************
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Ephemeris>))
{
// ### 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));
// 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
<< ", Week Number =" << glonass_gnav_eph->d_WN
<< " and Ephemeris IOD in UTC = " << glonass_gnav_eph->compute_GLONASS_time(glonass_gnav_eph->d_t_b)
<< " from SV = " << glonass_gnav_eph->i_satellite_slot_number;
// update/insert new ephemeris record to the global ephemeris map
d_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>))
{
// ### 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));
d_pvt_solver->glonass_gnav_utc_model = *glonass_gnav_utc_model;
DLOG(INFO) << "New GLONASS GNAV UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Glonass_Gnav_Almanac>))
{
// ### 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));
d_pvt_solver->glonass_gnav_almanac = *glonass_gnav_almanac;
DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "
<< ", 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 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));
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] << ")"
<< "inserted with Toe=" << bds_dnav_eph->d_Toe << " and BDS Week="
<< bds_dnav_eph->i_BEIDOU_week;
// update/insert new ephemeris record to the global ephemeris map
d_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>))
{
// ### 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));
d_pvt_solver->beidou_dnav_iono = *bds_dnav_iono;
DLOG(INFO) << "New BeiDou DNAV IONO record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Utc_Model>))
{
// ### 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));
d_pvt_solver->beidou_dnav_utc_model = *bds_dnav_utc_model;
DLOG(INFO) << "New BeiDou DNAV UTC record has arrived ";
}
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Beidou_Dnav_Almanac>))
{
// ### 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));
d_pvt_solver->beidou_dnav_almanac_map[bds_dnav_almanac->i_satellite_PRN] = *bds_dnav_almanac;
DLOG(INFO) << "New BeiDou DNAV almanac record has arrived ";
}
else
{
LOG(WARNING) << "msg_handler_telemetry unknown object type!";
}
}
catch (boost::bad_any_cast& e)
{
LOG(WARNING) << "msg_handler_telemetry Bad any cast!";
}
}
std::map<int, Gps_Ephemeris> rtklib_pvt_cc::get_gps_ephemeris_map() const
{
return d_pvt_solver->gps_ephemeris_map;
}
std::map<int, Gps_Almanac> rtklib_pvt_cc::get_gps_almanac_map() const
{
return d_pvt_solver->gps_almanac_map;
}
std::map<int, Galileo_Ephemeris> rtklib_pvt_cc::get_galileo_ephemeris_map() const
{
return d_pvt_solver->galileo_ephemeris_map;
}
std::map<int, Galileo_Almanac> rtklib_pvt_cc::get_galileo_almanac_map() const
{
return d_pvt_solver->galileo_almanac_map;
}
std::map<int, Beidou_Dnav_Ephemeris> rtklib_pvt_cc::get_beidou_dnav_ephemeris_map() const
{
return d_pvt_solver->beidou_dnav_ephemeris_map;
}
std::map<int, Beidou_Dnav_Almanac> rtklib_pvt_cc::get_beidou_dnav_almanac_map() const
{
return d_pvt_solver->beidou_dnav_almanac_map;
}
void rtklib_pvt_cc::clear_ephemeris()
{
d_pvt_solver->gps_ephemeris_map.clear();
d_pvt_solver->gps_almanac_map.clear();
d_pvt_solver->galileo_ephemeris_map.clear();
d_pvt_solver->galileo_almanac_map.clear();
d_pvt_solver->beidou_dnav_ephemeris_map.clear();
d_pvt_solver->beidou_dnav_almanac_map.clear();
}
bool rtklib_pvt_cc::observables_pairCompare_min(const std::pair<int, Gnss_Synchro>& a, const std::pair<int, Gnss_Synchro>& b) bool rtklib_pvt_cc::observables_pairCompare_min(const std::pair<int, Gnss_Synchro>& a, const std::pair<int, Gnss_Synchro>& b)
{ {
return (a.second.Pseudorange_m) < (b.second.Pseudorange_m); return (a.second.Pseudorange_m) < (b.second.Pseudorange_m);
@@ -1215,7 +1212,7 @@ bool rtklib_pvt_cc::load_gnss_synchro_map_xml(const std::string& file_name)
} }
std::vector<std::string> rtklib_pvt_cc::split_string(const std::string& s, char delim) std::vector<std::string> rtklib_pvt_cc::split_string(const std::string& s, char delim) const
{ {
std::vector<std::string> v; std::vector<std::string> v;
std::stringstream ss(s); std::stringstream ss(s);

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

@@ -31,7 +31,13 @@
#ifndef GNSS_SDR_RTKLIB_PVT_CC_H #ifndef GNSS_SDR_RTKLIB_PVT_CC_H
#define GNSS_SDR_RTKLIB_PVT_CC_H #define GNSS_SDR_RTKLIB_PVT_CC_H
#include "beidou_dnav_almanac.h"
#include "beidou_dnav_ephemeris.h"
#include "galileo_almanac.h"
#include "galileo_ephemeris.h"
#include "geojson_printer.h" #include "geojson_printer.h"
#include "gnss_synchro.h"
#include "gps_almanac.h"
#include "gps_ephemeris.h" #include "gps_ephemeris.h"
#include "gpx_printer.h" #include "gpx_printer.h"
#include "kml_printer.h" #include "kml_printer.h"
@@ -44,14 +50,17 @@
#include <boost/date_time/gregorian/gregorian.hpp> #include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp> #include <boost/date_time/posix_time/posix_time.hpp>
#include <gnuradio/sync_block.h> #include <gnuradio/sync_block.h>
#include <pmt/pmt.h>
#include <chrono> #include <chrono>
#include <cstdint> #include <cstdint>
#include <fstream> #include <map>
#include <memory>
#include <string> #include <string>
#include <sys/ipc.h> #include <sys/ipc.h>
#include <sys/msg.h> #include <sys/msg.h>
#include <sys/types.h> #include <sys/types.h>
#include <utility> #include <utility>
#include <vector>
class rtklib_pvt_cc; class rtklib_pvt_cc;
@@ -146,7 +155,7 @@ private:
bool flag_monitor_pvt_enabled; bool flag_monitor_pvt_enabled;
std::unique_ptr<Monitor_Pvt_Udp_Sink> udp_sink_ptr; std::unique_ptr<Monitor_Pvt_Udp_Sink> udp_sink_ptr;
std::vector<std::string> split_string(const std::string& s, char delim); std::vector<std::string> split_string(const std::string& s, char delim) const;
public: public:
rtklib_pvt_cc(uint32_t nchannels, rtklib_pvt_cc(uint32_t nchannels,

1
src/algorithms/libs/rtklib/rtklib.h
View File

@@ -60,7 +60,6 @@
#include <cmath> #include <cmath>
#include <cstdarg> #include <cstdarg>
#include <cstdint> #include <cstdint>
#include <cstdio>
#include <cstdlib> #include <cstdlib>
#include <netinet/in.h> #include <netinet/in.h>
#include <pthread.h> #include <pthread.h>

2
src/core/receiver/gnss_block_factory.cc
View File

@@ -1802,7 +1802,7 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
// PVT ------------------------------------------------------------------------- // PVT -------------------------------------------------------------------------
else if ((implementation == "RTKLIB_PVT") || (implementation == "GPS_L1_CA_PVT") || (implementation == "Galileo_E1_PVT") || (implementation == "Hybrid_PVT")) else if ((implementation == "RTKLIB_PVT") || (implementation == "GPS_L1_CA_PVT") || (implementation == "Galileo_E1_PVT") || (implementation == "Hybrid_PVT"))
{ {
std::unique_ptr<GNSSBlockInterface> block_(new RtklibPvt(configuration.get(), role, in_streams, std::unique_ptr<GNSSBlockInterface> block_(new Rtklib_Pvt(configuration.get(), role, in_streams,
out_streams)); out_streams));
block = std::move(block_); block = std::move(block_);
} }