diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 6fb9a2510..83299a356 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -1573,11 +1573,15 @@ void rtklib_pvt_gs::msg_handler_has_data(const pmt::pmt_t& msg) const 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) { - if (d_enable_has_messages) + const auto has_data = wht::any_cast>(pmt::any_ref(msg)); + if (d_has_simple_printer) { - const auto has_data = wht::any_cast>(pmt::any_ref(msg)); d_has_simple_printer->print_message(has_data.get()); } + if (d_rtcm_printer && has_data->tow <= 604800) + { + d_rtcm_printer->Print_IGM_Messages(*has_data.get()); + } } } catch (const wht::bad_any_cast& e) diff --git a/src/algorithms/PVT/libs/rtcm.cc b/src/algorithms/PVT/libs/rtcm.cc index 5beb3d9ea..bbd0abeb1 100644 --- a/src/algorithms/PVT/libs/rtcm.cc +++ b/src/algorithms/PVT/libs/rtcm.cc @@ -3428,7 +3428,7 @@ std::string Rtcm::get_IGM01_header(const Galileo_HAS_data& has_data, uint8_t nsy { std::string header; - uint32_t tow = 0; // TODO + uint32_t tow = has_data.tow; uint16_t ssr_provider_id = 0; // ? uint8_t igm_version = 0; // ? uint8_t ssr_solution_id = 0; // ? @@ -3505,7 +3505,7 @@ std::string Rtcm::get_IGM02_header(const Galileo_HAS_data& has_data, uint8_t nsy { std::string header; - uint32_t tow = 0; // TODO + uint32_t tow = has_data.tow; uint16_t ssr_provider_id = 0; // ? uint8_t igm_version = 0; // ? uint8_t ssr_solution_id = 0; // ? @@ -3576,7 +3576,7 @@ std::string Rtcm::get_IGM03_header(const Galileo_HAS_data& has_data, uint8_t nsy { std::string header; - uint32_t tow = 0; // TODO + uint32_t tow = has_data.tow; uint16_t ssr_provider_id = 0; // ? uint8_t igm_version = 0; // ? uint8_t ssr_solution_id = 0; // ? @@ -3661,7 +3661,7 @@ std::string Rtcm::get_IGM05_header(const Galileo_HAS_data& has_data, uint8_t nsy { std::string header; - uint32_t tow = 0; // TODO + uint32_t tow = has_data.tow; uint16_t ssr_provider_id = 0; // ? uint8_t igm_version = 0; // ? uint8_t ssr_solution_id = 0; // ? 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 2ff4a6612..53d4ced5b 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 @@ -571,6 +571,10 @@ void galileo_telemetry_decoder_gs::decode_CNAV_word(uint64_t time_stamp, float * } else { + if (d_E6_TOW_set == true) + { + d_cnav_nav.set_tow(d_TOW_at_Preamble_ms / 1000); + } 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)); if (d_print_cnav_page == true) diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 53fc3d6bc..6b4c326d2 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -26,6 +26,7 @@ #include // for DLOG #include // for gr::io_signature::make #include // for std::find, std::count +#include // for std::remainder #include // for size_t #include // for std::back_inserter #include // for std::numeric_limits @@ -111,6 +112,7 @@ void galileo_e6_has_msg_receiver::set_enable_navdata_monitor(bool enable) std::shared_ptr galileo_e6_has_msg_receiver::process_test_page(const pmt::pmt_t& msg) { int64_t timestamp = std::numeric_limits::max(); + uint32_t tow = std::numeric_limits::max(); try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); @@ -126,6 +128,7 @@ std::shared_ptr galileo_e6_has_msg_receiver::process_test_page d_current_has_status = HAS_data_page->has_status; d_current_message_id = HAS_data_page->message_id; timestamp = HAS_data_page->time_stamp; + tow = HAS_data_page->tow; if (d_printed_mids[d_current_message_id] == false) { process_HAS_page(*HAS_data_page.get()); @@ -146,6 +149,7 @@ std::shared_ptr galileo_e6_has_msg_receiver::process_test_page { d_HAS_data.has_status = d_current_has_status; d_HAS_data.message_id = d_current_message_id; + d_HAS_data.tow = tow - static_cast(std::remainder(tow, 3600)) + d_HAS_data.header.toh; auto has_data_ptr = std::make_shared(d_HAS_data); d_new_message = false; d_printed_mids[d_current_message_id] = true; @@ -160,6 +164,7 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m { gr::thread::scoped_lock lock(d_setlock); // require mutex with msg_handler_galileo_e6_has function called by the scheduler int64_t timestamp = std::numeric_limits::max(); + uint32_t tow = std::numeric_limits::max(); try { const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code(); @@ -175,6 +180,7 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m d_current_has_status = HAS_data_page->has_status; d_current_message_id = HAS_data_page->message_id; timestamp = HAS_data_page->time_stamp; + tow = HAS_data_page->tow; if (d_printed_mids[d_current_message_id] == false) { process_HAS_page(*HAS_data_page.get()); @@ -195,6 +201,7 @@ void galileo_e6_has_msg_receiver::msg_handler_galileo_e6_has(const pmt::pmt_t& m { d_HAS_data.has_status = d_current_has_status; d_HAS_data.message_id = d_current_message_id; + d_HAS_data.tow = tow - static_cast(std::remainder(tow, 3600)) + d_HAS_data.header.toh; d_printed_mids[d_current_message_id] = true; d_printed_timestamps[d_current_message_id] = timestamp; auto has_data_ptr = std::make_shared(d_HAS_data); @@ -399,6 +406,7 @@ 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(); + d_HAS_data.tow = std::numeric_limits::max(); // Unknown read_MT1_header(decoded_message_type_1.substr(0, GALILEO_CNAV_MT1_HEADER_BITS)); diff --git a/src/core/system_parameters/galileo_cnav_message.cc b/src/core/system_parameters/galileo_cnav_message.cc index 0b93f6bf0..e514b00b7 100644 --- a/src/core/system_parameters/galileo_cnav_message.cc +++ b/src/core/system_parameters/galileo_cnav_message.cc @@ -21,6 +21,7 @@ #include // for boost::dynamic_bitset #include #include // for reverse +#include #include using CRC_Galileo_CNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; @@ -60,6 +61,7 @@ void Galileo_Cnav_Message::read_HAS_page(const std::string& page_string) const std::bitset checksum(CRC_data); d_new_HAS_page = false; has_page = Galileo_HAS_page(); + has_page.tow = std::numeric_limits::max(); // Unknown d_flag_CRC_test = CRC_test(Word_for_CRC_bits, checksum.to_ulong()); if (d_flag_CRC_test == true) { diff --git a/src/core/system_parameters/galileo_cnav_message.h b/src/core/system_parameters/galileo_cnav_message.h index 90444175b..436aed93f 100644 --- a/src/core/system_parameters/galileo_cnav_message.h +++ b/src/core/system_parameters/galileo_cnav_message.h @@ -74,6 +74,11 @@ public: has_page.time_stamp = time_stamp; } + inline void set_tow(uint32_t tow) + { + has_page.tow = tow; + } + private: uint8_t read_has_page_header_parameter(const std::bitset& bits, const std::pair& parameter) const; bool CRC_test(const std::bitset& bits, uint32_t checksum) const; diff --git a/src/core/system_parameters/galileo_has_data.h b/src/core/system_parameters/galileo_has_data.h index a4a3c289e..79b50eaeb 100644 --- a/src/core/system_parameters/galileo_has_data.h +++ b/src/core/system_parameters/galileo_has_data.h @@ -104,6 +104,8 @@ public: std::vector> phase_bias; //!< PB - Phase bias for the m-th signal of the n-th SV. See HAS SIS ICD 1.0 Section 5.2.6 std::vector> phase_discontinuity_indicator; //!< PDI - Phase Discontinuity Indicator. See HAS SIS ICD 1.0 Section 5.2.6. + uint32_t tow; //!< Time of Week + mt1_header header; //!< MT1 Header parameters. See HAS SIS ICD 1.0 Section 5.1.1 uint8_t has_status; //!< HASS - HAS Status (from HAS page header). See HAS SIS ICD 1.0 Section 3.1.1 uint8_t message_id; //!< MID - Message ID (from HAS page header). See HAS SIS ICD 1.0 Section 3.1 diff --git a/src/core/system_parameters/galileo_has_page.h b/src/core/system_parameters/galileo_has_page.h index 099ddd645..27da69f9b 100644 --- a/src/core/system_parameters/galileo_has_page.h +++ b/src/core/system_parameters/galileo_has_page.h @@ -39,6 +39,7 @@ public: std::string has_message_string; //!< HAS message content uint64_t time_stamp{}; //!< HAS page time stamp, in [s] + uint32_t tow{}; //!< HAS page time of week, in [s] // HAS page header uint8_t has_status{}; //!< HAS status