From 0ad8dc3efe4cbcd244e5c3e9b2b8c8aba5f31891 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 2 Jun 2021 17:13:46 +0200 Subject: [PATCH 1/8] Adding Galileo E6 HAS message transport system --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 15 +++ .../galileo_telemetry_decoder_gs.cc | 6 ++ src/core/libs/CMakeLists.txt | 1 + src/core/libs/galileo_e6_has_msg_receiver.cc | 96 +++++++++++++++++++ src/core/libs/galileo_e6_has_msg_receiver.h | 57 +++++++++++ src/core/receiver/gnss_flowgraph.cc | 43 +++++++++ src/core/receiver/gnss_flowgraph.h | 4 + 7 files changed, 222 insertions(+) create mode 100644 src/core/libs/galileo_e6_has_msg_receiver.cc create mode 100644 src/core/libs/galileo_e6_has_msg_receiver.h diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 9914c0f28..76cd1a7ce 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -186,6 +186,21 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, boost::bind(&rtklib_pvt_gs::msg_handler_telemetry, this, _1)); #endif #endif + + //Galileo E6 HAS messages + this->message_port_register_in(pmt::mp("E6_HAS_to_PVT")); + //TODO: bind the Galileo E6 HAS message input with the desired function + // this->set_msg_handler(pmt::mp("E6_HAS_to_PVT"), + //#if HAS_GENERIC_LAMBDA + // [this](auto&& PH1) { msg_handler_telemetry(PH1); }); + //#else + //#if USE_BOOST_BIND_PLACEHOLDERS + // boost::bind(&rtklib_pvt_gs::msg_handler_telemetry, this, boost::placeholders::_1)); + //#else + // boost::bind(&rtklib_pvt_gs::msg_handler_telemetry, this, _1)); + //#endif + //#endif + // initialize kml_printer const std::string kml_dump_filename = d_dump_filename; d_kml_output_enabled = conf_.kml_output_enabled; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 5070a02b6..3a4c9f1e1 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -65,6 +65,9 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block 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")); + d_last_valid_preamble = 0; d_sent_tlm_failed_msg = false; d_band = '1'; @@ -537,9 +540,12 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t } else { + //TODO: do not send HAS data over telemetry msg port const std::shared_ptr tmp_obj = std::make_shared(d_cnav_nav.get_HAS_data()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); std::cout << TEXT_MAGENTA << "New Galileo E6 HAS message received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n'; + //Send HAS data to HAS processing class + this->message_port_pub(pmt::mp("E6_HAS_from_TLM"), pmt::make_any(tmp_obj)); } } } diff --git a/src/core/libs/CMakeLists.txt b/src/core/libs/CMakeLists.txt index a525f87c4..ba6e96932 100644 --- a/src/core/libs/CMakeLists.txt +++ b/src/core/libs/CMakeLists.txt @@ -15,6 +15,7 @@ set(CORE_LIBS_SOURCES channel_status_msg_receiver.cc channel_event.cc command_event.cc + galileo_e6_has_msg_receiver.cc ) set(CORE_LIBS_HEADERS diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc new file mode 100644 index 000000000..e3c03e5c2 --- /dev/null +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -0,0 +1,96 @@ +/*! + * \file galileo_e6_has_msg_receiver.cc + * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message sections from Galileo E6 telemetry blocks + * \author Javier Arribas, 2021. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "galileo_e6_has_msg_receiver.h" +#include "galileo_has_data.h" // For Galileo HAS messages +#include +#include +#include +#include +#include +#include +#include +#include + +#if HAS_GENERIC_LAMBDA +#else +#include +#endif + + +galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make() +{ + return galileo_e6_has_msg_receiver_sptr(new galileo_e6_has_msg_receiver()); +} + + +galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_e6_has_msg_receiver", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + //register Gal E6 HAS input message port + this->message_port_register_in(pmt::mp("E6_HAS_from_TLM")); + this->set_msg_handler(pmt::mp("E6_HAS_from_TLM"), +#if HAS_GENERIC_LAMBDA + [this](auto&& PH1) { msg_handler_galileo_e6_has(PH1); }); +#else +#if USE_BOOST_BIND_PLACEHOLDERS + boost::bind(&galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has, this, boost::placeholders::_1)); +#else + boost::bind(&galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has, this, _1)); +#endif +#endif + + //register Gal E6 processed HAS out + this->message_port_register_out(pmt::mp("E6_HAS_to_PVT")); +} + + +void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(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 + //1. receive the PMT message and reconstruct the object... + try + { + const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); + + //TODO: change example + // ****************** Gnss_Synchro received ************************ + if (msg_type_hash_code == typeid(std::shared_ptr).hash_code()) + { + const auto HAS_data_obj_obj = boost::any_cast>(pmt::any_ref(msg)); + + // store/ do things with the data + } + else + { + LOG(WARNING) << "channel_status_msg_receiver unknown object type!"; + } + } + catch (const boost::bad_any_cast& e) + { + LOG(WARNING) << "channel_status_msg_receiver Bad any_cast: " << e.what(); + } + + //2. Trigger the HAS processing function if required + //TODO + + //3. Send the resulting decoded HAS data (if available) to PVT + + //TODO: change example + Gnss_Synchro dummy; + this->message_port_pub(pmt::mp("E6_HAS_to_PVT"), pmt::make_any(dummy)); + //TODO +} diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h new file mode 100644 index 000000000..317e94ce6 --- /dev/null +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -0,0 +1,57 @@ +/*! + * \file GALILEO_E6_HAS_msg_receiver.h + * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message sections from Galileo E6 telemetry blocks + * \author Javier Arribas, 2021. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_CC_H +#define GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_CC_H + +#include "gnss_block_interface.h" +#include "gnss_synchro.h" +#include "monitor_pvt.h" +#include +#include +#include +#include + +/** \addtogroup Core + * \{ */ +/** \addtogroup Core_Receiver_Library + * \{ */ + + +class galileo_e6_has_msg_receiver; + +using galileo_e6_has_msg_receiver_sptr = gnss_shared_ptr; + +galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make(); + +/*! + * \brief GNU Radio block that receives asynchronous galileo e6 has messages from tlm blocks + */ +class galileo_e6_has_msg_receiver : public gr::block +{ +public: + ~galileo_e6_has_msg_receiver() = default; //!< Default destructor + +private: + friend galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make(); + galileo_e6_has_msg_receiver(); + void msg_handler_galileo_e6_has(const pmt::pmt_t& msg); +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_CC_H diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index fdb2d43a1..c6c09013e 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -99,6 +99,8 @@ void GNSSFlowgraph::init() channels_status_ = channel_status_msg_receiver_make(); + gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make(); + // 1. read the number of RF front-ends available (one file_source per RF front-end) int sources_count_deprecated = configuration_->property("Receiver.sources_count", 1); sources_count_ = configuration_->property("GNSS-SDR.num_sources", sources_count_deprecated); @@ -443,6 +445,11 @@ int GNSSFlowgraph::connect_desktop_flowgraph() return 1; } + if (connect_gal_e6_has() != 0) + { + return 1; + } + // Activate acquisition in enabled channels for (int i = 0; i < channels_count_; i++) { @@ -1529,6 +1536,42 @@ int GNSSFlowgraph::connect_monitors() return 0; } +int GNSSFlowgraph::connect_gal_e6_has() +{ + try + { + bool gal_e6_channels = false; + for (int i = 0; i < channels_count_; i++) + { + const std::string gnss_signal = channels_.at(i)->get_signal().get_signal_str(); + std::string gnss_system; + Gnss_Signal signal_value; + switch (mapStringValues_[gnss_signal]) + { + case evGAL_E6: + top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("E6_HAS_from_TLM"), gal_e6_has_rx_, pmt::mp("E6_HAS_from_TLM")); + gal_e6_channels = true; + break; + + default: + break; + } + } + + if (gal_e6_channels == true) + { + top_block_->msg_connect(gal_e6_has_rx_, pmt::mp("E6_HAS_to_PVT"), pvt_->get_left_block(), pmt::mp("E6_HAS_to_PVT")); + } + } + catch (const std::exception& e) + { + LOG(ERROR) << "Can't connect Galileo E6 HAS msg ports: " << e.what(); + top_block_->disconnect_all(); + return 1; + } + DLOG(INFO) << "Galileo E6 HAS message ports connected"; + return 0; +} int GNSSFlowgraph::disconnect_monitors() { diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index aea6c94e0..457bb5722 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -26,6 +26,7 @@ #include "channel_status_msg_receiver.h" #include "concurrent_queue.h" +#include "galileo_e6_has_msg_receiver.h" #include "gnss_sdr_sample_counter.h" #include "gnss_signal.h" #include "pvt_interface.h" @@ -176,6 +177,7 @@ private: int connect_channels_to_observables(); int connect_observables_to_pvt(); int connect_monitors(); + int connect_gal_e6_has(); int connect_gnss_synchro_monitor(); int connect_acquisition_monitor(); int connect_tracking_monitor(); @@ -244,6 +246,8 @@ private: gr::basic_block_sptr GnssSynchroAcquisitionMonitor_; gr::basic_block_sptr GnssSynchroTrackingMonitor_; 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_; + gnss_sdr_sample_counter_sptr ch_out_sample_counter_; #if ENABLE_FPGA gnss_sdr_fpga_sample_counter_sptr ch_out_fpga_sample_counter_; From deb49eeba3f02d0cb45ebc41dece4aec1a2fffe0 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 2 Jun 2021 21:26:39 +0200 Subject: [PATCH 2/8] Establish data path from Tlm to PVT for HAS data --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 43 +++++++++++++------ .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 3 ++ src/core/libs/galileo_e6_has_msg_receiver.cc | 33 +++++++------- src/core/libs/galileo_e6_has_msg_receiver.h | 15 ++++--- 4 files changed, 58 insertions(+), 36 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 76cd1a7ce..da3f93197 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -187,19 +187,18 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, #endif #endif - //Galileo E6 HAS messages + // Galileo E6 HAS messages port in this->message_port_register_in(pmt::mp("E6_HAS_to_PVT")); - //TODO: bind the Galileo E6 HAS message input with the desired function - // this->set_msg_handler(pmt::mp("E6_HAS_to_PVT"), - //#if HAS_GENERIC_LAMBDA - // [this](auto&& PH1) { msg_handler_telemetry(PH1); }); - //#else - //#if USE_BOOST_BIND_PLACEHOLDERS - // boost::bind(&rtklib_pvt_gs::msg_handler_telemetry, this, boost::placeholders::_1)); - //#else - // boost::bind(&rtklib_pvt_gs::msg_handler_telemetry, this, _1)); - //#endif - //#endif + this->set_msg_handler(pmt::mp("E6_HAS_to_PVT"), +#if HAS_GENERIC_LAMBDA + [this](auto&& PH1) { msg_handler_has_data(PH1); }); +#else +#if USE_BOOST_BIND_PLACEHOLDERS + boost::bind(&rtklib_pvt_gs::msg_handler_has_data, this, boost::placeholders::_1)); +#else + boost::bind(&rtklib_pvt_gs::msg_handler_has_data, this, _1)); +#endif +#endif // initialize kml_printer const std::string kml_dump_filename = d_dump_filename; @@ -524,6 +523,7 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_beidou_dnav_iono_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_beidou_dnav_utc_model_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_beidou_dnav_almanac_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); + d_galileo_has_data_sptr_type_hash_code = typeid(std::shared_ptr).hash_code(); d_start = std::chrono::system_clock::now(); } @@ -1485,6 +1485,25 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) } +void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) +{ + try + { + const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); + if (msg_type_hash_code == d_galileo_has_data_sptr_type_hash_code) + { + const auto has_data = boost::any_cast>(pmt::any_ref(msg)); + // TODO: Store HAS message + // std::cout << "HAS data received at PVT block.\n"; + } + } + catch (const boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any_cast: " << e.what(); + } +} + + std::map rtklib_pvt_gs::get_gps_ephemeris_map() const { return d_internal_pvt_solver->gps_ephemeris_map; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 4e6fd3144..086ceec9c 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -133,6 +133,8 @@ private: void msg_handler_telemetry(const pmt::pmt_t& msg); + void msg_handler_has_data(const pmt::pmt_t& msg); + void initialize_and_apply_carrier_phase_offset(); void apply_rx_clock_offset(std::map& observables_map, @@ -224,6 +226,7 @@ private: 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; + size_t d_galileo_has_data_sptr_type_hash_code; double d_rinex_version; double d_rx_time; diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index e3c03e5c2..266d45039 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -1,6 +1,7 @@ /*! * \file galileo_e6_has_msg_receiver.cc - * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message sections from Galileo E6 telemetry blocks + * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message + * sections from Galileo E6 telemetry blocks * \author Javier Arribas, 2021. jarribas(at)cttc.es * * ----------------------------------------------------------------------------- @@ -16,7 +17,8 @@ #include "galileo_e6_has_msg_receiver.h" -#include "galileo_has_data.h" // For Galileo HAS messages +#include "galileo_has_data.h" // for Galileo_HAS_data +#include "galileo_has_page.h" // for Galileo_HAS_page #include #include #include @@ -40,7 +42,7 @@ galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make() galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_e6_has_msg_receiver", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) { - //register Gal E6 HAS input message port + // register Gal E6 HAS input message port this->message_port_register_in(pmt::mp("E6_HAS_from_TLM")); this->set_msg_handler(pmt::mp("E6_HAS_from_TLM"), #if HAS_GENERIC_LAMBDA @@ -53,7 +55,7 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ #endif #endif - //register Gal E6 processed HAS out + // register Gal E6 processed HAS out this->message_port_register_out(pmt::mp("E6_HAS_to_PVT")); } @@ -61,18 +63,16 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(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 - //1. receive the PMT message and reconstruct the object... + // 1. receive the PMT message and reconstruct the object... try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); - //TODO: change example - // ****************** Gnss_Synchro received ************************ - if (msg_type_hash_code == typeid(std::shared_ptr).hash_code()) + if (msg_type_hash_code == typeid(std::shared_ptr).hash_code()) { - const auto HAS_data_obj_obj = boost::any_cast>(pmt::any_ref(msg)); - - // store/ do things with the data + const auto HAS_data_obj_obj = boost::any_cast>(pmt::any_ref(msg)); + // std::cout << HAS_data_obj_obj->has_message_string << '\n'; + // store / do things with the data } else { @@ -84,13 +84,12 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m LOG(WARNING) << "channel_status_msg_receiver Bad any_cast: " << e.what(); } - //2. Trigger the HAS processing function if required - //TODO + // 2. Trigger the HAS processing function if required + // TODO - //3. Send the resulting decoded HAS data (if available) to PVT + // 3. Send the resulting decoded HAS data (if available) to PVT - //TODO: change example - Gnss_Synchro dummy; + // TODO: fill message object and send to PVT when ready + std::shared_ptr dummy{}; this->message_port_pub(pmt::mp("E6_HAS_to_PVT"), pmt::make_any(dummy)); - //TODO } diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index 317e94ce6..6b5e2aa29 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -1,6 +1,7 @@ /*! - * \file GALILEO_E6_HAS_msg_receiver.h - * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message sections from Galileo E6 telemetry blocks + * \file galileo_e6_has_msg_receiver.h + * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message + * sections from Galileo E6 telemetry blocks * \author Javier Arribas, 2021. jarribas(at)cttc.es * * ----------------------------------------------------------------------------- @@ -14,12 +15,11 @@ * ----------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_CC_H -#define GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_CC_H +#ifndef GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_H +#define GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_H #include "gnss_block_interface.h" -#include "gnss_synchro.h" -#include "monitor_pvt.h" +#include "reed_solomon.h" #include #include #include @@ -49,9 +49,10 @@ private: friend galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make(); galileo_e6_has_msg_receiver(); void msg_handler_galileo_e6_has(const pmt::pmt_t& msg); + ReedSolomon rs = ReedSolomon(); }; /** \} */ /** \} */ -#endif // GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_CC_H +#endif // GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_H From ca442c70768d66f9d2ddeac6e9e0410e6b7e8fc9 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 2 Jun 2021 21:43:29 +0200 Subject: [PATCH 3/8] Fix cpplint CI job --- .../gnuradio_blocks/galileo_telemetry_decoder_gs.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 4fe1cfb39..6e957163a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -65,7 +65,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( this->message_port_register_out(pmt::mp("telemetry")); // Control messages to tracking block 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")); d_last_valid_preamble = 0; From 1feca37b4e21dfbb627c74b20679b5768d971e14 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 4 Jun 2021 17:13:35 +0200 Subject: [PATCH 4/8] Move HAS decoding to the galileo_e6_has_msg_receiver block --- src/core/libs/galileo_e6_has_msg_receiver.cc | 626 ++++++++++++++++++- src/core/libs/galileo_e6_has_msg_receiver.h | 44 +- 2 files changed, 638 insertions(+), 32 deletions(-) diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 266d45039..f6dcc52fe 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -1,8 +1,10 @@ /*! * \file galileo_e6_has_msg_receiver.cc - * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message - * sections from Galileo E6 telemetry blocks + * \brief GNU Radio block that processes Galileo HAS message pages received from + * Galileo E6B telemetry blocks. After successful decoding, sends the content to + * the PVT block. * \author Javier Arribas, 2021. jarribas(at)cttc.es + * \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es * * ----------------------------------------------------------------------------- * @@ -17,16 +19,16 @@ #include "galileo_e6_has_msg_receiver.h" -#include "galileo_has_data.h" // for Galileo_HAS_data #include "galileo_has_page.h" // for Galileo_HAS_page +#include "gnss_sdr_make_unique.h" +#include "reed_solomon.h" #include #include -#include #include -#include -#include -#include -#include +#include // std::find +#include // size_t +#include // std::accumulate +#include // typeid #if HAS_GENERIC_LAMBDA #else @@ -42,7 +44,7 @@ galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make() galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_e6_has_msg_receiver", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) { - // register Gal E6 HAS input message port + // register Gal E6 HAS input message port from telemetry blocks this->message_port_register_in(pmt::mp("E6_HAS_from_TLM")); this->set_msg_handler(pmt::mp("E6_HAS_from_TLM"), #if HAS_GENERIC_LAMBDA @@ -55,41 +57,615 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ #endif #endif - // register Gal E6 processed HAS out + // register Gal E6 processed HAS async output message port towards PVT this->message_port_register_out(pmt::mp("E6_HAS_to_PVT")); + + // initialize Reed-Solomon decoder + d_rs = std::make_unique(); } void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(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 - // 1. receive the PMT message and reconstruct the object... + try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); - if (msg_type_hash_code == typeid(std::shared_ptr).hash_code()) { - const auto HAS_data_obj_obj = boost::any_cast>(pmt::any_ref(msg)); - // std::cout << HAS_data_obj_obj->has_message_string << '\n'; - // store / do things with the data + const auto HAS_data_page = boost::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "New HAS page received: " + << "Status: " << static_cast(HAS_data_page->has_status) << ", " + << "MT: " << static_cast(HAS_data_page->message_type) << ", " + << "MID: " << static_cast(HAS_data_page->message_id) << ", " + << "MS: " << static_cast(HAS_data_page->message_size) << ", " + << "PID: " << static_cast(HAS_data_page->message_page_id); + + process_HAS_page(*HAS_data_page.get()); } else { - LOG(WARNING) << "channel_status_msg_receiver unknown object type!"; + LOG(WARNING) << "galileo_e6_has_msg_receiver received an unknown object type!"; } } catch (const boost::bad_any_cast& e) { - LOG(WARNING) << "channel_status_msg_receiver Bad any_cast: " << e.what(); + LOG(WARNING) << "galileo_e6_has_msg_receiver Bad any_cast: " << e.what(); } - // 2. Trigger the HAS processing function if required - // TODO - - // 3. Send the resulting decoded HAS data (if available) to PVT - - // TODO: fill message object and send to PVT when ready - std::shared_ptr dummy{}; - this->message_port_pub(pmt::mp("E6_HAS_to_PVT"), pmt::make_any(dummy)); + // Send the resulting decoded HAS data (if available) to PVT + if (d_new_message == true) + { + auto has_data_ptr = std::make_shared(d_HAS_data); + this->message_port_pub(pmt::mp("E6_HAS_to_PVT"), pmt::make_any(has_data_ptr)); + d_new_message = false; + DLOG(INFO) << "HAS message sent to the PVT block through the E6_HAS_to_PVT async message port"; + } +} + + +void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_page) +{ + if (has_page.has_status == 0 || has_page.has_status == 1) + { + std::string page_string(has_page.has_message_string); + if (has_page.message_page_id != 0) // PID=0 is reserved, ignore it + { + if (has_page.message_type == 1) // contains satellite corrections + { + if (has_page.message_id < 32) // MID range is from 0 to 31 + { + if (std::find(d_received_pages[has_page.message_id].begin(), d_received_pages[has_page.message_id].end(), has_page.message_page_id) == d_received_pages[has_page.message_id].end()) + { + // New pid! Annotate it. + d_received_pages[has_page.message_id].push_back(has_page.message_page_id); + for (int k = 0; k < GALILEO_CNAV_OCTETS_IN_SUBPAGE; k++) + { + std::string bits8 = page_string.substr(k * 8, 8); + std::bitset<8> bs(bits8); + d_C_matrix[has_page.message_id][has_page.message_page_id - 1][k] = static_cast(bs.to_ulong()); + } + } + } + } + } + } + + // If we have received for this message ID a number of pages equal to the message size + if (d_received_pages[has_page.message_id].size() == has_page.message_size) + { + // Try to decode the message + int res = decode_message_type1(has_page.message_id, has_page.message_size); + + if (res == 0) + { + // Successful decoding, we have a valid HAS message stored at d_HAS_data + d_new_message = true; + } + else + { + d_new_message = false; + } + } +} + + +int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_t message_size) +{ + DLOG(INFO) << "Start decoding of a HAS message"; + + // Compute erasure positions + std::vector erasure_positions; + erasure_positions.reserve(223); // Maximum erasure positions ( = number of parity symbols in a block) + for (int i = 0; i < message_size; i++) // we know that from message_size to 32, the value is 0 + { + if (d_C_matrix[message_id][i][0] == 0) + { + erasure_positions.push_back(i); + } + } + for (int i = 32; i < 255; i++) + { + if (d_C_matrix[message_id][i][0] == 0) + { + erasure_positions.push_back(i); + } + } + + // Reset HAS message matrix + d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; + + // Vertical decoding of d_C_matrix + for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++) + { + std::vector C_column(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, 0); + for (auto pid : d_received_pages[message_id]) + { + C_column[pid - 1] = d_C_matrix[message_id][pid - 1][col]; + } + + int result = d_rs->decode(C_column, erasure_positions); + + if (result < 0) + { + DLOG(INFO) << "Decoding of HAS page failed"; + return -1; + } + DLOG(INFO) << "Successful HAS page decoding"; + + std::vector M_column(C_column.begin(), C_column.begin() + GALILEO_CNAV_INFORMATION_VECTOR_LENGTH); + for (int i = 0; i < GALILEO_CNAV_INFORMATION_VECTOR_LENGTH; i++) + { + d_M_matrix[i][col] = M_column[i]; + } + } + + // Form the decoded HAS message by reading rows of d_M_matrix + std::string decoded_message_type_1; + decoded_message_type_1.reserve(message_size * GALILEO_CNAV_OCTETS_IN_SUBPAGE * 8); + for (uint8_t row = 0; row < message_size; row++) + { + for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++) + { + std::bitset<8> bs(d_M_matrix[row][col]); + decoded_message_type_1 += bs.to_string(); + } + } + + // reset data for next decoding + d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; + d_received_pages[message_id].clear(); + + // Trigger HAS message content reading and fill the d_HAS_data object + d_HAS_data = Galileo_HAS_data(); + read_MT1_header(decoded_message_type_1); + read_MT1_body(decoded_message_type_1); + + return 0; +} + + +void galileo_e6_has_msg_receiver::read_MT1_header(const std::string& message_string) +{ + // ICD v1.2 Table 6: MT1 Message Header + const std::bitset has_mt1_header(message_string); + d_HAS_data.header.toh = read_has_message_header_parameter_uint16(has_mt1_header, GALILEO_MT1_HEADER_TOH); + d_HAS_data.header.mask_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_MASK_ID); + d_HAS_data.header.iod_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_IOD_ID); + d_HAS_data.header.mask_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_MASK_FLAG); + d_HAS_data.header.orbit_correction_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_ORBIT_CORRECTION_FLAG); + d_HAS_data.header.clock_fullset_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CLOCK_FULLSET_FLAG); + d_HAS_data.header.clock_subset_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CLOCK_SUBSET_FLAG); + d_HAS_data.header.code_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_CODE_BIAS_FLAG); + d_HAS_data.header.phase_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_PHASE_BIAS_FLAG); + d_HAS_data.header.ura_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_URA_FLAG); + + DLOG(INFO) << "MT1 header: " + << "TOH: " << static_cast(d_HAS_data.header.toh) << ", " + << "mask iD: " << static_cast(d_HAS_data.header.mask_id) << ", " + << "iod iD: " << static_cast(d_HAS_data.header.iod_id) << ", " + << "mask_flag: " << static_cast(d_HAS_data.header.mask_flag) << ", " + << "orbit_correction_flag: " << static_cast(d_HAS_data.header.orbit_correction_flag) << ", " + << "clock_fullset_flag: " << static_cast(d_HAS_data.header.clock_fullset_flag) << ", " + << "clock_subset_flag: " << static_cast(d_HAS_data.header.clock_subset_flag) << ", " + << "code_bias_flag: " << static_cast(d_HAS_data.header.code_bias_flag) << ", " + << "phase_bias_flag: " << static_cast(d_HAS_data.header.phase_bias_flag) << ", " + << "ura_flag: " << static_cast(d_HAS_data.header.ura_flag); +} + + +void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_string) +{ + // ICD v1.2 Table 7: MT1 Message Body. + auto message = std::string(message_string.begin() + GALILEO_CNAV_MT1_HEADER_BITS, message_string.end()); // Remove header + int Nsat = 0; + if (d_HAS_data.header.mask_flag) + { + // read mask + d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH)); + message = std::string(message.begin() + HAS_MSG_NSYS_LENGTH, message.end()); + d_HAS_data.gnss_id_mask.reserve(d_HAS_data.Nsys); + d_HAS_data.cell_mask.reserve(d_HAS_data.Nsys); + d_HAS_data.cell_mask_availability_flag.reserve(d_HAS_data.Nsys); + d_HAS_data.nav_message.reserve(d_HAS_data.Nsys); + for (uint8_t i = 0; i < d_HAS_data.Nsys; i++) + { + d_HAS_data.gnss_id_mask[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_MASK_LENGTH)); + message = std::string(message.begin() + HAS_MSG_ID_MASK_LENGTH, message.end()); + std::string msg = message.substr(0, HAS_MSG_SATELLITE_MASK_LENGTH); + d_HAS_data.satellite_mask[i] = read_has_message_body_uint64(msg); + int ones_in_satellite_mask = 0; + for (char c : msg) + { + if (c == '1') + { + ones_in_satellite_mask++; + } + } + Nsat += ones_in_satellite_mask; + message = std::string(message.begin() + HAS_MSG_SATELLITE_MASK_LENGTH, message.end()); + + msg = message.substr(0, HAS_MSG_SIGNAL_MASK_LENGTH); + d_HAS_data.signal_mask[i] = read_has_message_body_uint16(msg); + int ones_in_signal_mask = 0; + for (char c : msg) + { + if (c == '1') + { + ones_in_signal_mask++; + } + } + message = std::string(message.begin() + HAS_MSG_SIGNAL_MASK_LENGTH, message.end()); + + if (message.substr(0, 1) == "1") + { + d_HAS_data.cell_mask_availability_flag[i] = true; + } + else + { + d_HAS_data.cell_mask_availability_flag[i] = false; + } + message = std::string(message.begin() + 1, message.end()); + int size_cell = ones_in_satellite_mask * ones_in_signal_mask; + + d_HAS_data.cell_mask[i].reserve(ones_in_satellite_mask); + for (int s = 0; s < ones_in_satellite_mask; s++) + { + d_HAS_data.cell_mask[i][s].reserve(ones_in_signal_mask); + for (int sig = 0; sig < ones_in_signal_mask; sig++) + { + d_HAS_data.cell_mask[i][s][sig] = (message[sig] == '1' ? true : false); + } + } + message = std::string(message.begin() + size_cell, message.end()); + + d_HAS_data.nav_message[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_NAV_MESSAGE_LENGTH)); + message = std::string(message.begin() + HAS_MSG_NAV_MESSAGE_LENGTH, message.end()); + } + } + if (d_HAS_data.header.orbit_correction_flag) + { + // read orbit corrections + d_HAS_data.validity_interval_index_orbit_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + d_HAS_data.gnss_iod.reserve(Nsat); + d_HAS_data.delta_radial.reserve(Nsat); + d_HAS_data.delta_along_track.reserve(Nsat); + d_HAS_data.delta_cross_track.reserve(Nsat); + for (int i = 0; i < Nsat; i++) + { + if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GPS_SYSTEM) + { + d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GPS_LENGTH)); + message = std::string(message.begin() + HAS_MSG_IOD_GPS_LENGTH, message.end()); + } + if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GALILEO_SYSTEM) + { + d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GAL_LENGTH)); + message = std::string(message.begin() + HAS_MSG_IOD_GAL_LENGTH, message.end()); + } + d_HAS_data.delta_radial[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_RADIAL_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_RADIAL_LENGTH, message.end()); + + d_HAS_data.delta_along_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_ALONG_TRACK_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_ALONG_TRACK_LENGTH, message.end()); + + d_HAS_data.delta_cross_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CROSS_TRACK_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_CROSS_TRACK_LENGTH, message.end()); + } + } + if (d_HAS_data.header.clock_fullset_flag) + { + // read clock full-set corrections + d_HAS_data.validity_interval_index_clock_fullset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + + d_HAS_data.delta_clock_c0_multiplier.reserve(d_HAS_data.Nsys); + for (uint8_t i = 0; i < d_HAS_data.Nsys; i++) + { + if (d_HAS_data.gnss_id_mask[i] != HAS_MSG_GALILEO_SYSTEM) + { + d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH, message.end()); + } + } + d_HAS_data.iod_change_flag.reserve(Nsat); + d_HAS_data.delta_clock_c0.reserve(Nsat); + for (int i = 0; i < Nsat; i++) + { + d_HAS_data.iod_change_flag[i] = (message[0] == '1' ? true : false); + message = std::string(message.begin() + 1, message.end()); + d_HAS_data.delta_clock_c0[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_LENGTH, message.end()); + } + } + if (d_HAS_data.header.clock_subset_flag) + { + // read clock subset corrections + d_HAS_data.validity_interval_index_clock_subset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + + d_HAS_data.Nsysprime = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYSPRIME_LENGTH)); + message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end()); + + d_HAS_data.gnss_id_clock_subset.reserve(d_HAS_data.Nsysprime); + d_HAS_data.delta_clock_c0_multiplier_clock_subset.reserve(d_HAS_data.Nsysprime); + d_HAS_data.satellite_submask.reserve(d_HAS_data.Nsysprime); + d_HAS_data.iod_change_flag_clock_subset.reserve(d_HAS_data.Nsysprime); + d_HAS_data.delta_clock_c0_clock_subset.reserve(d_HAS_data.Nsysprime); + for (uint8_t i = 0; i < d_HAS_data.Nsysprime; i++) + { + d_HAS_data.gnss_id_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH)); + message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end()); + if (d_HAS_data.gnss_id_clock_subset[i] != HAS_MSG_GALILEO_SYSTEM) + { + d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end()); + } + uint64_t number_sats_this_gnss_id = 0; + for (uint8_t j = 0; j < d_HAS_data.Nsys; j++) + { + if (d_HAS_data.gnss_id_mask[j] == d_HAS_data.gnss_id_clock_subset[i]) + { + uint64_t n = d_HAS_data.satellite_mask[j]; + while (n) + { + number_sats_this_gnss_id += n & 1; + n >>= 1; + } + break; + } + } + + d_HAS_data.satellite_submask[i].reserve(number_sats_this_gnss_id); + for (uint64_t j = 0; j < number_sats_this_gnss_id; j++) + { + d_HAS_data.satellite_submask[i][j] = read_has_message_body_uint64(message.substr(0, 1)); + message = std::string(message.begin() + 1, message.end()); + } + d_HAS_data.iod_change_flag_clock_subset[i] = (message[0] == '1' ? true : false); + message = std::string(message.begin() + 1, message.end()); + + d_HAS_data.delta_clock_c0_clock_subset[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH)); + message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH, message.end()); + } + } + if (d_HAS_data.header.code_bias_flag) + { + // read code bias + d_HAS_data.validity_interval_index_code_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + std::vector number_sats(d_HAS_data.Nsys, 0); + std::vector number_codes(d_HAS_data.Nsys, 0); + for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + { + uint64_t number_sats_this_gnss_id = 0; + uint64_t number_signals_this_gnss_id = 0; + if (d_HAS_data.cell_mask_availability_flag[sys] == true) + { + uint64_t n = d_HAS_data.satellite_mask[sys]; + while (n) + { + number_sats_this_gnss_id += n & 1; + n >>= 1; + } + uint64_t m = d_HAS_data.signal_mask[sys]; + while (m) + { + number_signals_this_gnss_id += m & 1; + m >>= 1; + } + } + else + { + number_sats_this_gnss_id = HAS_MSG_MAX_SATS; + number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; + } + number_sats[sys] = number_sats_this_gnss_id; + number_codes[sys] = number_signals_this_gnss_id; + } + uint64_t Nsat_b = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); + + d_HAS_data.code_bias.reserve(Nsat_b); + int sat = 0; + for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + { + d_HAS_data.code_bias[sat].reserve(number_codes[sys]); + for (uint64_t c = 0; c < number_codes[sys]; c++) + { + d_HAS_data.code_bias[sat][c] = read_has_message_body_int16(message.substr(0, HAS_MSG_CODE_BIAS_LENGTH)); + message = std::string(message.begin() + HAS_MSG_CODE_BIAS_LENGTH, message.end()); + sat += 1; + } + } + } + if (d_HAS_data.header.phase_bias_flag) + { + // read phase bias + d_HAS_data.validity_interval_index_phase_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + + std::vector number_sats(d_HAS_data.Nsys, 0); + std::vector number_phases(d_HAS_data.Nsys, 0); + for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + { + uint64_t number_sats_this_gnss_id = 0; + uint64_t number_signals_this_gnss_id = 0; + if (d_HAS_data.cell_mask_availability_flag[sys] == true) + { + uint64_t n = d_HAS_data.satellite_mask[sys]; + while (n) + { + number_sats_this_gnss_id += n & 1; + n >>= 1; + } + uint64_t m = d_HAS_data.signal_mask[sys]; + while (m) + { + number_signals_this_gnss_id += m & 1; + m >>= 1; + } + } + else + { + number_sats_this_gnss_id = HAS_MSG_MAX_SATS; + number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; + } + number_sats[sys] = number_sats_this_gnss_id; + number_phases[sys] = number_signals_this_gnss_id; + } + uint64_t Nsat_p = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); + + d_HAS_data.phase_bias.reserve(Nsat_p); + d_HAS_data.phase_discontinuity_indicator.reserve(Nsat_p); + int sat = 0; + for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + { + d_HAS_data.phase_bias[sat].reserve(number_phases[sys]); + d_HAS_data.phase_discontinuity_indicator[sat].reserve(number_phases[sys]); + for (uint64_t p = 0; p < number_phases[sys]; p++) + { + d_HAS_data.phase_bias[sat][p] = read_has_message_body_int16(message.substr(0, HAS_MSG_PHASE_BIAS_LENGTH)); + message = std::string(message.begin() + HAS_MSG_PHASE_BIAS_LENGTH, message.end()); + + d_HAS_data.phase_discontinuity_indicator[sat][p] = read_has_message_body_uint8(message.substr(0, HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH)); + message = std::string(message.begin() + HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH, message.end()); + sat += 1; + } + } + } + if (d_HAS_data.header.ura_flag) + { + // read URA + d_HAS_data.validity_interval_index_ura_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + d_HAS_data.ura.reserve(Nsat); + for (int i = 0; i < Nsat; i++) + { + d_HAS_data.ura[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_URA_LENGTH)); + message = std::string(message.begin() + HAS_MSG_URA_LENGTH, message.end()); + } + } +} + + +uint8_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint8(const std::bitset& bits, const std::pair& parameter) const +{ + uint8_t value = 0U; + for (int j = 0; j < parameter.second; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +uint16_t galileo_e6_has_msg_receiver::read_has_message_header_parameter_uint16(const std::bitset& bits, const std::pair& parameter) const +{ + uint16_t value = 0U; + for (int j = 0; j < parameter.second; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +bool galileo_e6_has_msg_receiver::read_has_message_header_parameter_bool(const std::bitset& bits, const std::pair& parameter) const +{ + bool value = false; + if (static_cast(bits[GALILEO_CNAV_MT1_HEADER_BITS - parameter.first]) == 1) + { + value = true; + } + return value; +} + + +uint8_t galileo_e6_has_msg_receiver::read_has_message_body_uint8(const std::string& bits) const +{ + uint8_t value = 0U; + size_t len = bits.length(); + + for (size_t j = 0; j < len; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[len - 1 - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +uint16_t galileo_e6_has_msg_receiver::read_has_message_body_uint16(const std::string& bits) const +{ + uint16_t value = 0U; + size_t len = bits.length(); + + for (size_t j = 0; j < len; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[len - 1 - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +uint64_t galileo_e6_has_msg_receiver::read_has_message_body_uint64(const std::string& bits) const +{ + uint64_t value = 0U; + size_t len = bits.length(); + + for (size_t j = 0; j < len; j++) + { + value <<= 1U; // shift left + if (static_cast(bits[len - 1 - j]) == 1) + { + value += 1; // insert the bit + } + } + return value; +} + + +int16_t galileo_e6_has_msg_receiver::read_has_message_body_int16(const std::string& bits) const +{ + int16_t value = 0; + size_t len = bits.length(); + + // read the MSB and perform the sign extension + if (static_cast(bits[len - 1]) == 1) + { + value ^= 0xFFFF; // 16 bits variable + } + else + { + value &= 0; + } + + for (size_t j = 0; j < len; j++) + { + value *= 2; // shift left the signed integer + value &= 0xFFFE; // reset the corresponding bit (for the 16 bits variable) + if (static_cast(bits[len - 1 - j]) == 1) + { + value += 1; // insert the bit + } + } + + return value; } diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index 6b5e2aa29..3548685d3 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -1,8 +1,10 @@ /*! * \file galileo_e6_has_msg_receiver.h - * \brief GNU Radio block that receives asynchronous Galileo E6 HAS message - * sections from Galileo E6 telemetry blocks + * \brief GNU Radio block that processes Galileo HAS message pages received from + * Galileo E6B telemetry blocks. After successful decoding, sends the content to + * the PVT block. * \author Javier Arribas, 2021. jarribas(at)cttc.es + * \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es * * ----------------------------------------------------------------------------- * @@ -18,19 +20,25 @@ #ifndef GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_H #define GNSS_SDR_GALILEO_E6_HAS_MSG_RECEIVER_H +#include "Galileo_CNAV.h" +#include "galileo_has_data.h" #include "gnss_block_interface.h" -#include "reed_solomon.h" #include #include -#include +#include +#include #include +#include +#include // std::pair +#include /** \addtogroup Core * \{ */ /** \addtogroup Core_Receiver_Library * \{ */ - +class Galileo_HAS_page; +class ReedSolomon; class galileo_e6_has_msg_receiver; using galileo_e6_has_msg_receiver_sptr = gnss_shared_ptr; @@ -38,7 +46,10 @@ using galileo_e6_has_msg_receiver_sptr = gnss_shared_ptr& bits, const std::pair& parameter) const; + uint16_t read_has_message_header_parameter_uint16(const std::bitset& bits, const std::pair& parameter) const; + bool read_has_message_header_parameter_bool(const std::bitset& bits, const std::pair& parameter) const; + + uint8_t read_has_message_body_uint8(const std::string& bits) const; + uint16_t read_has_message_body_uint16(const std::string& bits) const; + uint64_t read_has_message_body_uint64(const std::string& bits) const; + int16_t read_has_message_body_int16(const std::string& bits) const; + + std::unique_ptr d_rs; + Galileo_HAS_data d_HAS_data{}; + std::vector>> d_C_matrix{32, std::vector>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0))}; // 32 x 255 x 53 + std::vector> d_M_matrix{GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; // HAS message matrix 32 x 53 + std::vector> d_received_pages{32, std::vector()}; + bool d_new_message{}; }; From 32b7cee871f455cfa5df504c43b782feb1eed375 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 5 Jun 2021 20:35:12 +0200 Subject: [PATCH 5/8] Add work on Galileo E6B HAS message decoding --- .../galileo_telemetry_decoder_gs.cc | 15 +++++++++++---- .../galileo_telemetry_decoder_gs.h | 1 + src/core/libs/galileo_e6_has_msg_receiver.cc | 18 ++++++++++++------ 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 6e957163a..c91185651 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -206,6 +206,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_flag_PLL_180_deg_phase_locked = false; d_symbol_history.set_capacity(d_required_symbols + 1); d_cnav_dummy_page = false; + d_print_cnav_page = true; // vars for Viterbi decoder const int32_t max_states = 1U << static_cast(d_mm); // 2^d_mm @@ -538,10 +539,12 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t bool is_page_dummy = d_cnav_nav.is_HAS_page_dummy(); if (is_page_dummy == true) { + d_print_cnav_page = true; // Only print the message once if (is_page_dummy != d_cnav_dummy_page) { d_cnav_dummy_page = is_page_dummy; + std::cout << TEXT_MAGENTA << "Receiving Galileo E6 CNAV dummy pages in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << '\n'; @@ -551,10 +554,14 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(float *page_symbols, int32_t { const std::shared_ptr tmp_obj = std::make_shared(d_cnav_nav.get_HAS_encoded_page()); this->message_port_pub(pmt::mp("E6_HAS_from_TLM"), pmt::make_any(tmp_obj)); - std::cout << TEXT_MAGENTA << "New Galileo E6 HAS page received in channel " - << d_channel << " from satellite " << d_satellite - << (d_cnav_nav.is_HAS_in_test_mode() == true ? " (test mode)" : "") - << TEXT_RESET << '\n'; + if (d_print_cnav_page == true) + { + d_print_cnav_page = false; // only print the first page + std::cout << TEXT_MAGENTA << "Receiving Galileo E6 HAS pages" + << (d_cnav_nav.is_HAS_in_test_mode() == true ? " (test mode) " : " ") + << "in channel " << d_channel << " from satellite " << d_satellite + << TEXT_RESET << '\n'; + } } } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index 6ab86e166..3e602041f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -141,6 +141,7 @@ private: bool d_remove_dat; bool d_first_eph_sent; bool d_cnav_dummy_page; + bool d_print_cnav_page; }; diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index f6dcc52fe..5ac3f6d9d 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -19,6 +19,7 @@ #include "galileo_e6_has_msg_receiver.h" +#include "display.h" #include "galileo_has_page.h" // for Galileo_HAS_page #include "gnss_sdr_make_unique.h" #include "reed_solomon.h" @@ -141,6 +142,8 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p if (res == 0) { // Successful decoding, we have a valid HAS message stored at d_HAS_data + std::cout << TEXT_MAGENTA << "New Galileo HAS message type " << static_cast(has_page.message_id) + << " received and successfully decoded" << TEXT_RESET << '\n'; d_new_message = true; } else @@ -157,23 +160,24 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ // Compute erasure positions std::vector erasure_positions; - erasure_positions.reserve(223); // Maximum erasure positions ( = number of parity symbols in a block) - for (int i = 0; i < message_size; i++) // we know that from message_size to 32, the value is 0 + erasure_positions.reserve(223); // Maximum erasure positions ( = number of parity symbols in a block) + + for (uint8_t i = 0; i < message_size; i++) // we know that from message_size to 32, the value is 0 { - if (d_C_matrix[message_id][i][0] == 0) + if (std::find(d_received_pages[message_id].begin(), d_received_pages[message_id].end(), i) == d_received_pages[message_id].end()) { erasure_positions.push_back(i); } } for (int i = 32; i < 255; i++) { - if (d_C_matrix[message_id][i][0] == 0) + if (std::find(d_received_pages[message_id].begin(), d_received_pages[message_id].end(), static_cast(i)) == d_received_pages[message_id].end()) { erasure_positions.push_back(i); } } - // Reset HAS message matrix + // Reset HAS decoded message matrix d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; // Vertical decoding of d_C_matrix @@ -189,7 +193,7 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ if (result < 0) { - DLOG(INFO) << "Decoding of HAS page failed"; + DLOG(ERROR) << "Decoding of HAS page failed"; return -1; } DLOG(INFO) << "Successful HAS page decoding"; @@ -264,6 +268,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_strin { // read mask d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH)); + DLOG(INFO) << "Nsys: " << static_cast(d_HAS_data.Nsys); message = std::string(message.begin() + HAS_MSG_NSYS_LENGTH, message.end()); d_HAS_data.gnss_id_mask.reserve(d_HAS_data.Nsys); d_HAS_data.cell_mask.reserve(d_HAS_data.Nsys); @@ -387,6 +392,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_strin message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); d_HAS_data.Nsysprime = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYSPRIME_LENGTH)); + DLOG(INFO) << "Nsysprime: " << static_cast(d_HAS_data.Nsysprime); message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end()); d_HAS_data.gnss_id_clock_subset.reserve(d_HAS_data.Nsysprime); From dfd27e2815872b78d001fc5e70782563cd4615d9 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 7 Jun 2021 11:09:52 +0200 Subject: [PATCH 6/8] WIP: Add work on HAS decoding --- .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 6 +- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 2 +- src/core/libs/galileo_e6_has_msg_receiver.cc | 594 +++++++++--------- src/core/libs/galileo_e6_has_msg_receiver.h | 2 +- 4 files changed, 302 insertions(+), 302 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index da3f93197..622ba7543 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -1485,7 +1485,7 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg) } -void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) +void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) const { try { @@ -1493,13 +1493,13 @@ void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) if (msg_type_hash_code == d_galileo_has_data_sptr_type_hash_code) { const auto has_data = boost::any_cast>(pmt::any_ref(msg)); - // TODO: Store HAS message + // TODO: Dump HAS message // std::cout << "HAS data received at PVT block.\n"; } } catch (const boost::bad_any_cast& e) { - LOG(WARNING) << "msg_handler_telemetry Bad any_cast: " << e.what(); + LOG(WARNING) << "msg_handler_has_data Bad any_cast: " << e.what(); } } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 086ceec9c..77e40e9b4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -133,7 +133,7 @@ private: void msg_handler_telemetry(const pmt::pmt_t& msg); - void msg_handler_has_data(const pmt::pmt_t& msg); + void msg_handler_has_data(const pmt::pmt_t& msg) const; void initialize_and_apply_carrier_phase_offset(); diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 5ac3f6d9d..94f9badd3 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -117,10 +117,10 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p { if (has_page.message_id < 32) // MID range is from 0 to 31 { - if (std::find(d_received_pages[has_page.message_id].begin(), d_received_pages[has_page.message_id].end(), has_page.message_page_id) == d_received_pages[has_page.message_id].end()) + if (std::find(d_received_pids[has_page.message_id].begin(), d_received_pids[has_page.message_id].end(), has_page.message_page_id) == d_received_pids[has_page.message_id].end()) { // New pid! Annotate it. - d_received_pages[has_page.message_id].push_back(has_page.message_page_id); + d_received_pids[has_page.message_id].push_back(has_page.message_page_id); for (int k = 0; k < GALILEO_CNAV_OCTETS_IN_SUBPAGE; k++) { std::string bits8 = page_string.substr(k * 8, 8); @@ -134,7 +134,7 @@ void galileo_e6_has_msg_receiver::process_HAS_page(const Galileo_HAS_page& has_p } // If we have received for this message ID a number of pages equal to the message size - if (d_received_pages[has_page.message_id].size() == has_page.message_size) + if (d_received_pids[has_page.message_id].size() == has_page.message_size) { // Try to decode the message int res = decode_message_type1(has_page.message_id, has_page.message_size); @@ -162,18 +162,18 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ std::vector erasure_positions; erasure_positions.reserve(223); // Maximum erasure positions ( = number of parity symbols in a block) - for (uint8_t i = 0; i < message_size; i++) // we know that from message_size to 32, the value is 0 + for (uint8_t i = 1; i < message_size + 1; i++) // we know that from message_size to 32, the value is 0 { - if (std::find(d_received_pages[message_id].begin(), d_received_pages[message_id].end(), i) == d_received_pages[message_id].end()) + if (std::find(d_received_pids[message_id].begin(), d_received_pids[message_id].end(), i) == d_received_pids[message_id].end()) { - erasure_positions.push_back(i); + erasure_positions.push_back(i - 1); } } - for (int i = 32; i < 255; i++) + for (int i = 33; i < 256; i++) { - if (std::find(d_received_pages[message_id].begin(), d_received_pages[message_id].end(), static_cast(i)) == d_received_pages[message_id].end()) + if (std::find(d_received_pids[message_id].begin(), d_received_pids[message_id].end(), static_cast(i)) == d_received_pids[message_id].end()) { - erasure_positions.push_back(i); + erasure_positions.push_back(i - 1); } } @@ -184,7 +184,7 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ for (int col = 0; col < GALILEO_CNAV_OCTETS_IN_SUBPAGE; col++) { std::vector C_column(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, 0); - for (auto pid : d_received_pages[message_id]) + for (auto pid : d_received_pids[message_id]) { C_column[pid - 1] = d_C_matrix[message_id][pid - 1][col]; } @@ -219,7 +219,7 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ // reset data for next decoding d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; - d_received_pages[message_id].clear(); + d_received_pids[message_id].clear(); // Trigger HAS message content reading and fill the d_HAS_data object d_HAS_data = Galileo_HAS_data(); @@ -263,7 +263,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_strin { // ICD v1.2 Table 7: MT1 Message Body. auto message = std::string(message_string.begin() + GALILEO_CNAV_MT1_HEADER_BITS, message_string.end()); // Remove header - int Nsat = 0; + // int Nsat = 0; if (d_HAS_data.header.mask_flag) { // read mask @@ -276,283 +276,284 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_strin d_HAS_data.nav_message.reserve(d_HAS_data.Nsys); for (uint8_t i = 0; i < d_HAS_data.Nsys; i++) { - d_HAS_data.gnss_id_mask[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_MASK_LENGTH)); - message = std::string(message.begin() + HAS_MSG_ID_MASK_LENGTH, message.end()); - std::string msg = message.substr(0, HAS_MSG_SATELLITE_MASK_LENGTH); - d_HAS_data.satellite_mask[i] = read_has_message_body_uint64(msg); - int ones_in_satellite_mask = 0; - for (char c : msg) - { - if (c == '1') - { - ones_in_satellite_mask++; - } - } - Nsat += ones_in_satellite_mask; - message = std::string(message.begin() + HAS_MSG_SATELLITE_MASK_LENGTH, message.end()); - - msg = message.substr(0, HAS_MSG_SIGNAL_MASK_LENGTH); - d_HAS_data.signal_mask[i] = read_has_message_body_uint16(msg); - int ones_in_signal_mask = 0; - for (char c : msg) - { - if (c == '1') - { - ones_in_signal_mask++; - } - } - message = std::string(message.begin() + HAS_MSG_SIGNAL_MASK_LENGTH, message.end()); - - if (message.substr(0, 1) == "1") - { - d_HAS_data.cell_mask_availability_flag[i] = true; - } - else - { - d_HAS_data.cell_mask_availability_flag[i] = false; - } - message = std::string(message.begin() + 1, message.end()); - int size_cell = ones_in_satellite_mask * ones_in_signal_mask; - - d_HAS_data.cell_mask[i].reserve(ones_in_satellite_mask); - for (int s = 0; s < ones_in_satellite_mask; s++) - { - d_HAS_data.cell_mask[i][s].reserve(ones_in_signal_mask); - for (int sig = 0; sig < ones_in_signal_mask; sig++) - { - d_HAS_data.cell_mask[i][s][sig] = (message[sig] == '1' ? true : false); - } - } - message = std::string(message.begin() + size_cell, message.end()); - - d_HAS_data.nav_message[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_NAV_MESSAGE_LENGTH)); - message = std::string(message.begin() + HAS_MSG_NAV_MESSAGE_LENGTH, message.end()); - } - } - if (d_HAS_data.header.orbit_correction_flag) - { - // read orbit corrections - d_HAS_data.validity_interval_index_orbit_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); - message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - d_HAS_data.gnss_iod.reserve(Nsat); - d_HAS_data.delta_radial.reserve(Nsat); - d_HAS_data.delta_along_track.reserve(Nsat); - d_HAS_data.delta_cross_track.reserve(Nsat); - for (int i = 0; i < Nsat; i++) - { - if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GPS_SYSTEM) - { - d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GPS_LENGTH)); - message = std::string(message.begin() + HAS_MSG_IOD_GPS_LENGTH, message.end()); - } - if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GALILEO_SYSTEM) - { - d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GAL_LENGTH)); - message = std::string(message.begin() + HAS_MSG_IOD_GAL_LENGTH, message.end()); - } - d_HAS_data.delta_radial[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_RADIAL_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_RADIAL_LENGTH, message.end()); - - d_HAS_data.delta_along_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_ALONG_TRACK_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_ALONG_TRACK_LENGTH, message.end()); - - d_HAS_data.delta_cross_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CROSS_TRACK_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_CROSS_TRACK_LENGTH, message.end()); - } - } - if (d_HAS_data.header.clock_fullset_flag) - { - // read clock full-set corrections - d_HAS_data.validity_interval_index_clock_fullset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); - message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - - d_HAS_data.delta_clock_c0_multiplier.reserve(d_HAS_data.Nsys); - for (uint8_t i = 0; i < d_HAS_data.Nsys; i++) - { - if (d_HAS_data.gnss_id_mask[i] != HAS_MSG_GALILEO_SYSTEM) - { - d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH, message.end()); - } - } - d_HAS_data.iod_change_flag.reserve(Nsat); - d_HAS_data.delta_clock_c0.reserve(Nsat); - for (int i = 0; i < Nsat; i++) - { - d_HAS_data.iod_change_flag[i] = (message[0] == '1' ? true : false); - message = std::string(message.begin() + 1, message.end()); - d_HAS_data.delta_clock_c0[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_LENGTH, message.end()); - } - } - if (d_HAS_data.header.clock_subset_flag) - { - // read clock subset corrections - d_HAS_data.validity_interval_index_clock_subset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); - message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - - d_HAS_data.Nsysprime = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYSPRIME_LENGTH)); - DLOG(INFO) << "Nsysprime: " << static_cast(d_HAS_data.Nsysprime); - message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end()); - - d_HAS_data.gnss_id_clock_subset.reserve(d_HAS_data.Nsysprime); - d_HAS_data.delta_clock_c0_multiplier_clock_subset.reserve(d_HAS_data.Nsysprime); - d_HAS_data.satellite_submask.reserve(d_HAS_data.Nsysprime); - d_HAS_data.iod_change_flag_clock_subset.reserve(d_HAS_data.Nsysprime); - d_HAS_data.delta_clock_c0_clock_subset.reserve(d_HAS_data.Nsysprime); - for (uint8_t i = 0; i < d_HAS_data.Nsysprime; i++) - { - d_HAS_data.gnss_id_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH)); - message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end()); - if (d_HAS_data.gnss_id_clock_subset[i] != HAS_MSG_GALILEO_SYSTEM) - { - d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end()); - } - uint64_t number_sats_this_gnss_id = 0; - for (uint8_t j = 0; j < d_HAS_data.Nsys; j++) - { - if (d_HAS_data.gnss_id_mask[j] == d_HAS_data.gnss_id_clock_subset[i]) - { - uint64_t n = d_HAS_data.satellite_mask[j]; - while (n) - { - number_sats_this_gnss_id += n & 1; - n >>= 1; - } - break; - } - } - - d_HAS_data.satellite_submask[i].reserve(number_sats_this_gnss_id); - for (uint64_t j = 0; j < number_sats_this_gnss_id; j++) - { - d_HAS_data.satellite_submask[i][j] = read_has_message_body_uint64(message.substr(0, 1)); - message = std::string(message.begin() + 1, message.end()); - } - d_HAS_data.iod_change_flag_clock_subset[i] = (message[0] == '1' ? true : false); - message = std::string(message.begin() + 1, message.end()); - - d_HAS_data.delta_clock_c0_clock_subset[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH)); - message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH, message.end()); - } - } - if (d_HAS_data.header.code_bias_flag) - { - // read code bias - d_HAS_data.validity_interval_index_code_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); - message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - std::vector number_sats(d_HAS_data.Nsys, 0); - std::vector number_codes(d_HAS_data.Nsys, 0); - for (int sys = 0; sys < d_HAS_data.Nsys; sys++) - { - uint64_t number_sats_this_gnss_id = 0; - uint64_t number_signals_this_gnss_id = 0; - if (d_HAS_data.cell_mask_availability_flag[sys] == true) - { - uint64_t n = d_HAS_data.satellite_mask[sys]; - while (n) - { - number_sats_this_gnss_id += n & 1; - n >>= 1; - } - uint64_t m = d_HAS_data.signal_mask[sys]; - while (m) - { - number_signals_this_gnss_id += m & 1; - m >>= 1; - } - } - else - { - number_sats_this_gnss_id = HAS_MSG_MAX_SATS; - number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; - } - number_sats[sys] = number_sats_this_gnss_id; - number_codes[sys] = number_signals_this_gnss_id; - } - uint64_t Nsat_b = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); - - d_HAS_data.code_bias.reserve(Nsat_b); - int sat = 0; - for (int sys = 0; sys < d_HAS_data.Nsys; sys++) - { - d_HAS_data.code_bias[sat].reserve(number_codes[sys]); - for (uint64_t c = 0; c < number_codes[sys]; c++) - { - d_HAS_data.code_bias[sat][c] = read_has_message_body_int16(message.substr(0, HAS_MSG_CODE_BIAS_LENGTH)); - message = std::string(message.begin() + HAS_MSG_CODE_BIAS_LENGTH, message.end()); - sat += 1; - } - } - } - if (d_HAS_data.header.phase_bias_flag) - { - // read phase bias - d_HAS_data.validity_interval_index_phase_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); - message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - - std::vector number_sats(d_HAS_data.Nsys, 0); - std::vector number_phases(d_HAS_data.Nsys, 0); - for (int sys = 0; sys < d_HAS_data.Nsys; sys++) - { - uint64_t number_sats_this_gnss_id = 0; - uint64_t number_signals_this_gnss_id = 0; - if (d_HAS_data.cell_mask_availability_flag[sys] == true) - { - uint64_t n = d_HAS_data.satellite_mask[sys]; - while (n) - { - number_sats_this_gnss_id += n & 1; - n >>= 1; - } - uint64_t m = d_HAS_data.signal_mask[sys]; - while (m) - { - number_signals_this_gnss_id += m & 1; - m >>= 1; - } - } - else - { - number_sats_this_gnss_id = HAS_MSG_MAX_SATS; - number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; - } - number_sats[sys] = number_sats_this_gnss_id; - number_phases[sys] = number_signals_this_gnss_id; - } - uint64_t Nsat_p = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); - - d_HAS_data.phase_bias.reserve(Nsat_p); - d_HAS_data.phase_discontinuity_indicator.reserve(Nsat_p); - int sat = 0; - for (int sys = 0; sys < d_HAS_data.Nsys; sys++) - { - d_HAS_data.phase_bias[sat].reserve(number_phases[sys]); - d_HAS_data.phase_discontinuity_indicator[sat].reserve(number_phases[sys]); - for (uint64_t p = 0; p < number_phases[sys]; p++) - { - d_HAS_data.phase_bias[sat][p] = read_has_message_body_int16(message.substr(0, HAS_MSG_PHASE_BIAS_LENGTH)); - message = std::string(message.begin() + HAS_MSG_PHASE_BIAS_LENGTH, message.end()); - - d_HAS_data.phase_discontinuity_indicator[sat][p] = read_has_message_body_uint8(message.substr(0, HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH)); - message = std::string(message.begin() + HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH, message.end()); - sat += 1; - } - } - } - if (d_HAS_data.header.ura_flag) - { - // read URA - d_HAS_data.validity_interval_index_ura_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); - message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); - d_HAS_data.ura.reserve(Nsat); - for (int i = 0; i < Nsat; i++) - { - d_HAS_data.ura[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_URA_LENGTH)); - message = std::string(message.begin() + HAS_MSG_URA_LENGTH, message.end()); + // d_HAS_data.gnss_id_mask[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_MASK_LENGTH)); + // // DLOG(ERROR) << "GNSS ID" << static_cast(i) << ": " << static_cast(d_HAS_data.gnss_id_mask[i]); + // message = std::string(message.begin() + HAS_MSG_ID_MASK_LENGTH, message.end()); + // std::string msg = message.substr(0, HAS_MSG_SATELLITE_MASK_LENGTH); + // d_HAS_data.satellite_mask[i] = read_has_message_body_uint64(msg); + // int ones_in_satellite_mask = 0; + // for (char c : msg) + // { + // if (c == '1') + // { + // ones_in_satellite_mask++; + // } + // } + // Nsat += ones_in_satellite_mask; + // message = std::string(message.begin() + HAS_MSG_SATELLITE_MASK_LENGTH, message.end()); + // + // msg = message.substr(0, HAS_MSG_SIGNAL_MASK_LENGTH); + // d_HAS_data.signal_mask[i] = read_has_message_body_uint16(msg); + // int ones_in_signal_mask = 0; + // for (char c : msg) + // { + // if (c == '1') + // { + // ones_in_signal_mask++; + // } + // } + // message = std::string(message.begin() + HAS_MSG_SIGNAL_MASK_LENGTH, message.end()); + // + // if (message.substr(0, 1) == "1") + // { + // d_HAS_data.cell_mask_availability_flag[i] = true; + // } + // else + // { + // d_HAS_data.cell_mask_availability_flag[i] = false; + // } + // message = std::string(message.begin() + 1, message.end()); + // int size_cell = ones_in_satellite_mask * ones_in_signal_mask; + // + // d_HAS_data.cell_mask[i].reserve(ones_in_satellite_mask); + // for (int s = 0; s < ones_in_satellite_mask; s++) + // { + // d_HAS_data.cell_mask[i][s].reserve(ones_in_signal_mask); + // for (int sig = 0; sig < ones_in_signal_mask; sig++) + // { + // d_HAS_data.cell_mask[i][s][sig] = (message[sig] == '1' ? true : false); + // } + // } + // message = std::string(message.begin() + size_cell, message.end()); + // + // d_HAS_data.nav_message[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_NAV_MESSAGE_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_NAV_MESSAGE_LENGTH, message.end()); } } + // if (d_HAS_data.header.orbit_correction_flag) + // { + // // read orbit corrections + // d_HAS_data.validity_interval_index_orbit_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + // d_HAS_data.gnss_iod.reserve(Nsat); + // d_HAS_data.delta_radial.reserve(Nsat); + // d_HAS_data.delta_along_track.reserve(Nsat); + // d_HAS_data.delta_cross_track.reserve(Nsat); + // for (int i = 0; i < Nsat; i++) + // { + // if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GPS_SYSTEM) + // { + // d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GPS_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_IOD_GPS_LENGTH, message.end()); + // } + // if (d_HAS_data.gnss_id_mask[i] == HAS_MSG_GALILEO_SYSTEM) + // { + // d_HAS_data.gnss_iod[i] = read_has_message_body_uint16(message.substr(0, HAS_MSG_IOD_GAL_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_IOD_GAL_LENGTH, message.end()); + // } + // d_HAS_data.delta_radial[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_RADIAL_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_DELTA_RADIAL_LENGTH, message.end()); + // + // d_HAS_data.delta_along_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_ALONG_TRACK_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_DELTA_ALONG_TRACK_LENGTH, message.end()); + // + // d_HAS_data.delta_cross_track[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CROSS_TRACK_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_DELTA_CROSS_TRACK_LENGTH, message.end()); + // } + // } + // if (d_HAS_data.header.clock_fullset_flag) + // { + // // read clock full-set corrections + // d_HAS_data.validity_interval_index_clock_fullset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + // + // d_HAS_data.delta_clock_c0_multiplier.reserve(d_HAS_data.Nsys); + // for (uint8_t i = 0; i < d_HAS_data.Nsys; i++) + // { + // if (d_HAS_data.gnss_id_mask[i] != HAS_MSG_GALILEO_SYSTEM) + // { + // d_HAS_data.delta_clock_c0_multiplier[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_MULTIPLIER_LENGTH, message.end()); + // } + // } + // d_HAS_data.iod_change_flag.reserve(Nsat); + // d_HAS_data.delta_clock_c0.reserve(Nsat); + // for (int i = 0; i < Nsat; i++) + // { + // d_HAS_data.iod_change_flag[i] = (message[0] == '1' ? true : false); + // message = std::string(message.begin() + 1, message.end()); + // d_HAS_data.delta_clock_c0[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_LENGTH, message.end()); + // } + // } + // if (d_HAS_data.header.clock_subset_flag) + // { + // // read clock subset corrections + // d_HAS_data.validity_interval_index_clock_subset_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + // + // d_HAS_data.Nsysprime = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYSPRIME_LENGTH)); + // DLOG(INFO) << "Nsysprime: " << static_cast(d_HAS_data.Nsysprime); + // message = std::string(message.begin() + HAS_MSG_NSYSPRIME_LENGTH, message.end()); + // + // d_HAS_data.gnss_id_clock_subset.reserve(d_HAS_data.Nsysprime); + // d_HAS_data.delta_clock_c0_multiplier_clock_subset.reserve(d_HAS_data.Nsysprime); + // d_HAS_data.satellite_submask.reserve(d_HAS_data.Nsysprime); + // d_HAS_data.iod_change_flag_clock_subset.reserve(d_HAS_data.Nsysprime); + // d_HAS_data.delta_clock_c0_clock_subset.reserve(d_HAS_data.Nsysprime); + // for (uint8_t i = 0; i < d_HAS_data.Nsysprime; i++) + // { + // d_HAS_data.gnss_id_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_CLOCK_SUBSET_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_ID_CLOCK_SUBSET_LENGTH, message.end()); + // if (d_HAS_data.gnss_id_clock_subset[i] != HAS_MSG_GALILEO_SYSTEM) + // { + // d_HAS_data.delta_clock_c0_multiplier_clock_subset[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_MULTIPLIER_SUBSET_LENGTH, message.end()); + // } + // uint64_t number_sats_this_gnss_id = 0; + // for (uint8_t j = 0; j < d_HAS_data.Nsys; j++) + // { + // if (d_HAS_data.gnss_id_mask[j] == d_HAS_data.gnss_id_clock_subset[i]) + // { + // uint64_t n = d_HAS_data.satellite_mask[j]; + // while (n) + // { + // number_sats_this_gnss_id += n & 1; + // n >>= 1; + // } + // break; + // } + // } + // + // d_HAS_data.satellite_submask[i].reserve(number_sats_this_gnss_id); + // for (uint64_t j = 0; j < number_sats_this_gnss_id; j++) + // { + // d_HAS_data.satellite_submask[i][j] = read_has_message_body_uint64(message.substr(0, 1)); + // message = std::string(message.begin() + 1, message.end()); + // } + // d_HAS_data.iod_change_flag_clock_subset[i] = (message[0] == '1' ? true : false); + // message = std::string(message.begin() + 1, message.end()); + // + // d_HAS_data.delta_clock_c0_clock_subset[i] = read_has_message_body_int16(message.substr(0, HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_DELTA_CLOCK_C0_SUBSET_LENGTH, message.end()); + // } + // } + // if (d_HAS_data.header.code_bias_flag) + // { + // // read code bias + // d_HAS_data.validity_interval_index_code_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + // std::vector number_sats(d_HAS_data.Nsys, 0); + // std::vector number_codes(d_HAS_data.Nsys, 0); + // for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + // { + // uint64_t number_sats_this_gnss_id = 0; + // uint64_t number_signals_this_gnss_id = 0; + // if (d_HAS_data.cell_mask_availability_flag[sys] == true) + // { + // uint64_t n = d_HAS_data.satellite_mask[sys]; + // while (n) + // { + // number_sats_this_gnss_id += n & 1; + // n >>= 1; + // } + // uint64_t m = d_HAS_data.signal_mask[sys]; + // while (m) + // { + // number_signals_this_gnss_id += m & 1; + // m >>= 1; + // } + // } + // else + // { + // number_sats_this_gnss_id = HAS_MSG_MAX_SATS; + // number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; + // } + // number_sats[sys] = number_sats_this_gnss_id; + // number_codes[sys] = number_signals_this_gnss_id; + // } + // uint64_t Nsat_b = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); + // + // d_HAS_data.code_bias.reserve(Nsat_b); + // int sat = 0; + // for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + // { + // d_HAS_data.code_bias[sat].reserve(number_codes[sys]); + // for (uint64_t c = 0; c < number_codes[sys]; c++) + // { + // d_HAS_data.code_bias[sat][c] = read_has_message_body_int16(message.substr(0, HAS_MSG_CODE_BIAS_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_CODE_BIAS_LENGTH, message.end()); + // sat += 1; + // } + // } + // } + // if (d_HAS_data.header.phase_bias_flag) + // { + // // read phase bias + // d_HAS_data.validity_interval_index_phase_bias_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + // + // std::vector number_sats(d_HAS_data.Nsys, 0); + // std::vector number_phases(d_HAS_data.Nsys, 0); + // for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + // { + // uint64_t number_sats_this_gnss_id = 0; + // uint64_t number_signals_this_gnss_id = 0; + // if (d_HAS_data.cell_mask_availability_flag[sys] == true) + // { + // uint64_t n = d_HAS_data.satellite_mask[sys]; + // while (n) + // { + // number_sats_this_gnss_id += n & 1; + // n >>= 1; + // } + // uint64_t m = d_HAS_data.signal_mask[sys]; + // while (m) + // { + // number_signals_this_gnss_id += m & 1; + // m >>= 1; + // } + // } + // else + // { + // number_sats_this_gnss_id = HAS_MSG_MAX_SATS; + // number_signals_this_gnss_id = HAS_MSG_MAX_SIGNALS; + // } + // number_sats[sys] = number_sats_this_gnss_id; + // number_phases[sys] = number_signals_this_gnss_id; + // } + // uint64_t Nsat_p = std::accumulate(number_sats.begin(), number_sats.end(), 0ULL); + // + // d_HAS_data.phase_bias.reserve(Nsat_p); + // d_HAS_data.phase_discontinuity_indicator.reserve(Nsat_p); + // int sat = 0; + // for (int sys = 0; sys < d_HAS_data.Nsys; sys++) + // { + // d_HAS_data.phase_bias[sat].reserve(number_phases[sys]); + // d_HAS_data.phase_discontinuity_indicator[sat].reserve(number_phases[sys]); + // for (uint64_t p = 0; p < number_phases[sys]; p++) + // { + // d_HAS_data.phase_bias[sat][p] = read_has_message_body_int16(message.substr(0, HAS_MSG_PHASE_BIAS_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_PHASE_BIAS_LENGTH, message.end()); + // + // d_HAS_data.phase_discontinuity_indicator[sat][p] = read_has_message_body_uint8(message.substr(0, HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_PHASE_DISCONTINUITY_INDICATOR_LENGTH, message.end()); + // sat += 1; + // } + // } + // } + // if (d_HAS_data.header.ura_flag) + // { + // // read URA + // d_HAS_data.validity_interval_index_ura_corrections = read_has_message_body_uint8(message.substr(0, HAS_MSG_VALIDITY_INDEX_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_VALIDITY_INDEX_LENGTH, message.end()); + // d_HAS_data.ura.reserve(Nsat); + // for (int i = 0; i < Nsat; i++) + // { + // d_HAS_data.ura[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_URA_LENGTH)); + // message = std::string(message.begin() + HAS_MSG_URA_LENGTH, message.end()); + // } + // } } @@ -600,12 +601,11 @@ bool galileo_e6_has_msg_receiver::read_has_message_header_parameter_bool(const s uint8_t galileo_e6_has_msg_receiver::read_has_message_body_uint8(const std::string& bits) const { uint8_t value = 0U; - size_t len = bits.length(); - + const size_t len = bits.length(); for (size_t j = 0; j < len; j++) { value <<= 1U; // shift left - if (static_cast(bits[len - 1 - j]) == 1) + if (bits[j] == '1') { value += 1; // insert the bit } @@ -617,12 +617,12 @@ uint8_t galileo_e6_has_msg_receiver::read_has_message_body_uint8(const std::stri uint16_t galileo_e6_has_msg_receiver::read_has_message_body_uint16(const std::string& bits) const { uint16_t value = 0U; - size_t len = bits.length(); + const size_t len = bits.length(); for (size_t j = 0; j < len; j++) { value <<= 1U; // shift left - if (static_cast(bits[len - 1 - j]) == 1) + if (bits[j] == '1') { value += 1; // insert the bit } @@ -634,12 +634,12 @@ uint16_t galileo_e6_has_msg_receiver::read_has_message_body_uint16(const std::st uint64_t galileo_e6_has_msg_receiver::read_has_message_body_uint64(const std::string& bits) const { uint64_t value = 0U; - size_t len = bits.length(); + const size_t len = bits.length(); for (size_t j = 0; j < len; j++) { value <<= 1U; // shift left - if (static_cast(bits[len - 1 - j]) == 1) + if (bits[j] == '1') { value += 1; // insert the bit } @@ -651,10 +651,10 @@ uint64_t galileo_e6_has_msg_receiver::read_has_message_body_uint64(const std::st int16_t galileo_e6_has_msg_receiver::read_has_message_body_int16(const std::string& bits) const { int16_t value = 0; - size_t len = bits.length(); + const size_t len = bits.length(); // read the MSB and perform the sign extension - if (static_cast(bits[len - 1]) == 1) + if (static_cast(bits[0]) == 1) { value ^= 0xFFFF; // 16 bits variable } @@ -667,7 +667,7 @@ int16_t galileo_e6_has_msg_receiver::read_has_message_body_int16(const std::stri { value *= 2; // shift left the signed integer value &= 0xFFFE; // reset the corresponding bit (for the 16 bits variable) - if (static_cast(bits[len - 1 - j]) == 1) + if (bits[j] == '1') { value += 1; // insert the bit } diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index 3548685d3..e92d48abd 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -78,7 +78,7 @@ private: Galileo_HAS_data d_HAS_data{}; std::vector>> d_C_matrix{32, std::vector>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0))}; // 32 x 255 x 53 std::vector> d_M_matrix{GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; // HAS message matrix 32 x 53 - std::vector> d_received_pages{32, std::vector()}; + std::vector> d_received_pids{32, std::vector()}; bool d_new_message{}; }; From 18fd999bda8ceccf82f340921b43f4340ceaa052 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 8 Jun 2021 11:15:34 +0200 Subject: [PATCH 7/8] Add debug helpers --- src/core/libs/galileo_e6_has_msg_receiver.cc | 63 ++++++++++++++++---- src/core/libs/galileo_e6_has_msg_receiver.h | 8 ++- 2 files changed, 59 insertions(+), 12 deletions(-) diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 94f9badd3..572bbe528 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -29,6 +29,7 @@ #include // std::find #include // size_t #include // std::accumulate +#include // std::stringstream #include // typeid #if HAS_GENERIC_LAMBDA @@ -177,6 +178,10 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ } } + DLOG(INFO) << debug_print_vector("List of received PIDs", d_received_pids[message_id]); + DLOG(INFO) << debug_print_vector("erasure_positions", erasure_positions); + DLOG(INFO) << debug_print_matrix("d_C_matrix produced", d_C_matrix[message_id]); + // Reset HAS decoded message matrix d_M_matrix = {GALILEO_CNAV_INFORMATION_VECTOR_LENGTH, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE)}; @@ -189,6 +194,8 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ C_column[pid - 1] = d_C_matrix[message_id][pid - 1][col]; } + DLOG(INFO) << debug_print_vector("C_column entering the decoder", C_column); + int result = d_rs->decode(C_column, erasure_positions); if (result < 0) @@ -205,6 +212,8 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ } } + DLOG(INFO) << debug_print_matrix("M_matrix", d_M_matrix); + // Form the decoded HAS message by reading rows of d_M_matrix std::string decoded_message_type_1; decoded_message_type_1.reserve(message_size * GALILEO_CNAV_OCTETS_IN_SUBPAGE * 8); @@ -223,17 +232,17 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ // Trigger HAS message content reading and fill the d_HAS_data object d_HAS_data = Galileo_HAS_data(); - read_MT1_header(decoded_message_type_1); - read_MT1_body(decoded_message_type_1); + read_MT1_header(decoded_message_type_1.substr(0, GALILEO_CNAV_MT1_HEADER_BITS)); + read_MT1_body(std::string(decoded_message_type_1.begin() + GALILEO_CNAV_MT1_HEADER_BITS, decoded_message_type_1.end())); return 0; } -void galileo_e6_has_msg_receiver::read_MT1_header(const std::string& message_string) +void galileo_e6_has_msg_receiver::read_MT1_header(const std::string& message_header) { // ICD v1.2 Table 6: MT1 Message Header - const std::bitset has_mt1_header(message_string); + const std::bitset has_mt1_header(message_header); d_HAS_data.header.toh = read_has_message_header_parameter_uint16(has_mt1_header, GALILEO_MT1_HEADER_TOH); d_HAS_data.header.mask_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_MASK_ID); d_HAS_data.header.iod_id = read_has_message_header_parameter_uint8(has_mt1_header, GALILEO_MT1_HEADER_IOD_ID); @@ -245,10 +254,10 @@ void galileo_e6_has_msg_receiver::read_MT1_header(const std::string& message_str d_HAS_data.header.phase_bias_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_PHASE_BIAS_FLAG); d_HAS_data.header.ura_flag = read_has_message_header_parameter_bool(has_mt1_header, GALILEO_MT1_HEADER_URA_FLAG); - DLOG(INFO) << "MT1 header: " + DLOG(INFO) << "MT1 header " << message_header << ": " << "TOH: " << static_cast(d_HAS_data.header.toh) << ", " - << "mask iD: " << static_cast(d_HAS_data.header.mask_id) << ", " - << "iod iD: " << static_cast(d_HAS_data.header.iod_id) << ", " + << "mask ID: " << static_cast(d_HAS_data.header.mask_id) << ", " + << "iod ID: " << static_cast(d_HAS_data.header.iod_id) << ", " << "mask_flag: " << static_cast(d_HAS_data.header.mask_flag) << ", " << "orbit_correction_flag: " << static_cast(d_HAS_data.header.orbit_correction_flag) << ", " << "clock_fullset_flag: " << static_cast(d_HAS_data.header.clock_fullset_flag) << ", " @@ -259,16 +268,16 @@ void galileo_e6_has_msg_receiver::read_MT1_header(const std::string& message_str } -void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_string) +void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) { // ICD v1.2 Table 7: MT1 Message Body. - auto message = std::string(message_string.begin() + GALILEO_CNAV_MT1_HEADER_BITS, message_string.end()); // Remove header + auto message = std::string(message_body); // int Nsat = 0; if (d_HAS_data.header.mask_flag) { // read mask d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH)); - DLOG(INFO) << "Nsys: " << static_cast(d_HAS_data.Nsys); + // DLOG(ERROR) << "Nsys " << static_cast(d_HAS_data.Nsys); message = std::string(message.begin() + HAS_MSG_NSYS_LENGTH, message.end()); d_HAS_data.gnss_id_mask.reserve(d_HAS_data.Nsys); d_HAS_data.cell_mask.reserve(d_HAS_data.Nsys); @@ -675,3 +684,37 @@ int16_t galileo_e6_has_msg_receiver::read_has_message_body_int16(const std::stri return value; } + + +template +std::string galileo_e6_has_msg_receiver::debug_print_vector(const std::string& title, const std::vector& vec) const +{ + std::string msg(title); + msg += ": \n"; + std::stringstream ss; + for (auto el : vec) + { + ss << static_cast(el) << " "; + } + msg += ss.str(); + return msg; +} + + +std::string galileo_e6_has_msg_receiver::debug_print_matrix(const std::string& title, const std::vector>& mat) const +{ + std::string msg(title); + msg += ": \n"; + std::stringstream ss; + + for (size_t row = 0; row < mat.size(); row++) + { + for (size_t col = 0; col < mat[0].size(); col++) + { + ss << static_cast(mat[row][col]) << " "; + } + ss << '\n'; + } + msg += ss.str(); + return msg; +} diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index e92d48abd..45a0cd065 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -61,8 +61,8 @@ private: galileo_e6_has_msg_receiver(); void msg_handler_galileo_e6_has(const pmt::pmt_t& msg); void process_HAS_page(const Galileo_HAS_page& has_page); - void read_MT1_header(const std::string& message_string); - void read_MT1_body(const std::string& message_string); + void read_MT1_header(const std::string& message_header); + void read_MT1_body(const std::string& message_body); int decode_message_type1(uint8_t message_id, uint8_t message_size); uint8_t read_has_message_header_parameter_uint8(const std::bitset& bits, const std::pair& parameter) const; @@ -74,6 +74,10 @@ private: uint64_t read_has_message_body_uint64(const std::string& bits) const; int16_t read_has_message_body_int16(const std::string& bits) const; + template + std::string debug_print_vector(const std::string& title, const std::vector& vec) const; // only for debug purposes + std::string debug_print_matrix(const std::string& title, const std::vector>& mat) const; // only for debug purposes + std::unique_ptr d_rs; Galileo_HAS_data d_HAS_data{}; std::vector>> d_C_matrix{32, std::vector>(GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0))}; // 32 x 255 x 53 From db971a116bbf7045b1363e4ece931e66b2ffb4fd Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 9 Jun 2021 14:04:25 +0200 Subject: [PATCH 8/8] Add moe debug logging --- src/core/libs/galileo_e6_has_msg_receiver.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 572bbe528..eee929070 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -225,6 +225,9 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ decoded_message_type_1 += bs.to_string(); } } + DLOG(INFO) << "Decoded message ID " << static_cast(message_id) + << " (size: " << static_cast(message_size) << ") with body: " + << std::string(decoded_message_type_1.begin() + GALILEO_CNAV_MT1_HEADER_BITS, decoded_message_type_1.end()); // reset data for next decoding d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; @@ -277,7 +280,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) { // read mask d_HAS_data.Nsys = read_has_message_body_uint8(message.substr(0, HAS_MSG_NSYS_LENGTH)); - // DLOG(ERROR) << "Nsys " << static_cast(d_HAS_data.Nsys); + DLOG(INFO) << "Nsys " << static_cast(d_HAS_data.Nsys); message = std::string(message.begin() + HAS_MSG_NSYS_LENGTH, message.end()); d_HAS_data.gnss_id_mask.reserve(d_HAS_data.Nsys); d_HAS_data.cell_mask.reserve(d_HAS_data.Nsys);