diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.cc b/src/core/system_parameters/beidou_dnav_navigation_message.cc index f1a0fc1b1..0444a3555 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.cc +++ b/src/core/system_parameters/beidou_dnav_navigation_message.cc @@ -21,7 +21,7 @@ #include // for cos, sin, fmod, sqrt, atan2, fabs, floor #include // for string, operator<<, cout, ostream #include // for std::numeric_limits - +#include // for accumulate Beidou_Dnav_Navigation_Message::Beidou_Dnav_Navigation_Message() { @@ -30,9 +30,6 @@ Beidou_Dnav_Navigation_Message::Beidou_Dnav_Navigation_Message() for (uint32_t i = 1; i < 64; i++) { satelliteBlock[i] = gnss_sat.what_block(_system, i); - } - for (uint32_t i = 1; i < 64; i++) - { almanacHealth[i] = 0; } } @@ -48,16 +45,7 @@ bool Beidou_Dnav_Navigation_Message::read_navigation_bool( const std::bitset& bits, const std::vector>& parameter) const { - bool value; - - if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) - { - value = true; - } - else - { - value = false; - } + bool value = bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first]; return value; } @@ -67,16 +55,11 @@ uint64_t Beidou_Dnav_Navigation_Message::read_navigation_unsigned( const std::vector>& parameter) const { uint64_t value = 0ULL; - const int32_t num_of_slices = parameter.size(); - for (int32_t i = 0; i < num_of_slices; i++) + for (const auto& param : parameter) { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < param.second; j++) { - value <<= 1U; // shift left - if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[i].first - j] == 1) - { - value += 1U; // insert the bit - } + value = (value << 1U) | static_cast(bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - param.first - j]); } } return value; @@ -87,29 +70,12 @@ int64_t Beidou_Dnav_Navigation_Message::read_navigation_signed( const std::bitset& bits, const std::vector>& parameter) const { - int64_t value = 0; - const int32_t num_of_slices = parameter.size(); - - // read the MSB and perform the sign extension - if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) + int64_t value = bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] ? -1LL : 0LL; + for (const auto& param : parameter) { - value ^= 0xFFFFFFFFFFFFFFFF; // 64 bits variable - } - else - { - value &= 0; - } - - for (int32_t i = 0; i < num_of_slices; i++) - { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < param.second; j++) { - value *= 2; // shift left the signed integer - value &= 0xFFFFFFFFFFFFFFFE; // reset the corresponding bit (for the 64 bits variable) - if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[i].first - j] == 1) - { - value += 1; // insert the bit - } + value = (value << 1) | static_cast(bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - param.first - j]); } } return value; diff --git a/src/core/system_parameters/galileo_fnav_message.cc b/src/core/system_parameters/galileo_fnav_message.cc index 1d313ceed..5dea81898 100644 --- a/src/core/system_parameters/galileo_fnav_message.cc +++ b/src/core/system_parameters/galileo_fnav_message.cc @@ -282,16 +282,12 @@ void Galileo_Fnav_Message::decode_page(const std::string& data) uint64_t Galileo_Fnav_Message::read_navigation_unsigned(const std::bitset& bits, const std::vector>& parameter) const { uint64_t value = 0ULL; - const int num_of_slices = parameter.size(); - for (int i = 0; i < num_of_slices; i++) + for (const auto& p : parameter) { - for (int j = 0; j < parameter[i].second; j++) + for (int j = 0; j < p.second; j++) { value <<= 1U; // shift left - if (static_cast(bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[i].first - j]) == 1) - { - value += 1; // insert the bit - } + value |= static_cast(bits[GALILEO_FNAV_DATA_FRAME_BITS - p.first - j]); } } return value; @@ -300,29 +296,12 @@ uint64_t Galileo_Fnav_Message::read_navigation_unsigned(const std::bitset& bits, const std::vector>& parameter) const { - int64_t value = 0LL; - const int num_of_slices = parameter.size(); - - // read the MSB and perform the sign extension - if (static_cast(bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[0].first]) == 1) + int64_t value = (bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[0].first] == 1) ? -1LL : 0LL; + for (const auto& p : parameter) { - value ^= 0x0FFFFFFFFFFFFFFF; // 64 bits variable - } - else - { - value &= 0; - } - - for (int i = 0; i < num_of_slices; i++) - { - for (int j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < p.second; j++) { - value <<= 1; // shift left - value &= 0xFFFFFFFFFFFFFFFE; // reset the corresponding bit (for the 64 bits variable) - if (static_cast(bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[i].first - j]) == 1) - { - value += 1; // insert the bit - } + value = (value << 1) | static_cast(bits[GALILEO_FNAV_DATA_FRAME_BITS - p.first - j]); } } return value; diff --git a/src/core/system_parameters/galileo_inav_message.cc b/src/core/system_parameters/galileo_inav_message.cc index 850f2ffb9..81c47c3bb 100644 --- a/src/core/system_parameters/galileo_inav_message.cc +++ b/src/core/system_parameters/galileo_inav_message.cc @@ -74,16 +74,12 @@ bool Galileo_Inav_Message::CRC_test(const std::bitset& 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(); - for (int32_t i = 0; i < num_of_slices; i++) + for (const auto& p : parameter) { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int j = 0; j < p.second; j++) { value <<= 1U; // shift left - if (static_cast(bits[GALILEO_DATA_JK_BITS - parameter[i].first - j]) == 1) - { - value += 1; // insert the bit - } + value |= static_cast(bits[GALILEO_DATA_JK_BITS - p.first - j]); } } return value; @@ -93,16 +89,12 @@ uint64_t Galileo_Inav_Message::read_navigation_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 (const auto& p : parameter) { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int j = 0; j < p.second; j++) { value <<= 1; // shift left - if (static_cast(bits[GALILEO_DATA_JK_BITS - parameter[i].first - j]) == 1) - { - value += 1; // insert the bit - } + value |= static_cast(bits[GALILEO_DATA_JK_BITS - p.first - j]); } } return value; @@ -112,16 +104,12 @@ uint8_t Galileo_Inav_Message::read_octet_unsigned(const std::bitset& bits, const std::vector>& parameter) const { uint64_t value = 0ULL; - const int32_t num_of_slices = parameter.size(); - for (int32_t i = 0; i < num_of_slices; i++) + for (const auto& p : parameter) { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int j = 0; j < p.second; j++) { - value <<= 1U; // shift left - if (static_cast(bits[GALILEO_PAGE_TYPE_BITS - parameter[i].first - j]) == 1) - { - value += 1ULL; // insert the bit - } + value <<= 1; // shift left + value |= static_cast(bits[GALILEO_PAGE_TYPE_BITS - p.first - j]); } } return value; @@ -130,29 +118,12 @@ uint64_t Galileo_Inav_Message::read_page_type_unsigned(const std::bitset& bits, const std::vector>& parameter) const { - int64_t value = 0LL; - const int32_t num_of_slices = parameter.size(); - - // read the MSB and perform the sign extension - if (static_cast(bits[GALILEO_DATA_JK_BITS - parameter[0].first]) == 1) + int64_t value = (bits[GALILEO_DATA_JK_BITS - parameter[0].first] == 1) ? -1LL : 0LL; + for (const auto& p : parameter) { - value ^= 0xFFFFFFFFFFFFFFFFLL; // 64 bits variable - } - else - { - value &= 0LL; - } - - for (int32_t i = 0; i < num_of_slices; i++) - { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < p.second; j++) { - value *= 2; // shift left the signed integer - value &= 0xFFFFFFFFFFFFFFFE; // reset the corresponding bit (for the 64 bits variable) - if (static_cast(bits[GALILEO_DATA_JK_BITS - parameter[i].first - j]) == 1) - { - value += 1LL; // insert the bit - } + value = (value << 1) | static_cast(bits[GALILEO_DATA_JK_BITS - p.first - j]); } } return value; @@ -161,15 +132,7 @@ int64_t Galileo_Inav_Message::read_navigation_signed(const std::bitset& bits, const std::vector>& parameter) const { - bool value; - if (static_cast(bits[GALILEO_DATA_JK_BITS - parameter[0].first]) == 1) - { - value = true; - } - else - { - value = false; - } + bool value = bits[GALILEO_DATA_JK_BITS - parameter[0].first]; return value; } diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index 586f795b2..9428875c4 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -156,16 +156,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset& bits, const std::vector>& parameter) const { - bool value; - - if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) - { - value = true; - } - else - { - value = false; - } + bool value = bits[GLONASS_GNAV_STRING_BITS - parameter[0].first]; return value; } @@ -173,16 +164,11 @@ bool Glonass_Gnav_Navigation_Message::read_navigation_bool(const std::bitset& bits, const std::vector>& parameter) const { uint64_t value = 0ULL; - const int32_t num_of_slices = parameter.size(); - for (int32_t i = 0; i < num_of_slices; i++) + for (const auto& p : parameter) { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < p.second; j++) { - value <<= 1ULL; // shift left - if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1) - { - value += 1ULL; // insert the bit - } + value = (value << 1U) | static_cast(bits[GLONASS_GNAV_STRING_BITS - p.first - j]); } } return value; @@ -192,26 +178,14 @@ uint64_t Glonass_Gnav_Navigation_Message::read_navigation_unsigned(const std::bi int64_t Glonass_Gnav_Navigation_Message::read_navigation_signed(const std::bitset& bits, const std::vector>& parameter) const { int64_t value = 0LL; - int64_t sign = 0LL; - const int32_t num_of_slices = parameter.size(); - // read the MSB and perform the sign extension - if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) + int64_t sign = (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) ? -1LL : 1LL; + + // const int32_t num_of_slices = parameter.size(); + for (const auto& p : parameter) { - sign = -1LL; - } - else - { - sign = 1LL; - } - for (int32_t i = 0; i < num_of_slices; i++) - { - for (int32_t j = 1; j < parameter[i].second; j++) + for (int32_t j = 1; j < p.second; j++) { - value <<= 1; // shift left - if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1) - { - value += 1LL; // insert the bit - } + value = (value << 1) + bits[GLONASS_GNAV_STRING_BITS - p.first - j]; } } return (sign * value); diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index a644f7f2a..9b9b2e907 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -36,16 +36,7 @@ Gps_CNAV_Navigation_Message::Gps_CNAV_Navigation_Message() bool Gps_CNAV_Navigation_Message::read_navigation_bool(const std::bitset& bits, const std::vector>& parameter) const { - bool value; - - if (static_cast(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first]) == 1) - { - value = true; - } - else - { - value = false; - } + bool value = bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first]; return value; } @@ -53,16 +44,11 @@ bool Gps_CNAV_Navigation_Message::read_navigation_bool(const std::bitset& bits, const std::vector>& parameter) const { uint64_t value = 0ULL; - const int32_t num_of_slices = parameter.size(); - for (int32_t i = 0; i < num_of_slices; i++) + for (const auto& p : parameter) { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < p.second; j++) { - value <<= 1ULL; // shift left - if (static_cast(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[i].first - j]) == 1) - { - value += 1ULL; // insert the bit - } + value = (value << 1) | static_cast(bits[GPS_CNAV_DATA_PAGE_BITS - p.first - j]); } } return value; @@ -71,29 +57,12 @@ uint64_t Gps_CNAV_Navigation_Message::read_navigation_unsigned(const std::bitset int64_t Gps_CNAV_Navigation_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(); - - // read the MSB and perform the sign extension - if (static_cast(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first]) == 1) + int64_t value = (bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first] == 1) ? -1LL : 0LL; + for (const auto& p : parameter) { - value ^= 0xFFFFFFFFFFFFFFFFLL; // 64 bits variable - } - else - { - value &= 0LL; - } - - for (int32_t i = 0; i < num_of_slices; i++) - { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < p.second; j++) { - value *= 2; // shift left the signed integer - value &= 0xFFFFFFFFFFFFFFFELL; // reset the corresponding bit (for the 64 bits variable) - if (static_cast(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[i].first - j]) == 1) - { - value += 1LL; // insert the bit - } + value = (value << 1) | static_cast(bits[GPS_CNAV_DATA_PAGE_BITS - p.first - j]); } } return value; diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index d65520720..b17458da7 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -32,9 +32,6 @@ Gps_Navigation_Message::Gps_Navigation_Message() for (uint32_t i = 1; i < 33; i++) { satelliteBlock[i] = gnss_sat.what_block(_system, i); - } - for (int32_t i = 1; i < 33; i++) - { almanacHealth[i] = 0; } } @@ -46,66 +43,35 @@ void Gps_Navigation_Message::print_gps_word_bytes(uint32_t GPS_word) const } -bool Gps_Navigation_Message::read_navigation_bool(const std::bitset& bits, const std::vector /*unused*/>& parameter) const +bool Gps_Navigation_Message::read_navigation_bool(const std::bitset& bits, const std::vector>& parameter) const { - bool value; - - if (static_cast(bits[GPS_SUBFRAME_BITS - parameter[0].first]) == 1) - { - value = true; - } - else - { - value = false; - } + bool value = bits[GPS_SUBFRAME_BITS - parameter[0].first]; return value; } -uint64_t Gps_Navigation_Message::read_navigation_unsigned(const std::bitset& bits, const std::vector /*unused*/>& parameter) const +uint64_t Gps_Navigation_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(); - for (int32_t i = 0; i < num_of_slices; i++) + for (const auto& p : parameter) { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < p.second; ++j) { - value <<= 1; // shift left - if (static_cast(bits[GPS_SUBFRAME_BITS - parameter[i].first - j]) == 1) - { - value += 1ULL; // insert the bit - } + value = (value << 1) | (bits.test(GPS_SUBFRAME_BITS - p.first - j) ? 1 : 0); } } return value; } -int64_t Gps_Navigation_Message::read_navigation_signed(const std::bitset& bits, const std::vector /*unused*/>& parameter) const +int64_t Gps_Navigation_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(); - - // read the MSB and perform the sign extension - if (static_cast(bits[GPS_SUBFRAME_BITS - parameter[0].first]) == 1) + int64_t value = (bits[GPS_SUBFRAME_BITS - parameter[0].first] == 1) ? -1LL : 0LL; + for (const auto& p : parameter) { - value ^= 0xFFFFFFFFFFFFFFFFLL; // 64 bits variable - } - else - { - value &= 0LL; - } - - for (int32_t i = 0; i < num_of_slices; i++) - { - for (int32_t j = 0; j < parameter[i].second; j++) + for (int32_t j = 0; j < p.second; j++) { - value *= 2; // shift left the signed integer - value &= 0xFFFFFFFFFFFFFFFELL; // reset the corresponding bit (for the 64 bits variable) - if (static_cast(bits[GPS_SUBFRAME_BITS - parameter[i].first - j]) == 1) - { - value += 1LL; // insert the bit - } + value = (value << 1) | static_cast(bits[GPS_SUBFRAME_BITS - p.first - j]); } } return value; @@ -351,7 +317,7 @@ int32_t Gps_Navigation_Message::subframe_decoder(const char* subframe) default: break; - } // switch subframeID ... + } // switch subframeID return subframe_ID; }