From 0319dd5ae1c922ae46c43ede547a55d8e43b0eb9 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 7 Sep 2021 14:58:16 +0200 Subject: [PATCH] Add decoded HAS mmesages to the navigation message monitor --- docs/protobuf/nav_message.proto | 9 +++-- src/core/libs/galileo_e6_has_msg_receiver.cc | 39 +++++++++++++++++++ src/core/libs/galileo_e6_has_msg_receiver.h | 5 +++ src/core/receiver/gnss_flowgraph.cc | 6 ++- src/utils/nav-listener/nav_message.proto | 9 +++-- .../nav-listener/nav_msg_udp_listener.cc | 2 +- 6 files changed, 59 insertions(+), 11 deletions(-) diff --git a/docs/protobuf/nav_message.proto b/docs/protobuf/nav_message.proto index 923b60872..dc7b227d5 100644 --- a/docs/protobuf/nav_message.proto +++ b/docs/protobuf/nav_message.proto @@ -6,11 +6,12 @@ package gnss_sdr; message navMsg { string system = 1; // GNSS constellation: "G" for GPS, "R" for Glonass, "E" for Galileo, and "C" for Beidou. - string signal = 2; // GNSS signal: "1C" for GPS L1 C/A, "1B" for Galileo E1b/c, "1G" for Glonass L1 C/A, "2S" for GPS L2 L2C(M), "2G" for Glonass L2 C/A, "L5" for GPS L5, and "5X" for Galileo E5a - int32 prn = 3; // SV ID + string signal = 2; // GNSS signal: "1C" for GPS L1 C/A, "1B" for Galileo E1b/c, "1G" for Glonass L1 C/A, "2S" for GPS L2 L2C(M), "2G" for Glonass L2 C/A, "L5" for GPS L5, "5X" for Galileo E5a, and "E6" for Galileo E6B. + int32 prn = 3; // SV ID. int32 tow_at_current_symbol_ms = 4; // Time of week of the last symbol received, in ms - string nav_message = 5; // for Galileo I/NAV: decoded half page (even or odd), 120 bits, as described in OS SIS ICD 2.0, paragraph 4.3.2.3. I/NAV Page Part - // for Galileo F/NAV: decoded word, 244 bits, as described in OS SIS ICD 2.0, paragraph 4.2.2. F/NAV Page Layout + string nav_message = 5; // for Galileo I/NAV: decoded half page (even or odd), 120 bits, as described in OS SIS ICD 2.0, paragraph 4.3.2.3. I/NAV Page Part. + // for Galileo F/NAV: decoded word, 244 bits, as described in OS SIS ICD 2.0, paragraph 4.2.2. F/NAV Page Layout. + // for Galileo HAS: decoded full HAS message (header + body), variable length, as described in Galileo HAS SIS ICD. // For GPS LNAV: decoded subframe, 300 bits, as described in IS-GPS-200M paragraph 20.3.2 Message Structure. // For GPS CNAV: decoded subframe, 300 bits, as described in IS-GPS-200M paragraph 30.3.3 Message Content. } diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index eee929070..209208c7d 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -48,6 +48,8 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ { // register Gal E6 HAS input message port from telemetry blocks this->message_port_register_in(pmt::mp("E6_HAS_from_TLM")); + // register nav message monitor out + this->message_port_register_out(pmt::mp("Nav_msg_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); }); @@ -64,6 +66,17 @@ galileo_e6_has_msg_receiver::galileo_e6_has_msg_receiver() : gr::block("galileo_ // initialize Reed-Solomon decoder d_rs = std::make_unique(); + + d_nav_msg_packet.system = std::string("E"); + d_nav_msg_packet.signal = std::string("E6"); + d_nav_msg_packet.prn = 0; + d_nav_msg_packet.tow_at_current_symbol_ms = 0; +} + + +void galileo_e6_has_msg_receiver::set_enable_navdata_monitor(bool enable) +{ + d_enable_navdata_monitor = enable; } @@ -178,6 +191,24 @@ int galileo_e6_has_msg_receiver::decode_message_type1(uint8_t message_id, uint8_ } } + if (erasure_positions.size() > 223) + { + // This should not happen! Maybe message_size < PID < 33 ? + // Don't even try to decode + std::string msg("Reed Solomon decoding of HAS message is not possible. Received PIDs:"); + std::stringstream ss; + for (auto pid : d_received_pids[message_id]) + { + ss << " " << static_cast(pid); + } + ss << ", Message size: " << static_cast(message_size) << " Message ID: " << static_cast(message_id); + msg += ss.str(); + LOG(ERROR) << msg; + d_received_pids[message_id].clear(); + d_C_matrix[message_id] = {GALILEO_CNAV_MAX_NUMBER_SYMBOLS_ENCODED_BLOCK, std::vector(GALILEO_CNAV_OCTETS_IN_SUBPAGE, 0)}; + return -1; + } + 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]); @@ -235,6 +266,14 @@ 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(); + + if (d_enable_navdata_monitor) + { + d_nav_msg_packet.nav_message = decoded_message_type_1; + const std::shared_ptr tmp_obj = std::make_shared(d_nav_msg_packet); + this->message_port_pub(pmt::mp("Nav_msg_from_TLM"), pmt::make_any(tmp_obj)); + } + 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())); diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index 45a0cd065..2f7fc8bfb 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -23,6 +23,7 @@ #include "Galileo_CNAV.h" #include "galileo_has_data.h" #include "gnss_block_interface.h" +#include "nav_message_packet.h" #include #include #include @@ -55,6 +56,7 @@ class galileo_e6_has_msg_receiver : public gr::block { public: ~galileo_e6_has_msg_receiver() = default; //!< Default destructor + void set_enable_navdata_monitor(bool enable); private: friend galileo_e6_has_msg_receiver_sptr galileo_e6_has_msg_receiver_make(); @@ -64,6 +66,8 @@ private: void read_MT1_header(const std::string& message_header); void read_MT1_body(const std::string& message_body); + Nav_Message_Packet d_nav_msg_packet; + 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; uint16_t read_has_message_header_parameter_uint16(const std::bitset& bits, const std::pair& parameter) const; @@ -84,6 +88,7 @@ private: 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_pids{32, std::vector()}; bool d_new_message{}; + bool d_enable_navdata_monitor{}; }; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 4b380459a..db1789086 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1527,6 +1527,7 @@ int GNSSFlowgraph::connect_tracking_monitor() return 0; } + int GNSSFlowgraph::connect_navdata_monitor() { try @@ -1535,6 +1536,8 @@ int GNSSFlowgraph::connect_navdata_monitor() { top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("Nav_msg_from_TLM"), NavDataMonitor_, pmt::mp("Nav_msg_from_TLM")); } + gal_e6_has_rx_->set_enable_navdata_monitor(true); + top_block_->msg_connect(gal_e6_has_rx_, pmt::mp("Nav_msg_from_TLM"), NavDataMonitor_, pmt::mp("Nav_msg_from_TLM")); } catch (const std::exception& e) { @@ -1596,8 +1599,6 @@ int GNSSFlowgraph::connect_gal_e6_has() 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: @@ -1647,6 +1648,7 @@ int GNSSFlowgraph::disconnect_monitors() if (enable_navdata_monitor_) { top_block_->msg_disconnect(channels_.at(i)->get_right_block(), pmt::mp("Nav_msg_from_TLM"), NavDataMonitor_, pmt::mp("Nav_msg_from_TLM")); + top_block_->msg_disconnect(gal_e6_has_rx_, pmt::mp("Nav_msg_from_TLM"), NavDataMonitor_, pmt::mp("Nav_msg_from_TLM")); } } } diff --git a/src/utils/nav-listener/nav_message.proto b/src/utils/nav-listener/nav_message.proto index 923b60872..dc7b227d5 100644 --- a/src/utils/nav-listener/nav_message.proto +++ b/src/utils/nav-listener/nav_message.proto @@ -6,11 +6,12 @@ package gnss_sdr; message navMsg { string system = 1; // GNSS constellation: "G" for GPS, "R" for Glonass, "E" for Galileo, and "C" for Beidou. - string signal = 2; // GNSS signal: "1C" for GPS L1 C/A, "1B" for Galileo E1b/c, "1G" for Glonass L1 C/A, "2S" for GPS L2 L2C(M), "2G" for Glonass L2 C/A, "L5" for GPS L5, and "5X" for Galileo E5a - int32 prn = 3; // SV ID + string signal = 2; // GNSS signal: "1C" for GPS L1 C/A, "1B" for Galileo E1b/c, "1G" for Glonass L1 C/A, "2S" for GPS L2 L2C(M), "2G" for Glonass L2 C/A, "L5" for GPS L5, "5X" for Galileo E5a, and "E6" for Galileo E6B. + int32 prn = 3; // SV ID. int32 tow_at_current_symbol_ms = 4; // Time of week of the last symbol received, in ms - string nav_message = 5; // for Galileo I/NAV: decoded half page (even or odd), 120 bits, as described in OS SIS ICD 2.0, paragraph 4.3.2.3. I/NAV Page Part - // for Galileo F/NAV: decoded word, 244 bits, as described in OS SIS ICD 2.0, paragraph 4.2.2. F/NAV Page Layout + string nav_message = 5; // for Galileo I/NAV: decoded half page (even or odd), 120 bits, as described in OS SIS ICD 2.0, paragraph 4.3.2.3. I/NAV Page Part. + // for Galileo F/NAV: decoded word, 244 bits, as described in OS SIS ICD 2.0, paragraph 4.2.2. F/NAV Page Layout. + // for Galileo HAS: decoded full HAS message (header + body), variable length, as described in Galileo HAS SIS ICD. // For GPS LNAV: decoded subframe, 300 bits, as described in IS-GPS-200M paragraph 20.3.2 Message Structure. // For GPS CNAV: decoded subframe, 300 bits, as described in IS-GPS-200M paragraph 30.3.3 Message Content. } diff --git a/src/utils/nav-listener/nav_msg_udp_listener.cc b/src/utils/nav-listener/nav_msg_udp_listener.cc index 646b332fe..91556b774 100644 --- a/src/utils/nav-listener/nav_msg_udp_listener.cc +++ b/src/utils/nav-listener/nav_msg_udp_listener.cc @@ -26,7 +26,7 @@ Nav_Msg_Udp_Listener::Nav_Msg_Udp_Listener(unsigned short port) bool Nav_Msg_Udp_Listener::read_nav_message(gnss_sdr::navMsg &message) { - char buff[1500]; // Buffer for storing the received data. + char buff[8192]; // Buffer for storing the received data. message_ = message; // This call will block until one or more bytes of data has been received.