From 531ca63398c9c6cb3689d784cf7930797d61e85f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 17 Sep 2021 13:55:09 +0200 Subject: [PATCH] Make the code more portable Avoid arrays with variable length, replace them by std::vector --- src/core/libs/galileo_e6_has_msg_receiver.cc | 124 ++++++++++--------- src/core/libs/galileo_e6_has_msg_receiver.h | 3 + src/core/system_parameters/reed_solomon.cc | 29 ++--- 3 files changed, 83 insertions(+), 73 deletions(-) diff --git a/src/core/libs/galileo_e6_has_msg_receiver.cc b/src/core/libs/galileo_e6_has_msg_receiver.cc index 11bd9bcbd..f0981f7b8 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.cc +++ b/src/core/libs/galileo_e6_has_msg_receiver.cc @@ -335,6 +335,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(INFO) << "Nsys " << static_cast(d_HAS_data.Nsys); + d_nsys_in_mask[d_HAS_data.header.mask_id] = d_HAS_data.Nsys; if (d_HAS_data.Nsys != 0) { message = std::string(message.begin() + HAS_MSG_NSYS_LENGTH, message.end()); @@ -349,6 +350,7 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) { d_HAS_data.gnss_id_mask[i] = read_has_message_body_uint8(message.substr(0, HAS_MSG_ID_MASK_LENGTH)); DLOG(INFO) << "GNSS ID" << static_cast(i) << ": " << static_cast(d_HAS_data.gnss_id_mask[i]); + d_gnss_id_in_mask[d_HAS_data.header.mask_id][i] = 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); @@ -387,66 +389,74 @@ void galileo_e6_has_msg_receiver::read_MT1_body(const std::string& message_body) 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()); } + d_nsat_in_mask_id[d_HAS_data.header.mask_id] = Nsat; + } + } + else + { + // Take data from a previously received mask with the same mask_id + Nsat = d_nsat_in_mask_id[d_HAS_data.header.mask_id]; + d_HAS_data.gnss_id_mask = d_gnss_id_in_mask[d_HAS_data.header.mask_id]; + d_HAS_data.Nsys = d_nsys_in_mask[d_HAS_data.header.mask_id]; + } + + if (d_HAS_data.header.orbit_correction_flag && (Nsat != 0) && std::count(d_HAS_data.gnss_id_mask.begin(), d_HAS_data.gnss_id_mask.end(), 1) != 0) + { + // 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.orbit_correction_flag && (Nsat != 0)) - // { - // // 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_fullset_flag && (Nsat != 0) && d_HAS_data.Nsys != 0 && std::count(d_HAS_data.gnss_id_mask.begin(), d_HAS_data.gnss_id_mask.end(), 1) != 0) + { + // 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) // { diff --git a/src/core/libs/galileo_e6_has_msg_receiver.h b/src/core/libs/galileo_e6_has_msg_receiver.h index 2f7fc8bfb..86956cc33 100644 --- a/src/core/libs/galileo_e6_has_msg_receiver.h +++ b/src/core/libs/galileo_e6_has_msg_receiver.h @@ -87,6 +87,9 @@ private: 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_pids{32, std::vector()}; + std::vector d_nsat_in_mask_id{32, 0}; + std::vector> d_gnss_id_in_mask{32, std::vector(16)}; + std::vector d_nsys_in_mask{32}; bool d_new_message{}; bool d_enable_navdata_monitor{}; }; diff --git a/src/core/system_parameters/reed_solomon.cc b/src/core/system_parameters/reed_solomon.cc index 4d3425e90..32da88932 100644 --- a/src/core/system_parameters/reed_solomon.cc +++ b/src/core/system_parameters/reed_solomon.cc @@ -794,14 +794,14 @@ int ReedSolomon::decode_rs_8(uint8_t* data, const int* eras_pos, int no_eras) co uint8_t den; uint8_t discr_r; - uint8_t lambda[d_nroots + 1]; // Err+Eras Locator poly - uint8_t s[d_nroots]; // syndrome poly - uint8_t b[d_nroots + 1]; - uint8_t t[d_nroots + 1]; - uint8_t omega[d_nroots + 1]; - uint8_t root[d_nroots]; - uint8_t reg[d_nroots + 1]; - uint8_t loc[d_nroots]; + std::vector lambda(d_nroots + 1); // Err+Eras Locator poly + std::vector s(d_nroots); // syndrome poly + std::vector b(d_nroots + 1); + std::vector t(d_nroots + 1); + std::vector omega(d_nroots + 1); + std::vector root(d_nroots); + std::vector reg(d_nroots + 1); + std::vector loc(d_nroots); // Syndrome computation // form the syndromes; i.e., evaluate data(x) at roots of g(x) @@ -840,7 +840,6 @@ int ReedSolomon::decode_rs_8(uint8_t* data, const int* eras_pos, int no_eras) co return 0; } - memset(&lambda[1], 0, d_nroots * sizeof(lambda[0])); lambda[0] = 1; if (no_eras > 0) @@ -884,9 +883,8 @@ int ReedSolomon::decode_rs_8(uint8_t* data, const int* eras_pos, int no_eras) co discr_r = d_index_of[discr_r]; // Index form if (discr_r == d_a0) { - // 2 lines below: B(x) <-- x*B(x) - memmove(&b[1], b, d_nroots * sizeof(b[0])); - b[0] = d_a0; + // line below: B(x) <-- x*B(x) + b.insert(b.begin(), d_a0); } else { @@ -914,11 +912,10 @@ int ReedSolomon::decode_rs_8(uint8_t* data, const int* eras_pos, int no_eras) co } else { - // 2 lines below: B(x) <-- x*B(x) - memmove(&b[1], b, d_nroots * sizeof(b[0])); - b[0] = d_a0; + // line below: B(x) <-- x*B(x) + b.insert(b.begin(), d_a0); } - memcpy(lambda, t, (d_nroots + 1) * sizeof(t[0])); + memcpy(&lambda[0], t.data(), (d_nroots + 1) * sizeof(t[0])); } }