mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 20:20:35 +00:00
Add work on getting TOW for E6 channels
This commit is contained in:
parent
8aeb4bf3ea
commit
1ae6cac249
@ -107,6 +107,13 @@ else()
|
|||||||
)
|
)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
if(PMT_USES_BOOST_ANY)
|
||||||
|
target_compile_definitions(telemetry_decoder_gr_blocks
|
||||||
|
PRIVATE
|
||||||
|
-DPMT_USES_BOOST_ANY=1
|
||||||
|
)
|
||||||
|
endif()
|
||||||
|
|
||||||
if(ENABLE_CLANG_TIDY)
|
if(ENABLE_CLANG_TIDY)
|
||||||
if(CLANG_TIDY_EXE)
|
if(CLANG_TIDY_EXE)
|
||||||
set_target_properties(telemetry_decoder_gr_blocks
|
set_target_properties(telemetry_decoder_gr_blocks
|
||||||
|
@ -36,17 +36,32 @@
|
|||||||
#include "viterbi_decoder.h" // for Viterbi_Decoder
|
#include "viterbi_decoder.h" // for Viterbi_Decoder
|
||||||
#include <glog/logging.h> // for LOG, DLOG
|
#include <glog/logging.h> // for LOG, DLOG
|
||||||
#include <gnuradio/io_signature.h> // for gr::io_signature::make
|
#include <gnuradio/io_signature.h> // for gr::io_signature::make
|
||||||
#include <pmt/pmt.h> // for pmt::make_any
|
|
||||||
#include <pmt/pmt_sugar.h> // for pmt::mp
|
#include <pmt/pmt_sugar.h> // for pmt::mp
|
||||||
#include <array> // for std::array
|
#include <array> // for std::array
|
||||||
#include <cmath> // for std::fmod, std::abs
|
#include <cmath> // for std::fmod, std::abs
|
||||||
#include <cstddef> // for size_t
|
#include <cstddef> // for size_t
|
||||||
#include <exception> // for std::exception
|
#include <exception> // for std::exception
|
||||||
#include <iostream> // for std::cout
|
#include <iostream> // for std::cout
|
||||||
|
#include <map> // for std::map
|
||||||
|
#include <stdexcept> // for std::out_of_range
|
||||||
|
#include <typeinfo> // for typeid
|
||||||
|
#include <utility> // for std::pair
|
||||||
|
|
||||||
|
#if HAS_GENERIC_LAMBDA
|
||||||
|
#else
|
||||||
|
#include <boost/bind/bind.hpp>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if PMT_USES_BOOST_ANY
|
||||||
|
#include <boost/any.hpp>
|
||||||
|
namespace wht = boost;
|
||||||
|
#else
|
||||||
|
#include <any>
|
||||||
|
namespace wht = std;
|
||||||
|
#endif
|
||||||
|
|
||||||
#define CRC_ERROR_LIMIT 6
|
#define CRC_ERROR_LIMIT 6
|
||||||
|
|
||||||
|
|
||||||
galileo_telemetry_decoder_gs_sptr
|
galileo_telemetry_decoder_gs_sptr
|
||||||
galileo_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, const Tlm_Conf &conf, int frame_type)
|
galileo_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, const Tlm_Conf &conf, int frame_type)
|
||||||
{
|
{
|
||||||
@ -96,6 +111,8 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
|
|||||||
this->message_port_register_out(pmt::mp("telemetry_to_trk"));
|
this->message_port_register_out(pmt::mp("telemetry_to_trk"));
|
||||||
// register Gal E6 messages HAS out
|
// register Gal E6 messages HAS out
|
||||||
this->message_port_register_out(pmt::mp("E6_HAS_from_TLM"));
|
this->message_port_register_out(pmt::mp("E6_HAS_from_TLM"));
|
||||||
|
// register TOW from map out
|
||||||
|
this->message_port_register_out(pmt::mp("TOW_from_TLM"));
|
||||||
|
|
||||||
if (d_enable_navdata_monitor)
|
if (d_enable_navdata_monitor)
|
||||||
{
|
{
|
||||||
@ -103,6 +120,20 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
|
|||||||
this->message_port_register_out(pmt::mp("Nav_msg_from_TLM"));
|
this->message_port_register_out(pmt::mp("Nav_msg_from_TLM"));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// register TOW to TLM input
|
||||||
|
this->message_port_register_in(pmt::mp("TOW_to_TLM"));
|
||||||
|
// handler for input port
|
||||||
|
this->set_msg_handler(pmt::mp("TOW_to_TLM"),
|
||||||
|
#if HAS_GENERIC_LAMBDA
|
||||||
|
[this](auto &&PH1) { msg_handler_read_galileo_tow_map(PH1); });
|
||||||
|
#else
|
||||||
|
#if USE_BOOST_BIND_PLACEHOLDERS
|
||||||
|
boost::bind(&galileo_telemetry_decoder_gs::msg_handler_read_galileo_tow_map, this, boost::placeholders::_1));
|
||||||
|
#else
|
||||||
|
boost::bind(&galileo_telemetry_decoder_gs::msg_handler_read_galileo_tow_map, this, _1));
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||||
|
|
||||||
// Viterbi decoder vars
|
// Viterbi decoder vars
|
||||||
@ -281,6 +312,34 @@ galileo_telemetry_decoder_gs::~galileo_telemetry_decoder_gs()
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void galileo_telemetry_decoder_gs::msg_handler_read_galileo_tow_map(const pmt::pmt_t &msg)
|
||||||
|
{
|
||||||
|
if (d_frame_type == 3)
|
||||||
|
{
|
||||||
|
// Check if the input has the right format
|
||||||
|
try
|
||||||
|
{
|
||||||
|
const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
|
||||||
|
if (msg_type_hash_code == typeid(std::shared_ptr<std::map<uint32_t, std::pair<uint32_t, uint64_t>>>).hash_code())
|
||||||
|
{
|
||||||
|
const auto received_tow_map = wht::any_cast<std::shared_ptr<std::map<uint32_t, std::pair<uint32_t, uint64_t>>>>(pmt::any_ref(msg));
|
||||||
|
const std::pair<uint32_t, uint64_t> received_tow_sample = received_tow_map->at(d_satellite.get_PRN());
|
||||||
|
std::cout << "Received TOW: " << received_tow_sample.first << '\n';
|
||||||
|
std::cout << "Received sample counter: " << received_tow_sample.second << '\n';
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (const wht::bad_any_cast &e)
|
||||||
|
{
|
||||||
|
LOG(WARNING) << "msg_handler_read_galileo_tow_map Bad any_cast: " << e.what();
|
||||||
|
}
|
||||||
|
catch (const std::out_of_range &oor)
|
||||||
|
{
|
||||||
|
LOG(WARNING) << "msg_handler_read_galileo_tow_map Out of Range error: " << oor.what();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
void galileo_telemetry_decoder_gs::deinterleaver(int32_t rows, int32_t cols, const float *in, float *out)
|
void galileo_telemetry_decoder_gs::deinterleaver(int32_t rows, int32_t cols, const float *in, float *out)
|
||||||
{
|
{
|
||||||
for (int32_t r = 0; r < rows; r++)
|
for (int32_t r = 0; r < rows; r++)
|
||||||
@ -904,6 +963,9 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
|
|||||||
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW5() * 1000.0);
|
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW5() * 1000.0);
|
||||||
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
|
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
|
||||||
d_inav_nav.set_TOW5_flag(false);
|
d_inav_nav.set_TOW5_flag(false);
|
||||||
|
const std::pair<uint32_t, uint64_t> tow_and_sample{d_inav_nav.get_TOW5(), current_symbol.Tracking_sample_counter};
|
||||||
|
const auto tmp_obj = std::make_shared<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>(d_satellite.get_PRN(), tow_and_sample);
|
||||||
|
this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj));
|
||||||
// timetag debug
|
// timetag debug
|
||||||
if (d_valid_timetag == true)
|
if (d_valid_timetag == true)
|
||||||
{
|
{
|
||||||
@ -924,6 +986,9 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
|
|||||||
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW6() * 1000.0);
|
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW6() * 1000.0);
|
||||||
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
|
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
|
||||||
d_inav_nav.set_TOW6_flag(false);
|
d_inav_nav.set_TOW6_flag(false);
|
||||||
|
const std::pair<uint32_t, uint64_t> tow_and_sample{d_inav_nav.get_TOW6(), current_symbol.Tracking_sample_counter};
|
||||||
|
const auto tmp_obj = std::make_shared<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>(d_satellite.get_PRN(), tow_and_sample);
|
||||||
|
this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj));
|
||||||
// timetag debug
|
// timetag debug
|
||||||
if (d_valid_timetag == true)
|
if (d_valid_timetag == true)
|
||||||
{
|
{
|
||||||
@ -943,6 +1008,9 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
|
|||||||
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW0() * 1000.0);
|
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW0() * 1000.0);
|
||||||
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
|
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms);
|
||||||
d_inav_nav.set_TOW0_flag(false);
|
d_inav_nav.set_TOW0_flag(false);
|
||||||
|
const std::pair<uint32_t, uint64_t> tow_and_sample{d_inav_nav.get_TOW0(), current_symbol.Tracking_sample_counter};
|
||||||
|
const auto tmp_obj = std::make_shared<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>(d_satellite.get_PRN(), tow_and_sample);
|
||||||
|
this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj));
|
||||||
// timetag debug
|
// timetag debug
|
||||||
if (d_valid_timetag == true)
|
if (d_valid_timetag == true)
|
||||||
{
|
{
|
||||||
@ -984,6 +1052,9 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
|
|||||||
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
|
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
|
||||||
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
|
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
|
||||||
d_fnav_nav.set_TOW1_flag(false);
|
d_fnav_nav.set_TOW1_flag(false);
|
||||||
|
const std::pair<uint32_t, uint64_t> tow_and_sample{d_fnav_nav.get_TOW1(), current_symbol.Tracking_sample_counter};
|
||||||
|
const auto tmp_obj = std::make_shared<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>(d_satellite.get_PRN(), tow_and_sample);
|
||||||
|
this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj));
|
||||||
}
|
}
|
||||||
else if (d_fnav_nav.is_TOW2_set() == true)
|
else if (d_fnav_nav.is_TOW2_set() == true)
|
||||||
{
|
{
|
||||||
@ -991,6 +1062,9 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
|
|||||||
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
|
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
|
||||||
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
|
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
|
||||||
d_fnav_nav.set_TOW2_flag(false);
|
d_fnav_nav.set_TOW2_flag(false);
|
||||||
|
const std::pair<uint32_t, uint64_t> tow_and_sample{d_fnav_nav.get_TOW2(), current_symbol.Tracking_sample_counter};
|
||||||
|
const auto tmp_obj = std::make_shared<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>(d_satellite.get_PRN(), tow_and_sample);
|
||||||
|
this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj));
|
||||||
}
|
}
|
||||||
else if (d_fnav_nav.is_TOW3_set() == true)
|
else if (d_fnav_nav.is_TOW3_set() == true)
|
||||||
{
|
{
|
||||||
@ -998,6 +1072,9 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
|
|||||||
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
|
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
|
||||||
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
|
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
|
||||||
d_fnav_nav.set_TOW3_flag(false);
|
d_fnav_nav.set_TOW3_flag(false);
|
||||||
|
const std::pair<uint32_t, uint64_t> tow_and_sample{d_fnav_nav.get_TOW3(), current_symbol.Tracking_sample_counter};
|
||||||
|
const auto tmp_obj = std::make_shared<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>(d_satellite.get_PRN(), tow_and_sample);
|
||||||
|
this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj));
|
||||||
}
|
}
|
||||||
else if (d_fnav_nav.is_TOW4_set() == true)
|
else if (d_fnav_nav.is_TOW4_set() == true)
|
||||||
{
|
{
|
||||||
@ -1005,6 +1082,9 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
|
|||||||
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
|
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
|
||||||
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
|
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
|
||||||
d_fnav_nav.set_TOW4_flag(false);
|
d_fnav_nav.set_TOW4_flag(false);
|
||||||
|
const std::pair<uint32_t, uint64_t> tow_and_sample{d_fnav_nav.get_TOW4(), current_symbol.Tracking_sample_counter};
|
||||||
|
const auto tmp_obj = std::make_shared<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>(d_satellite.get_PRN(), tow_and_sample);
|
||||||
|
this->message_port_pub(pmt::mp("TOW_from_TLM"), pmt::make_any(tmp_obj));
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
@ -32,6 +32,7 @@
|
|||||||
#include <boost/circular_buffer.hpp> // for boost::circular_buffer
|
#include <boost/circular_buffer.hpp> // for boost::circular_buffer
|
||||||
#include <gnuradio/block.h> // for block
|
#include <gnuradio/block.h> // for block
|
||||||
#include <gnuradio/types.h> // for gr_vector_const_void_star
|
#include <gnuradio/types.h> // for gr_vector_const_void_star
|
||||||
|
#include <pmt/pmt.h> // for pmt::pmt_t
|
||||||
#include <cstdint> // for int32_t, uint32_t
|
#include <cstdint> // for int32_t, uint32_t
|
||||||
#include <fstream> // for std::ofstream
|
#include <fstream> // for std::ofstream
|
||||||
#include <memory> // for std::unique_ptr
|
#include <memory> // for std::unique_ptr
|
||||||
@ -80,6 +81,7 @@ private:
|
|||||||
|
|
||||||
galileo_telemetry_decoder_gs(const Gnss_Satellite &satellite, const Tlm_Conf &conf, int frame_type);
|
galileo_telemetry_decoder_gs(const Gnss_Satellite &satellite, const Tlm_Conf &conf, int frame_type);
|
||||||
|
|
||||||
|
void msg_handler_read_galileo_tow_map(const pmt::pmt_t &msg);
|
||||||
void deinterleaver(int32_t rows, int32_t cols, const float *in, float *out);
|
void deinterleaver(int32_t rows, int32_t cols, const float *in, float *out);
|
||||||
void decode_INAV_word(float *page_part_symbols, int32_t frame_length);
|
void decode_INAV_word(float *page_part_symbols, int32_t frame_length);
|
||||||
void decode_FNAV_word(float *page_symbols, int32_t frame_length);
|
void decode_FNAV_word(float *page_symbols, int32_t frame_length);
|
||||||
|
@ -20,6 +20,7 @@ set(CORE_LIBS_SOURCES
|
|||||||
galileo_e6_has_msg_receiver.cc
|
galileo_e6_has_msg_receiver.cc
|
||||||
nav_message_monitor.cc
|
nav_message_monitor.cc
|
||||||
nav_message_udp_sink.cc
|
nav_message_udp_sink.cc
|
||||||
|
galileo_tow_map.cc
|
||||||
)
|
)
|
||||||
|
|
||||||
set(CORE_LIBS_HEADERS
|
set(CORE_LIBS_HEADERS
|
||||||
@ -35,6 +36,7 @@ set(CORE_LIBS_HEADERS
|
|||||||
nav_message_udp_sink.h
|
nav_message_udp_sink.h
|
||||||
serdes_nav_message.h
|
serdes_nav_message.h
|
||||||
nav_message_monitor.h
|
nav_message_monitor.h
|
||||||
|
galileo_tow_map.h
|
||||||
)
|
)
|
||||||
|
|
||||||
if(ENABLE_FPGA)
|
if(ENABLE_FPGA)
|
||||||
|
102
src/core/libs/galileo_tow_map.cc
Normal file
102
src/core/libs/galileo_tow_map.cc
Normal file
@ -0,0 +1,102 @@
|
|||||||
|
/*!
|
||||||
|
* \file galileo_tow_map.cc
|
||||||
|
* \brief GNU Radio block that stores TOW for Galileo channels
|
||||||
|
* \author Carles Fernandez-Prades, 2022. cfernandez(at)cttc.es
|
||||||
|
*
|
||||||
|
* -----------------------------------------------------------------------------
|
||||||
|
*
|
||||||
|
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||||
|
* This file is part of GNSS-SDR.
|
||||||
|
*
|
||||||
|
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
|
||||||
|
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||||
|
*
|
||||||
|
* -----------------------------------------------------------------------------
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include "galileo_tow_map.h"
|
||||||
|
#include <glog/logging.h> // for LOG
|
||||||
|
#include <limits> // for std::numeric_limits
|
||||||
|
#include <memory> // for std::shared
|
||||||
|
#include <typeinfo> // for typeid
|
||||||
|
|
||||||
|
#if HAS_GENERIC_LAMBDA
|
||||||
|
#else
|
||||||
|
#include <boost/bind/bind.hpp>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if PMT_USES_BOOST_ANY
|
||||||
|
#include <boost/any.hpp>
|
||||||
|
namespace wht = boost;
|
||||||
|
#else
|
||||||
|
#include <any>
|
||||||
|
namespace wht = std;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
galileo_tow_map_sptr galileo_tow_map_make()
|
||||||
|
{
|
||||||
|
return galileo_tow_map_sptr(new galileo_tow_map());
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
galileo_tow_map::galileo_tow_map() : gr::block("galileo_tow_map", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
|
||||||
|
{
|
||||||
|
// register Gal E6 HAS input message port from telemetry blocks
|
||||||
|
this->message_port_register_in(pmt::mp("TOW_from_TLM"));
|
||||||
|
// register nav message monitor out
|
||||||
|
this->message_port_register_out(pmt::mp("TOW_to_TLM"));
|
||||||
|
// handler for input port
|
||||||
|
this->set_msg_handler(pmt::mp("TOW_from_TLM"),
|
||||||
|
#if HAS_GENERIC_LAMBDA
|
||||||
|
[this](auto&& PH1) { msg_handler_galileo_tow_map(PH1); });
|
||||||
|
#else
|
||||||
|
#if USE_BOOST_BIND_PLACEHOLDERS
|
||||||
|
boost::bind(&galileo_tow_map::msg_handler_galileo_tow_map, this, boost::placeholders::_1));
|
||||||
|
#else
|
||||||
|
boost::bind(&galileo_tow_map::msg_handler_galileo_tow_map, this, _1));
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
for (uint32_t prn = 1; prn < 37; prn++)
|
||||||
|
{
|
||||||
|
d_galileo_tow[prn] = std::pair<uint32_t, uint64_t>(std::numeric_limits<uint32_t>::max(), std::numeric_limits<uint64_t>::max());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void galileo_tow_map::msg_handler_galileo_tow_map(const pmt::pmt_t& msg)
|
||||||
|
{
|
||||||
|
gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_galileo_e6_has function called by the scheduler
|
||||||
|
|
||||||
|
// Check if the input has the right format
|
||||||
|
try
|
||||||
|
{
|
||||||
|
const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
|
||||||
|
if (msg_type_hash_code == typeid(std::shared_ptr<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>).hash_code())
|
||||||
|
{
|
||||||
|
const auto received_tow_map = wht::any_cast<std::shared_ptr<std::pair<uint32_t, std::pair<uint32_t, uint64_t>>>>(pmt::any_ref(msg));
|
||||||
|
const uint32_t received_prn = received_tow_map->first;
|
||||||
|
const uint32_t received_tow = received_tow_map->second.first;
|
||||||
|
const uint64_t received_sample_counter = received_tow_map->second.second;
|
||||||
|
|
||||||
|
//uint64_t sample_counter = std::numeric_limits<uint32_t>::max();
|
||||||
|
d_galileo_tow.erase(received_prn);
|
||||||
|
if (received_tow < 604800)
|
||||||
|
{
|
||||||
|
d_galileo_tow[received_prn] = std::pair<uint32_t, uint64_t>(received_tow, received_sample_counter);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
d_galileo_tow[received_prn] = std::pair<uint32_t, uint64_t>(std::numeric_limits<uint32_t>::max(), std::numeric_limits<uint64_t>::max());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (const wht::bad_any_cast& e)
|
||||||
|
{
|
||||||
|
LOG(WARNING) << "galileo_tow_map Bad any_cast: " << e.what();
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::shared_ptr<std::map<uint32_t, std::pair<uint32_t, uint64_t>>> tmp_obj = std::make_shared<std::map<uint32_t, std::pair<uint32_t, uint64_t>>>(d_galileo_tow);
|
||||||
|
this->message_port_pub(pmt::mp("TOW_to_TLM"), pmt::make_any(tmp_obj));
|
||||||
|
}
|
56
src/core/libs/galileo_tow_map.h
Normal file
56
src/core/libs/galileo_tow_map.h
Normal file
@ -0,0 +1,56 @@
|
|||||||
|
/*!
|
||||||
|
* \file galileo_tow_map.h
|
||||||
|
* \brief GNU Radio block that stores TOW for Galileo channels
|
||||||
|
* \author Carles Fernandez-Prades, 2022. cfernandez(at)cttc.es
|
||||||
|
*
|
||||||
|
* -----------------------------------------------------------------------------
|
||||||
|
*
|
||||||
|
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||||
|
* This file is part of GNSS-SDR.
|
||||||
|
*
|
||||||
|
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
|
||||||
|
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||||
|
*
|
||||||
|
* -----------------------------------------------------------------------------
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef GNSS_SDR_GALILEO_TOW_MAP_H
|
||||||
|
#define GNSS_SDR_GALILEO_TOW_MAP_H
|
||||||
|
|
||||||
|
#include "gnss_block_interface.h" // for gnss_shared_ptr
|
||||||
|
#include <gnuradio/block.h> // for gr::block
|
||||||
|
#include <pmt/pmt.h> // for pmt::pmt_t
|
||||||
|
#include <cstdint>
|
||||||
|
#include <map>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
|
/** \addtogroup Core
|
||||||
|
* \{ */
|
||||||
|
/** \addtogroup Core_Receiver_Library
|
||||||
|
* \{ */
|
||||||
|
|
||||||
|
class galileo_tow_map;
|
||||||
|
|
||||||
|
using galileo_tow_map_sptr = gnss_shared_ptr<galileo_tow_map>;
|
||||||
|
|
||||||
|
galileo_tow_map_sptr galileo_tow_map_make();
|
||||||
|
|
||||||
|
class galileo_tow_map : public gr::block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
~galileo_tow_map() = default; //!< Default destructor
|
||||||
|
// void set_enable_navdata_monitor(bool enable);
|
||||||
|
// std::shared_ptr<Galileo_HAS_data> process_test_page(const pmt::pmt_t& msg); //!< For testing purposes only
|
||||||
|
|
||||||
|
private:
|
||||||
|
friend galileo_tow_map_sptr galileo_tow_map_make();
|
||||||
|
galileo_tow_map();
|
||||||
|
|
||||||
|
void msg_handler_galileo_tow_map(const pmt::pmt_t& msg);
|
||||||
|
|
||||||
|
std::map<uint32_t, std::pair<uint32_t, uint64_t>> d_galileo_tow;
|
||||||
|
};
|
||||||
|
|
||||||
|
/** \} */
|
||||||
|
/** \} */
|
||||||
|
#endif // GNSS_SDR_GALILEO_TOW_MAP_H
|
@ -106,10 +106,12 @@ void GNSSFlowgraph::init()
|
|||||||
{
|
{
|
||||||
enable_e6_has_rx_ = true;
|
enable_e6_has_rx_ = true;
|
||||||
gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make();
|
gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make();
|
||||||
|
galileo_tow_map_ = galileo_tow_map_make();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
gal_e6_has_rx_ = nullptr;
|
gal_e6_has_rx_ = nullptr;
|
||||||
|
galileo_tow_map_ = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 1. read the number of RF front-ends available (one file_source per RF front-end)
|
// 1. read the number of RF front-ends available (one file_source per RF front-end)
|
||||||
@ -474,7 +476,12 @@ int GNSSFlowgraph::connect_desktop_flowgraph()
|
|||||||
{
|
{
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
if (connect_galileo_tow_map() != 0)
|
||||||
|
{
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Activate acquisition in enabled channels
|
// Activate acquisition in enabled channels
|
||||||
for (int i = 0; i < channels_count_; i++)
|
for (int i = 0; i < channels_count_; i++)
|
||||||
{
|
{
|
||||||
@ -569,6 +576,18 @@ int GNSSFlowgraph::connect_fpga_flowgraph()
|
|||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (enable_e6_has_rx_)
|
||||||
|
{
|
||||||
|
if (connect_gal_e6_has() != 0)
|
||||||
|
{
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
if (connect_galileo_tow_map() != 0)
|
||||||
|
{
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
check_desktop_conf_in_fpga_env();
|
check_desktop_conf_in_fpga_env();
|
||||||
|
|
||||||
LOG(INFO) << "The GNU Radio flowgraph for the current GNSS-SDR configuration with FPGA off-loading has been successfully connected";
|
LOG(INFO) << "The GNU Radio flowgraph for the current GNSS-SDR configuration with FPGA off-loading has been successfully connected";
|
||||||
@ -768,6 +787,26 @@ int GNSSFlowgraph::connect_pvt()
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int GNSSFlowgraph::connect_galileo_tow_map()
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
for (int i = 0; i < channels_count_; i++)
|
||||||
|
{
|
||||||
|
top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("TOW_from_TLM"), galileo_tow_map_, pmt::mp("TOW_from_TLM"));
|
||||||
|
top_block_->msg_connect(galileo_tow_map_, pmt::mp("TOW_to_TLM"), channels_.at(i)->get_right_block(), pmt::mp("TOW_to_TLM"));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (const std::exception& e)
|
||||||
|
{
|
||||||
|
LOG(ERROR) << "Can't connect The Galileo TOW map internally: " << e.what();
|
||||||
|
top_block_->disconnect_all();
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
int GNSSFlowgraph::connect_sample_counter()
|
int GNSSFlowgraph::connect_sample_counter()
|
||||||
{
|
{
|
||||||
// connect the sample counter to the Signal Conditioner
|
// connect the sample counter to the Signal Conditioner
|
||||||
|
@ -27,6 +27,7 @@
|
|||||||
#include "channel_status_msg_receiver.h"
|
#include "channel_status_msg_receiver.h"
|
||||||
#include "concurrent_queue.h"
|
#include "concurrent_queue.h"
|
||||||
#include "galileo_e6_has_msg_receiver.h"
|
#include "galileo_e6_has_msg_receiver.h"
|
||||||
|
#include "galileo_tow_map.h"
|
||||||
#include "gnss_sdr_sample_counter.h"
|
#include "gnss_sdr_sample_counter.h"
|
||||||
#include "gnss_signal.h"
|
#include "gnss_signal.h"
|
||||||
#include "pvt_interface.h"
|
#include "pvt_interface.h"
|
||||||
@ -171,6 +172,7 @@ private:
|
|||||||
int connect_observables();
|
int connect_observables();
|
||||||
int connect_pvt();
|
int connect_pvt();
|
||||||
int connect_sample_counter();
|
int connect_sample_counter();
|
||||||
|
int connect_galileo_tow_map();
|
||||||
|
|
||||||
int connect_signal_sources_to_signal_conditioners();
|
int connect_signal_sources_to_signal_conditioners();
|
||||||
int connect_signal_conditioners_to_channels();
|
int connect_signal_conditioners_to_channels();
|
||||||
@ -231,6 +233,7 @@ private:
|
|||||||
gr::basic_block_sptr NavDataMonitor_;
|
gr::basic_block_sptr NavDataMonitor_;
|
||||||
channel_status_msg_receiver_sptr channels_status_; // class that receives and stores the current status of the receiver channels
|
channel_status_msg_receiver_sptr channels_status_; // class that receives and stores the current status of the receiver channels
|
||||||
galileo_e6_has_msg_receiver_sptr gal_e6_has_rx_;
|
galileo_e6_has_msg_receiver_sptr gal_e6_has_rx_;
|
||||||
|
galileo_tow_map_sptr galileo_tow_map_;
|
||||||
|
|
||||||
gnss_sdr_sample_counter_sptr ch_out_sample_counter_;
|
gnss_sdr_sample_counter_sptr ch_out_sample_counter_;
|
||||||
#if ENABLE_FPGA
|
#if ENABLE_FPGA
|
||||||
|
Loading…
Reference in New Issue
Block a user