From 24af4b228ec7aa5ac887e418a14dc83138b84e57 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 11 Apr 2021 14:46:50 +0200 Subject: [PATCH] Implement reading of INAV Reed-Solomon information and parity vectors --- .../galileo_telemetry_decoder_gs.cc | 4 + src/core/system_parameters/Galileo_INAV.h | 1 + .../system_parameters/galileo_inav_message.cc | 450 +++++++++++++----- .../system_parameters/galileo_inav_message.h | 42 +- 4 files changed, 356 insertions(+), 141 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 9064fbc59..4e9dc52e6 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 @@ -599,6 +599,10 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( // 1. Copy the current tracking output current_symbol = in[0][0]; d_band = current_symbol.Signal[0]; + if (d_band == '1') + { + d_inav_nav.enable_reed_solomon(); + } // add new symbol to the symbol queue switch (d_frame_type) diff --git a/src/core/system_parameters/Galileo_INAV.h b/src/core/system_parameters/Galileo_INAV.h index 793de4789..e7a3d2ddb 100644 --- a/src/core/system_parameters/Galileo_INAV.h +++ b/src/core/system_parameters/Galileo_INAV.h @@ -268,6 +268,7 @@ const std::vector> CED_af1red_BIT({{123, 6}}); const std::vector> RS_IODNAV_LSBS({{15, 2}}); constexpr size_t INAV_RS_SUBVECTOR_LENGTH = 15; constexpr size_t INAV_RS_PARITY_VECTOR_LENGTH = 60; +constexpr size_t INAV_RS_INFO_VECTOR_LENGTH = 58; constexpr int32_t BITS_IN_OCTET = 8; constexpr int32_t FIRST_RS_BIT = 7; constexpr int32_t FIRST_RS_BIT_AFTER_IODNAV = 17; diff --git a/src/core/system_parameters/galileo_inav_message.cc b/src/core/system_parameters/galileo_inav_message.cc index 43bbef2d9..62bcb2921 100644 --- a/src/core/system_parameters/galileo_inav_message.cc +++ b/src/core/system_parameters/galileo_inav_message.cc @@ -29,7 +29,13 @@ using CRC_Galileo_INAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>; -bool Galileo_Inav_Message::CRC_test(std::bitset bits, uint32_t checksum) const +Galileo_Inav_Message::Galileo_Inav_Message() +{ + rs_info_vector = std::vector(INAV_RS_INFO_VECTOR_LENGTH, 0); + rs_parity_vector = std::vector(INAV_RS_PARITY_VECTOR_LENGTH, 0); +} + +bool Galileo_Inav_Message::CRC_test(const std::bitset& bits, uint32_t checksum) const { CRC_Galileo_INAV_type CRC_Galileo; @@ -56,7 +62,7 @@ bool Galileo_Inav_Message::CRC_test(std::bitset bits, u } -uint64_t Galileo_Inav_Message::read_navigation_unsigned(std::bitset bits, const std::vector>& parameter) const +uint64_t Galileo_Inav_Message::read_navigation_unsigned(const std::bitset& bits, const std::vector>& parameter) const { uint64_t value = 0ULL; const int32_t num_of_slices = parameter.size(); @@ -75,7 +81,26 @@ uint64_t Galileo_Inav_Message::read_navigation_unsigned(std::bitset bits, const std::vector>& parameter) const +uint8_t Galileo_Inav_Message::read_octet_unsigned(const std::bitset& bits, const std::vector>& parameter) const +{ + uint8_t value = 0; + const int32_t num_of_slices = parameter.size(); + for (int32_t i = 0; i < num_of_slices; i++) + { + for (int32_t j = 0; j < parameter[i].second; j++) + { + value <<= 1; // shift left + if (static_cast(bits[GALILEO_DATA_JK_BITS - parameter[i].first - j]) == 1) + { + value += 1; // insert the bit + } + } + } + return value; +} + + +uint64_t Galileo_Inav_Message::read_page_type_unsigned(const std::bitset& bits, const std::vector>& parameter) const { uint64_t value = 0ULL; const int32_t num_of_slices = parameter.size(); @@ -94,7 +119,7 @@ uint64_t Galileo_Inav_Message::read_page_type_unsigned(std::bitset bits, const std::vector>& parameter) const +int64_t Galileo_Inav_Message::read_navigation_signed(const std::bitset& bits, const std::vector>& parameter) const { int64_t value = 0LL; const int32_t num_of_slices = parameter.size(); @@ -125,7 +150,7 @@ int64_t Galileo_Inav_Message::read_navigation_signed(std::bitset bits, const std::vector>& parameter) const +bool Galileo_Inav_Message::read_navigation_bool(const std::bitset& bits, const std::vector>& parameter) const { bool value; if (static_cast(static_cast(bits[GALILEO_DATA_JK_BITS - parameter[0].first])) == 1) @@ -461,10 +486,115 @@ Galileo_Ephemeris Galileo_Inav_Message::get_reduced_ced() const ced.af1red = ced_af1red; Galileo_Ephemeris eph = ced.compute_eph(); + eph.BGD_E1E5a = BGD_E1E5a_5; + eph.BGD_E1E5b = BGD_E1E5b_5; return eph; } +void Galileo_Inav_Message::read_page_1(const std::bitset& data_bits) +{ + IOD_nav_1 = static_cast(read_navigation_unsigned(data_bits, IOD_NAV_1_BIT)); + DLOG(INFO) << "IOD_nav_1= " << IOD_nav_1; + t0e_1 = static_cast(read_navigation_unsigned(data_bits, T0_E_1_BIT)); + t0e_1 = t0e_1 * T0E_1_LSB; + DLOG(INFO) << "t0e_1= " << t0e_1; + M0_1 = static_cast(read_navigation_signed(data_bits, M0_1_BIT)); + M0_1 = M0_1 * M0_1_LSB; + DLOG(INFO) << "M0_1= " << M0_1; + e_1 = static_cast(read_navigation_unsigned(data_bits, E_1_BIT)); + e_1 = e_1 * E_1_LSB; + DLOG(INFO) << "e_1= " << e_1; + A_1 = static_cast(read_navigation_unsigned(data_bits, A_1_BIT)); + A_1 = A_1 * A_1_LSB_GAL; + DLOG(INFO) << "A_1= " << A_1; + flag_ephemeris_1 = true; + DLOG(INFO) << "flag_tow_set" << flag_TOW_set; +} + + +void Galileo_Inav_Message::read_page_2(const std::bitset& data_bits) +{ + IOD_nav_2 = static_cast(read_navigation_unsigned(data_bits, IOD_NAV_2_BIT)); + DLOG(INFO) << "IOD_nav_2= " << IOD_nav_2; + OMEGA_0_2 = static_cast(read_navigation_signed(data_bits, OMEGA_0_2_BIT)); + OMEGA_0_2 = OMEGA_0_2 * OMEGA_0_2_LSB; + DLOG(INFO) << "OMEGA_0_2= " << OMEGA_0_2; + i_0_2 = static_cast(read_navigation_signed(data_bits, I_0_2_BIT)); + i_0_2 = i_0_2 * I_0_2_LSB; + DLOG(INFO) << "i_0_2= " << i_0_2; + omega_2 = static_cast(read_navigation_signed(data_bits, OMEGA_2_BIT)); + omega_2 = omega_2 * OMEGA_2_LSB; + DLOG(INFO) << "omega_2= " << omega_2; + iDot_2 = static_cast(read_navigation_signed(data_bits, I_DOT_2_BIT)); + iDot_2 = iDot_2 * I_DOT_2_LSB; + DLOG(INFO) << "iDot_2= " << iDot_2; + flag_ephemeris_2 = true; + DLOG(INFO) << "flag_tow_set" << flag_TOW_set; +} + + +void Galileo_Inav_Message::read_page_3(const std::bitset& data_bits) +{ + IOD_nav_3 = static_cast(read_navigation_unsigned(data_bits, IOD_NAV_3_BIT)); + DLOG(INFO) << "IOD_nav_3= " << IOD_nav_3; + OMEGA_dot_3 = static_cast(read_navigation_signed(data_bits, OMEGA_DOT_3_BIT)); + OMEGA_dot_3 = OMEGA_dot_3 * OMEGA_DOT_3_LSB; + DLOG(INFO) << "OMEGA_dot_3= " << OMEGA_dot_3; + delta_n_3 = static_cast(read_navigation_signed(data_bits, DELTA_N_3_BIT)); + delta_n_3 = delta_n_3 * DELTA_N_3_LSB; + DLOG(INFO) << "delta_n_3= " << delta_n_3; + C_uc_3 = static_cast(read_navigation_signed(data_bits, C_UC_3_BIT)); + C_uc_3 = C_uc_3 * C_UC_3_LSB; + DLOG(INFO) << "C_uc_3= " << C_uc_3; + C_us_3 = static_cast(read_navigation_signed(data_bits, C_US_3_BIT)); + C_us_3 = C_us_3 * C_US_3_LSB; + DLOG(INFO) << "C_us_3= " << C_us_3; + C_rc_3 = static_cast(read_navigation_signed(data_bits, C_RC_3_BIT)); + C_rc_3 = C_rc_3 * C_RC_3_LSB; + DLOG(INFO) << "C_rc_3= " << C_rc_3; + C_rs_3 = static_cast(read_navigation_signed(data_bits, C_RS_3_BIT)); + C_rs_3 = C_rs_3 * C_RS_3_LSB; + DLOG(INFO) << "C_rs_3= " << C_rs_3; + SISA_3 = static_cast(read_navigation_unsigned(data_bits, SISA_3_BIT)); + DLOG(INFO) << "SISA_3= " << SISA_3; + flag_ephemeris_3 = true; + DLOG(INFO) << "flag_tow_set" << flag_TOW_set; +} + + +void Galileo_Inav_Message::read_page_4(const std::bitset& data_bits) +{ + IOD_nav_4 = static_cast(read_navigation_unsigned(data_bits, IOD_NAV_4_BIT)); + DLOG(INFO) << "IOD_nav_4= " << IOD_nav_4; + SV_ID_PRN_4 = static_cast(read_navigation_unsigned(data_bits, SV_ID_PRN_4_BIT)); + DLOG(INFO) << "SV_ID_PRN_4= " << SV_ID_PRN_4; + C_ic_4 = static_cast(read_navigation_signed(data_bits, C_IC_4_BIT)); + C_ic_4 = C_ic_4 * C_IC_4_LSB; + DLOG(INFO) << "C_ic_4= " << C_ic_4; + C_is_4 = static_cast(read_navigation_signed(data_bits, C_IS_4_BIT)); + C_is_4 = C_is_4 * C_IS_4_LSB; + DLOG(INFO) << "C_is_4= " << C_is_4; + // Clock correction parameters + t0c_4 = static_cast(read_navigation_unsigned(data_bits, T0C_4_BIT)); + t0c_4 = t0c_4 * T0C_4_LSB; + DLOG(INFO) << "t0c_4= " << t0c_4; + af0_4 = static_cast(read_navigation_signed(data_bits, AF0_4_BIT)); + af0_4 = af0_4 * AF0_4_LSB; + DLOG(INFO) << "af0_4 = " << af0_4; + af1_4 = static_cast(read_navigation_signed(data_bits, AF1_4_BIT)); + af1_4 = af1_4 * AF1_4_LSB; + DLOG(INFO) << "af1_4 = " << af1_4; + af2_4 = static_cast(read_navigation_signed(data_bits, AF2_4_BIT)); + af2_4 = af2_4 * AF2_4_LSB; + DLOG(INFO) << "af2_4 = " << af2_4; + spare_4 = static_cast(read_navigation_unsigned(data_bits, SPARE_4_BIT)); + DLOG(INFO) << "spare_4 = " << spare_4; + flag_ephemeris_4 = true; + DLOG(INFO) << "flag_tow_set" << flag_TOW_set; +} + + int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk) { const std::string data_jk_string = data_jk; @@ -476,99 +606,123 @@ int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk) switch (page_number) { case 1: // Word type 1: Ephemeris (1/4) - IOD_nav_1 = static_cast(read_navigation_unsigned(data_jk_bits, IOD_NAV_1_BIT)); - DLOG(INFO) << "IOD_nav_1= " << IOD_nav_1; - t0e_1 = static_cast(read_navigation_unsigned(data_jk_bits, T0_E_1_BIT)); - t0e_1 = t0e_1 * T0E_1_LSB; - DLOG(INFO) << "t0e_1= " << t0e_1; - M0_1 = static_cast(read_navigation_signed(data_jk_bits, M0_1_BIT)); - M0_1 = M0_1 * M0_1_LSB; - DLOG(INFO) << "M0_1= " << M0_1; - e_1 = static_cast(read_navigation_unsigned(data_jk_bits, E_1_BIT)); - e_1 = e_1 * E_1_LSB; - DLOG(INFO) << "e_1= " << e_1; - A_1 = static_cast(read_navigation_unsigned(data_jk_bits, A_1_BIT)); - A_1 = A_1 * A_1_LSB_GAL; - DLOG(INFO) << "A_1= " << A_1; - flag_ephemeris_1 = true; - DLOG(INFO) << "flag_tow_set" << flag_TOW_set; - break; + { + read_page_1(data_jk_bits); + if (enable_rs) + { + if (current_IODnav == 0) + { + current_IODnav = IOD_nav_1; + } + if (current_IODnav != IOD_nav_1) + { + // IODnav changed, reset buffer + current_IODnav = IOD_nav_1; + rs_info_vector = std::vector(INAV_RS_INFO_VECTOR_LENGTH, 0); + } + + // Store RS information vector C_{RS,0} + std::vector> info_octet_bits({{1, 6}, {15, 2}}); + rs_info_vector[0] = read_octet_unsigned(data_jk_bits, info_octet_bits); + info_octet_bits = std::vector>({{7, BITS_IN_OCTET}}); + rs_info_vector[1] = read_octet_unsigned(data_jk_bits, info_octet_bits); + int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; + for (size_t i = 2; i < 16; i++) + { + info_octet_bits = std::vector>({{start_bit, BITS_IN_OCTET}}); + rs_info_vector[i] = read_octet_unsigned(data_jk_bits, info_octet_bits); + start_bit += BITS_IN_OCTET; + } + } + + break; + } case 2: // Word type 2: Ephemeris (2/4) - IOD_nav_2 = static_cast(read_navigation_unsigned(data_jk_bits, IOD_NAV_2_BIT)); - DLOG(INFO) << "IOD_nav_2= " << IOD_nav_2; - OMEGA_0_2 = static_cast(read_navigation_signed(data_jk_bits, OMEGA_0_2_BIT)); - OMEGA_0_2 = OMEGA_0_2 * OMEGA_0_2_LSB; - DLOG(INFO) << "OMEGA_0_2= " << OMEGA_0_2; - i_0_2 = static_cast(read_navigation_signed(data_jk_bits, I_0_2_BIT)); - i_0_2 = i_0_2 * I_0_2_LSB; - DLOG(INFO) << "i_0_2= " << i_0_2; - omega_2 = static_cast(read_navigation_signed(data_jk_bits, OMEGA_2_BIT)); - omega_2 = omega_2 * OMEGA_2_LSB; - DLOG(INFO) << "omega_2= " << omega_2; - iDot_2 = static_cast(read_navigation_signed(data_jk_bits, I_DOT_2_BIT)); - iDot_2 = iDot_2 * I_DOT_2_LSB; - DLOG(INFO) << "iDot_2= " << iDot_2; - flag_ephemeris_2 = true; - DLOG(INFO) << "flag_tow_set" << flag_TOW_set; - break; + { + read_page_2(data_jk_bits); + if (enable_rs) + { + if (current_IODnav == 0) + { + current_IODnav = IOD_nav_2; + } + if (current_IODnav != IOD_nav_2) + { + // IODnav changed, reset buffer + current_IODnav = IOD_nav_2; + rs_info_vector = std::vector(INAV_RS_INFO_VECTOR_LENGTH, 0); + } + // Store RS information vector C_{RS,1} + rs_info_vector[0] = 4 + current_IODnav % 4; // we always know c_{0,0} + int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; + for (size_t i = 16; i < 30; i++) + { + std::vector> info_octet_bits({{start_bit, BITS_IN_OCTET}}); + rs_info_vector[i] = read_octet_unsigned(data_jk_bits, info_octet_bits); + start_bit += BITS_IN_OCTET; + } + } + break; + } case 3: // Word type 3: Ephemeris (3/4) and SISA - IOD_nav_3 = static_cast(read_navigation_unsigned(data_jk_bits, IOD_NAV_3_BIT)); - DLOG(INFO) << "IOD_nav_3= " << IOD_nav_3; - OMEGA_dot_3 = static_cast(read_navigation_signed(data_jk_bits, OMEGA_DOT_3_BIT)); - OMEGA_dot_3 = OMEGA_dot_3 * OMEGA_DOT_3_LSB; - DLOG(INFO) << "OMEGA_dot_3= " << OMEGA_dot_3; - delta_n_3 = static_cast(read_navigation_signed(data_jk_bits, DELTA_N_3_BIT)); - delta_n_3 = delta_n_3 * DELTA_N_3_LSB; - DLOG(INFO) << "delta_n_3= " << delta_n_3; - C_uc_3 = static_cast(read_navigation_signed(data_jk_bits, C_UC_3_BIT)); - C_uc_3 = C_uc_3 * C_UC_3_LSB; - DLOG(INFO) << "C_uc_3= " << C_uc_3; - C_us_3 = static_cast(read_navigation_signed(data_jk_bits, C_US_3_BIT)); - C_us_3 = C_us_3 * C_US_3_LSB; - DLOG(INFO) << "C_us_3= " << C_us_3; - C_rc_3 = static_cast(read_navigation_signed(data_jk_bits, C_RC_3_BIT)); - C_rc_3 = C_rc_3 * C_RC_3_LSB; - DLOG(INFO) << "C_rc_3= " << C_rc_3; - C_rs_3 = static_cast(read_navigation_signed(data_jk_bits, C_RS_3_BIT)); - C_rs_3 = C_rs_3 * C_RS_3_LSB; - DLOG(INFO) << "C_rs_3= " << C_rs_3; - SISA_3 = static_cast(read_navigation_unsigned(data_jk_bits, SISA_3_BIT)); - DLOG(INFO) << "SISA_3= " << SISA_3; - flag_ephemeris_3 = true; - DLOG(INFO) << "flag_tow_set" << flag_TOW_set; - break; + { + read_page_3(data_jk_bits); + if (enable_rs) + { + if (current_IODnav == 0) + { + current_IODnav = IOD_nav_3; + } + if (current_IODnav != IOD_nav_3) + { + // IODnav changed, reset buffer + current_IODnav = IOD_nav_3; + rs_info_vector = std::vector(INAV_RS_INFO_VECTOR_LENGTH, 0); + } + + // Store RS information vector C_{RS,2} + rs_info_vector[0] = 4 + current_IODnav % 4; // we always know c_{0,0} + int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; + for (size_t i = 30; i < 44; i++) + { + std::vector> info_octet_bits({{start_bit, BITS_IN_OCTET}}); + rs_info_vector[i] = read_octet_unsigned(data_jk_bits, info_octet_bits); + start_bit += BITS_IN_OCTET; + } + } + break; + } case 4: // Word type 4: Ephemeris (4/4) and Clock correction parameters - IOD_nav_4 = static_cast(read_navigation_unsigned(data_jk_bits, IOD_NAV_4_BIT)); - DLOG(INFO) << "IOD_nav_4= " << IOD_nav_4; - SV_ID_PRN_4 = static_cast(read_navigation_unsigned(data_jk_bits, SV_ID_PRN_4_BIT)); - DLOG(INFO) << "SV_ID_PRN_4= " << SV_ID_PRN_4; - C_ic_4 = static_cast(read_navigation_signed(data_jk_bits, C_IC_4_BIT)); - C_ic_4 = C_ic_4 * C_IC_4_LSB; - DLOG(INFO) << "C_ic_4= " << C_ic_4; - C_is_4 = static_cast(read_navigation_signed(data_jk_bits, C_IS_4_BIT)); - C_is_4 = C_is_4 * C_IS_4_LSB; - DLOG(INFO) << "C_is_4= " << C_is_4; - // Clock correction parameters - t0c_4 = static_cast(read_navigation_unsigned(data_jk_bits, T0C_4_BIT)); - t0c_4 = t0c_4 * T0C_4_LSB; - DLOG(INFO) << "t0c_4= " << t0c_4; - af0_4 = static_cast(read_navigation_signed(data_jk_bits, AF0_4_BIT)); - af0_4 = af0_4 * AF0_4_LSB; - DLOG(INFO) << "af0_4 = " << af0_4; - af1_4 = static_cast(read_navigation_signed(data_jk_bits, AF1_4_BIT)); - af1_4 = af1_4 * AF1_4_LSB; - DLOG(INFO) << "af1_4 = " << af1_4; - af2_4 = static_cast(read_navigation_signed(data_jk_bits, AF2_4_BIT)); - af2_4 = af2_4 * AF2_4_LSB; - DLOG(INFO) << "af2_4 = " << af2_4; - spare_4 = static_cast(read_navigation_unsigned(data_jk_bits, SPARE_4_BIT)); - DLOG(INFO) << "spare_4 = " << spare_4; - flag_ephemeris_4 = true; - DLOG(INFO) << "flag_tow_set" << flag_TOW_set; - break; + { + read_page_4(data_jk_bits); + if (enable_rs) + { + if (current_IODnav == 0) + { + current_IODnav = IOD_nav_4; + } + if (current_IODnav != IOD_nav_4) + { + // IODnav changed, reset buffer + current_IODnav = IOD_nav_4; + rs_info_vector = std::vector(INAV_RS_INFO_VECTOR_LENGTH, 0); + } + + // Store RS information vector C_{RS,3} + rs_info_vector[0] = 4 + current_IODnav % 4; // we always know c_{0,0} + int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; + for (size_t i = 44; i < INAV_RS_INFO_VECTOR_LENGTH; i++) + { + std::vector> info_octet_bits({{start_bit, BITS_IN_OCTET}}); + rs_info_vector[i] = read_octet_unsigned(data_jk_bits, info_octet_bits); + start_bit += BITS_IN_OCTET; + } + } + break; + } case 5: // Word type 5: Ionospheric correction, BGD, signal health and data validity status and GST // Ionospheric correction @@ -830,64 +984,104 @@ int32_t Galileo_Inav_Message::page_jk_decoder(const char* data_jk) case 17: // Word type 17: FEC2 Reed-Solomon for CED { - std::vector> gamma_octet_bits({{FIRST_RS_BIT, BITS_IN_OCTET}}); - gamma_rs0[0] = static_cast(read_navigation_unsigned(data_jk_bits, gamma_octet_bits)); - IODnav_LSB17 = static_cast(read_navigation_unsigned(data_jk_bits, RS_IODNAV_LSBS)); - DLOG(INFO) << "IODnav 2 LSBs in Word type 17: " << static_cast(IODnav_LSB17); - int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; - for (size_t i = 1; i < INAV_RS_SUBVECTOR_LENGTH; i++) + if (enable_rs) { - gamma_octet_bits[0] = std::pair({start_bit, BITS_IN_OCTET}); - gamma_rs0[i] = static_cast(read_navigation_unsigned(data_jk_bits, gamma_octet_bits)); - start_bit += BITS_IN_OCTET; + IODnav_LSB17 = read_octet_unsigned(data_jk_bits, RS_IODNAV_LSBS); + DLOG(INFO) << "IODnav 2 LSBs in Word type 17: " << static_cast(IODnav_LSB17); + if (IODnav_LSB17 != static_cast((current_IODnav % 4))) + { + // IODnav changed, reset buffers + current_IODnav = 0; + rs_parity_vector = std::vector(INAV_RS_PARITY_VECTOR_LENGTH, 0); + } + // Store RS parity vector gamma_{RS,0} + std::vector> gamma_octet_bits({{FIRST_RS_BIT, BITS_IN_OCTET}}); + rs_parity_vector[0] = read_octet_unsigned(data_jk_bits, gamma_octet_bits); + int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; + for (size_t i = 1; i < INAV_RS_SUBVECTOR_LENGTH; i++) + { + gamma_octet_bits[0] = std::pair({start_bit, BITS_IN_OCTET}); + rs_parity_vector[i] = read_octet_unsigned(data_jk_bits, gamma_octet_bits); + start_bit += BITS_IN_OCTET; + } } break; } case 18: // Word type 18: FEC2 Reed-Solomon for CED { - std::vector> gamma_octet_bits({{FIRST_RS_BIT, BITS_IN_OCTET}}); - gamma_rs1[0] = static_cast(read_navigation_unsigned(data_jk_bits, gamma_octet_bits)); - IODnav_LSB18 = static_cast(read_navigation_unsigned(data_jk_bits, RS_IODNAV_LSBS)); - DLOG(INFO) << "IODnav 2 LSBs in Word type 18: " << static_cast(IODnav_LSB18); - int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; - for (size_t i = 1; i < INAV_RS_SUBVECTOR_LENGTH; i++) + if (enable_rs) { - gamma_octet_bits[0] = std::pair({start_bit, BITS_IN_OCTET}); - gamma_rs1[i] = static_cast(read_navigation_unsigned(data_jk_bits, gamma_octet_bits)); - start_bit += BITS_IN_OCTET; + IODnav_LSB18 = read_octet_unsigned(data_jk_bits, RS_IODNAV_LSBS); + DLOG(INFO) << "IODnav 2 LSBs in Word type 18: " << static_cast(IODnav_LSB18); + if (IODnav_LSB18 != static_cast((current_IODnav % 4))) + { + // IODnav changed, reset buffers + current_IODnav = 0; + rs_parity_vector = std::vector(INAV_RS_PARITY_VECTOR_LENGTH, 0); + } + // Store RS parity vector gamma_{RS,1} + std::vector> gamma_octet_bits({{FIRST_RS_BIT, BITS_IN_OCTET}}); + rs_parity_vector[INAV_RS_SUBVECTOR_LENGTH] = read_octet_unsigned(data_jk_bits, gamma_octet_bits); + int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; + for (size_t i = INAV_RS_SUBVECTOR_LENGTH + 1; i < 2 * INAV_RS_SUBVECTOR_LENGTH; i++) + { + gamma_octet_bits[0] = std::pair({start_bit, BITS_IN_OCTET}); + rs_parity_vector[i] = read_octet_unsigned(data_jk_bits, gamma_octet_bits); + start_bit += BITS_IN_OCTET; + } } break; } case 19: // Word type 19: FEC2 Reed-Solomon for CED { - std::vector> gamma_octet_bits({{FIRST_RS_BIT, BITS_IN_OCTET}}); - gamma_rs2[0] = static_cast(read_navigation_unsigned(data_jk_bits, gamma_octet_bits)); - IODnav_LSB19 = static_cast(read_navigation_unsigned(data_jk_bits, RS_IODNAV_LSBS)); - DLOG(INFO) << "IODnav 2 LSBs in Word type 19: " << static_cast(IODnav_LSB19); - int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; - for (size_t i = 1; i < INAV_RS_SUBVECTOR_LENGTH; i++) + if (enable_rs) { - gamma_octet_bits[0] = std::pair({start_bit, BITS_IN_OCTET}); - gamma_rs2[i] = static_cast(read_navigation_unsigned(data_jk_bits, gamma_octet_bits)); - start_bit += BITS_IN_OCTET; + IODnav_LSB19 = read_octet_unsigned(data_jk_bits, RS_IODNAV_LSBS); + DLOG(INFO) << "IODnav 2 LSBs in Word type 19: " << static_cast(IODnav_LSB19); + if (IODnav_LSB19 != static_cast((current_IODnav % 4))) + { + // IODnav changed, reset buffers + current_IODnav = 0; + rs_parity_vector = std::vector(INAV_RS_PARITY_VECTOR_LENGTH, 0); + } + // Store RS parity vector gamma_{RS,2} + std::vector> gamma_octet_bits({{FIRST_RS_BIT, BITS_IN_OCTET}}); + rs_parity_vector[2 * INAV_RS_SUBVECTOR_LENGTH] = read_octet_unsigned(data_jk_bits, gamma_octet_bits); + int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; + for (size_t i = 2 * INAV_RS_SUBVECTOR_LENGTH + 1; i < 3 * INAV_RS_SUBVECTOR_LENGTH; i++) + { + gamma_octet_bits[0] = std::pair({start_bit, BITS_IN_OCTET}); + rs_parity_vector[i] = read_octet_unsigned(data_jk_bits, gamma_octet_bits); + start_bit += BITS_IN_OCTET; + } } break; } case 20: // Word type 20: FEC2 Reed-Solomon for CED { - std::vector> gamma_octet_bits({{FIRST_RS_BIT, BITS_IN_OCTET}}); - gamma_rs3[0] = static_cast(read_navigation_unsigned(data_jk_bits, gamma_octet_bits)); - IODnav_LSB20 = static_cast(read_navigation_unsigned(data_jk_bits, RS_IODNAV_LSBS)); - DLOG(INFO) << "IODnav 2 LSBs in Word type 20: " << static_cast(IODnav_LSB20); - int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; - for (size_t i = 1; i < INAV_RS_SUBVECTOR_LENGTH; i++) + if (enable_rs) { - gamma_octet_bits[0] = std::pair({start_bit, BITS_IN_OCTET}); - gamma_rs3[i] = static_cast(read_navigation_unsigned(data_jk_bits, gamma_octet_bits)); - start_bit += BITS_IN_OCTET; + IODnav_LSB20 = read_octet_unsigned(data_jk_bits, RS_IODNAV_LSBS); + DLOG(INFO) << "IODnav 2 LSBs in Word type 20: " << static_cast(IODnav_LSB20); + if (IODnav_LSB20 != static_cast((current_IODnav % 4))) + { + // IODnav changed, reset buffers + current_IODnav = 0; + rs_parity_vector = std::vector(INAV_RS_PARITY_VECTOR_LENGTH, 0); + } + // Store RS parity vector gamma_{RS,4} + std::vector> gamma_octet_bits({{FIRST_RS_BIT, BITS_IN_OCTET}}); + rs_parity_vector[3 * INAV_RS_SUBVECTOR_LENGTH] = read_octet_unsigned(data_jk_bits, gamma_octet_bits); + int32_t start_bit = FIRST_RS_BIT_AFTER_IODNAV; + for (size_t i = 3 * INAV_RS_SUBVECTOR_LENGTH + 1; i < 4 * INAV_RS_SUBVECTOR_LENGTH; i++) + { + gamma_octet_bits[0] = std::pair({start_bit, BITS_IN_OCTET}); + rs_parity_vector[i] = read_octet_unsigned(data_jk_bits, gamma_octet_bits); + start_bit += BITS_IN_OCTET; + } } break; } diff --git a/src/core/system_parameters/galileo_inav_message.h b/src/core/system_parameters/galileo_inav_message.h index b5defdd34..93725af1f 100644 --- a/src/core/system_parameters/galileo_inav_message.h +++ b/src/core/system_parameters/galileo_inav_message.h @@ -44,7 +44,7 @@ class Galileo_Inav_Message { public: - Galileo_Inav_Message() = default; + Galileo_Inav_Message(); /* * \brief Takes in input a page (Odd or Even) of 120 bit, split it according ICD 4.3.2.3 and join Data_k with Data_j @@ -191,12 +191,25 @@ public: SV_ID_PRN_4 = prn; } + /* + * \brief Enable Reed-Solomon in Galileo E1B + */ + inline void enable_reed_solomon() + { + enable_rs = true; + } + private: - bool CRC_test(std::bitset bits, uint32_t checksum) const; - bool read_navigation_bool(std::bitset bits, const std::vector>& parameter) const; - uint64_t read_navigation_unsigned(std::bitset bits, const std::vector>& parameter) const; - uint64_t read_page_type_unsigned(std::bitset bits, const std::vector>& parameter) const; - int64_t read_navigation_signed(std::bitset bits, const std::vector>& parameter) const; + bool CRC_test(const std::bitset& bits, uint32_t checksum) const; + bool read_navigation_bool(const std::bitset& bits, const std::vector>& parameter) const; + uint64_t read_navigation_unsigned(const std::bitset& bits, const std::vector>& parameter) const; + uint64_t read_page_type_unsigned(const std::bitset& bits, const std::vector>& parameter) const; + int64_t read_navigation_signed(const std::bitset& bits, const std::vector>& parameter) const; + uint8_t read_octet_unsigned(const std::bitset& bits, const std::vector>& parameter) const; + void read_page_1(const std::bitset& data_bits); + void read_page_2(const std::bitset& data_bits); + void read_page_3(const std::bitset& data_bits); + void read_page_4(const std::bitset& data_bits); std::string page_Even{}; @@ -349,19 +362,21 @@ private: double ced_af0red{}; double ced_af1red{}; + double Galileo_satClkDrift{}; + + int32_t current_IODnav{}; + + // Word types 1, 2, 3, 4: Reed-Solomon information vector + std::vector rs_info_vector; + // Word types 17, 18, 19, 20: Reed-Solomon parity vector - std::vector gamma_rs0{INAV_RS_SUBVECTOR_LENGTH, 0}; - std::vector gamma_rs1{INAV_RS_SUBVECTOR_LENGTH, 0}; - std::vector gamma_rs2{INAV_RS_SUBVECTOR_LENGTH, 0}; - std::vector gamma_rs3{INAV_RS_SUBVECTOR_LENGTH, 0}; - std::vector rs_parity_vector{INAV_RS_PARITY_VECTOR_LENGTH, 0}; + std::vector rs_parity_vector; + uint8_t IODnav_LSB17{}; uint8_t IODnav_LSB18{}; uint8_t IODnav_LSB19{}; uint8_t IODnav_LSB20{}; - double Galileo_satClkDrift{}; - bool flag_CRC_test{}; bool flag_all_ephemeris{}; // Flag indicating that all words containing ephemeris have been received bool flag_ephemeris_1{}; // Flag indicating that ephemeris 1/4 (word 1) have been received @@ -387,6 +402,7 @@ private: bool flag_GGTO_4{}; bool flag_CED{}; + bool enable_rs{}; };