mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Add work on getting TOW for E6 channels

This commit is contained in:
Carles Fernandez 2022-06-01 10:33:36 +02:00
parent 8aeb4bf3ea
commit 1ae6cac249
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
8 changed files with 293 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt
View File

@ -107,6 +107,13 @@ else()
)
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(CLANG_TIDY_EXE)
set_target_properties(telemetry_decoder_gr_blocks

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

@ -36,17 +36,32 @@
#include "viterbi_decoder.h" // for Viterbi_Decoder
#include <glog/logging.h> // for LOG, DLOG
#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 <array> // for std::array
#include <cmath> // for std::fmod, std::abs
#include <cstddef> // for size_t
#include <exception> // for std::exception
#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
galileo_telemetry_decoder_gs_sptr
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"));
// register Gal E6 messages HAS out
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)
{
@ -103,6 +120,20 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
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());
// 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)
{
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_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);
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
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_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);
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
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_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);
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
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>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
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)
{
@ -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>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
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)
{
@ -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>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
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)
{
@ -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>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
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
{

2
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h
View File

@ -32,6 +32,7 @@
#include <boost/circular_buffer.hpp> // for boost::circular_buffer
#include <gnuradio/block.h> // for block
#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 <fstream> // for std::ofstream
#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);
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 decode_INAV_word(float *page_part_symbols, int32_t frame_length);
void decode_FNAV_word(float *page_symbols, int32_t frame_length);

2
src/core/libs/CMakeLists.txt
View File

@ -20,6 +20,7 @@ set(CORE_LIBS_SOURCES
galileo_e6_has_msg_receiver.cc
nav_message_monitor.cc
nav_message_udp_sink.cc
galileo_tow_map.cc
)
set(CORE_LIBS_HEADERS
@ -35,6 +36,7 @@ set(CORE_LIBS_HEADERS
nav_message_udp_sink.h
serdes_nav_message.h
nav_message_monitor.h
galileo_tow_map.h
)
if(ENABLE_FPGA)

102
src/core/libs/galileo_tow_map.cc Normal file
View 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
View 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

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

@ -106,10 +106,12 @@ void GNSSFlowgraph::init()
{
enable_e6_has_rx_ = true;
gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make();
galileo_tow_map_ = galileo_tow_map_make();
}
else
{
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)
@ -474,7 +476,12 @@ int GNSSFlowgraph::connect_desktop_flowgraph()
{
return 1;
}
if (connect_galileo_tow_map() != 0)
{
return 1;
}
}
// Activate acquisition in enabled channels
for (int i = 0; i < channels_count_; i++)
{
@ -569,6 +576,18 @@ int GNSSFlowgraph::connect_fpga_flowgraph()
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();
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()
{
// connect the sample counter to the Signal Conditioner

3
src/core/receiver/gnss_flowgraph.h
View File

@ -27,6 +27,7 @@
#include "channel_status_msg_receiver.h"
#include "concurrent_queue.h"
#include "galileo_e6_has_msg_receiver.h"
#include "galileo_tow_map.h"
#include "gnss_sdr_sample_counter.h"
#include "gnss_signal.h"
#include "pvt_interface.h"
@ -171,6 +172,7 @@ private:
int connect_observables();
int connect_pvt();
int connect_sample_counter();
int connect_galileo_tow_map();
int connect_signal_sources_to_signal_conditioners();
int connect_signal_conditioners_to_channels();
@ -231,6 +233,7 @@ private:
gr::basic_block_sptr NavDataMonitor_;
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_tow_map_sptr galileo_tow_map_;
gnss_sdr_sample_counter_sptr ch_out_sample_counter_;
#if ENABLE_FPGA