mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-10-31 23:26:22 +00:00
Code Issues Releases Wiki Activity

Improve value reading in navigation message decoding

Browse Source 
More concise code, less branching, fix some Wshorten-64-to-32 warnings
This commit is contained in:
Carles Fernandez 2024-01-13 12:07:06 +01:00
parent 5d5cbc05eb
commit 3697bc1dce
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
6 changed files with 61 additions and 244 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/core/system_parameters/beidou_dnav_navigation_message.cc
View File

@ -21,7 +21,7 @@
#include <cmath> // for cos, sin, fmod, sqrt, atan2, fabs, floor
#include <iostream> // for string, operator<<, cout, ostream
#include <limits> // for std::numeric_limits
#include <numeric> // 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<BEIDOU_DNAV_SUBFRAME_DATA_BITS>& bits,
const std::vector<std::pair<int32_t, int32_t>>& 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<std::pair<int32_t, int32_t>>& 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<uint64_t>(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<BEIDOU_DNAV_SUBFRAME_DATA_BITS>& bits,
const std::vector<std::pair<int32_t, int32_t>>& 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<int64_t>(bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - param.first - j]);
}
}
return value;

35
src/core/system_parameters/galileo_fnav_message.cc
View File

@ -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<GALILEO_FNAV_DATA_FRAME_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[i].first - j]) == 1)
{
value += 1; // insert the bit
}
value |= static_cast<uint64_t>(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<GALILE
int64_t Galileo_Fnav_Message::read_navigation_signed(const std::bitset<GALILEO_FNAV_DATA_FRAME_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
int64_t value = 0LL;
const int num_of_slices = parameter.size();
// read the MSB and perform the sign extension
if (static_cast<int>(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<int>(bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[i].first - j]) == 1)
{
value += 1; // insert the bit
}
value = (value << 1) | static_cast<int64_t>(bits[GALILEO_FNAV_DATA_FRAME_BITS - p.first - j]);
}
}
return value;

67
src/core/system_parameters/galileo_inav_message.cc
View File

@ -74,16 +74,12 @@ bool Galileo_Inav_Message::CRC_test(const std::bitset<GALILEO_DATA_FRAME_BITS>&
uint64_t Galileo_Inav_Message::read_navigation_unsigned(const std::bitset<GALILEO_DATA_JK_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(bits[GALILEO_DATA_JK_BITS - parameter[i].first - j]) == 1)
{
value += 1; // insert the bit
}
value |= static_cast<uint64_t>(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<GALILE
uint8_t Galileo_Inav_Message::read_octet_unsigned(const std::bitset<GALILEO_DATA_JK_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(bits[GALILEO_DATA_JK_BITS - parameter[i].first - j]) == 1)
{
value += 1; // insert the bit
}
value |= static_cast<uint8_t>(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<GALILEO_DATA
uint64_t Galileo_Inav_Message::read_page_type_unsigned(const std::bitset<GALILEO_PAGE_TYPE_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(bits[GALILEO_PAGE_TYPE_BITS - parameter[i].first - j]) == 1)
{
value += 1ULL; // insert the bit
}
value <<= 1; // shift left
value |= static_cast<uint64_t>(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<GALILEO
int64_t Galileo_Inav_Message::read_navigation_signed(const std::bitset<GALILEO_DATA_JK_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(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<int>(bits[GALILEO_DATA_JK_BITS - parameter[i].first - j]) == 1)
{
value += 1LL; // insert the bit
}
value = (value << 1) | static_cast<int64_t>(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<GALILEO_D
bool Galileo_Inav_Message::read_navigation_bool(const std::bitset<GALILEO_DATA_JK_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
bool value;
if (static_cast<int>(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;
}

46
src/core/system_parameters/glonass_gnav_navigation_message.cc
View File

@ -156,16 +156,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
bool Glonass_Gnav_Navigation_Message::read_navigation_bool(const std::bitset<GLONASS_GNAV_STRING_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<GLO
uint64_t Glonass_Gnav_Navigation_Message::read_navigation_unsigned(const std::bitset<GLONASS_GNAV_STRING_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<uint64_t>(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<GLONASS_GNAV_STRING_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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);

47
src/core/system_parameters/gps_cnav_navigation_message.cc
View File

@ -36,16 +36,7 @@ Gps_CNAV_Navigation_Message::Gps_CNAV_Navigation_Message()
bool Gps_CNAV_Navigation_Message::read_navigation_bool(const std::bitset<GPS_CNAV_DATA_PAGE_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
bool value;
if (static_cast<int>(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<GPS_CNA
uint64_t Gps_CNAV_Navigation_Message::read_navigation_unsigned(const std::bitset<GPS_CNAV_DATA_PAGE_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[i].first - j]) == 1)
{
value += 1ULL; // insert the bit
}
value = (value << 1) | static_cast<uint64_t>(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<GPS_CNAV_DATA_PAGE_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(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<int>(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[i].first - j]) == 1)
{
value += 1LL; // insert the bit
}
value = (value << 1) | static_cast<int64_t>(bits[GPS_CNAV_DATA_PAGE_BITS - p.first - j]);
}
}
return value;

58
src/core/system_parameters/gps_navigation_message.cc
View File

@ -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<GPS_SUBFRAME_BITS>& bits, const std::vector<std::pair<int32_t, int32_t> /*unused*/>& parameter) const
bool Gps_Navigation_Message::read_navigation_bool(const std::bitset<GPS_SUBFRAME_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
bool value;
if (static_cast<int>(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<GPS_SUBFRAME_BITS>& bits, const std::vector<std::pair<int32_t, int32_t> /*unused*/>& parameter) const
uint64_t Gps_Navigation_Message::read_navigation_unsigned(const std::bitset<GPS_SUBFRAME_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(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<GPS_SUBFRAME_BITS>& bits, const std::vector<std::pair<int32_t, int32_t> /*unused*/>& parameter) const
int64_t Gps_Navigation_Message::read_navigation_signed(const std::bitset<GPS_SUBFRAME_BITS>& bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(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<int>(bits[GPS_SUBFRAME_BITS - parameter[i].first - j]) == 1)
{
value += 1LL; // insert the bit
}
value = (value << 1) | static_cast<int64_t>(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;
}