diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 6ca378e60..472ffc9d4 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -496,7 +496,8 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ if (rtk_.ssat[i].vsat[0] == 1) used_sats++; } - double azel[used_sats * 2]; + std::vector azel; + azel.reserve(used_sats * 2); unsigned int index_aux = 0; for (unsigned int i = 0; i < MAXSAT; i++) { @@ -507,7 +508,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ index_aux++; } } - if (index_aux > 0) dops(index_aux, azel, 0.0, dop_); + if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_); this->set_valid_position(true); arma::vec rx_position_and_time(4); diff --git a/src/algorithms/libs/galileo_e1_signal_processing.cc b/src/algorithms/libs/galileo_e1_signal_processing.cc index e4b589a9d..23580afc7 100644 --- a/src/algorithms/libs/galileo_e1_signal_processing.cc +++ b/src/algorithms/libs/galileo_e1_signal_processing.cc @@ -33,6 +33,7 @@ #include "galileo_e1_signal_processing.h" #include "Galileo_E1.h" #include "gnss_signal_processing.h" +#include #include @@ -42,7 +43,7 @@ void galileo_e1_code_gen_int(int* _dest, char _Signal[3], int32_t _prn) int32_t prn = _prn - 1; int32_t index = 0; - /* A simple error check */ + // A simple error check if ((_prn < 1) || (_prn > 50)) { return; @@ -107,7 +108,7 @@ void galileo_e1_sinboc_61_gen_int(int* _dest, int* _prn, uint32_t _length_out) void galileo_e1_code_gen_sinboc11_float(float* _dest, char _Signal[3], uint32_t _prn) { std::string _galileo_signal = _Signal; - uint32_t _codeLength = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + uint32_t _codeLength = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); int32_t primary_code_E1_chips[4092]; // _codeLength not accepted by Clang galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); //generate Galileo E1 code, 1 sample per chip for (uint32_t i = 0; i < _codeLength; i++) @@ -157,15 +158,16 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], // This function is based on the GNU software GPS for MATLAB in Kay Borre's book std::string _galileo_signal = _Signal; uint32_t _samplesPerCode; - const int32_t _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ; //Hz - uint32_t _codeLength = Galileo_E1_B_CODE_LENGTH_CHIPS; - int32_t primary_code_E1_chips[static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS)]; + const int32_t _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ; // Hz + uint32_t _codeLength = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS); + int32_t* primary_code_E1_chips = static_cast(volk_gnsssdr_malloc(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(int32_t), volk_gnsssdr_get_alignment())); + _samplesPerCode = static_cast(static_cast(_fs) / (static_cast(_codeFreqBasis) / static_cast(_codeLength))); const int32_t _samplesPerChip = (_cboc == true) ? 12 : 2; const uint32_t delay = ((static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS) - _chip_shift) % static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS)) * _samplesPerCode / Galileo_E1_B_CODE_LENGTH_CHIPS; - galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); //generate Galileo E1 code, 1 sample per chip + galileo_e1_code_gen_int(primary_code_E1_chips, _Signal, _prn); // generate Galileo E1 code, 1 sample per chip float* _signal_E1; @@ -174,24 +176,26 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], if (_cboc == true) { - galileo_e1_gen_float(_signal_E1, primary_code_E1_chips, _Signal); //generate cboc 12 samples per chip + galileo_e1_gen_float(_signal_E1, primary_code_E1_chips, _Signal); // generate cboc 12 samples per chip } else { - int32_t _signal_E1_int[_codeLength]; - galileo_e1_sinboc_11_gen_int(_signal_E1_int, primary_code_E1_chips, _codeLength); //generate sinboc(1,1) 2 samples per chip + int32_t* _signal_E1_int = static_cast(volk_gnsssdr_malloc(_codeLength * sizeof(int32_t), volk_gnsssdr_get_alignment())); + galileo_e1_sinboc_11_gen_int(_signal_E1_int, primary_code_E1_chips, _codeLength); // generate sinboc(1,1) 2 samples per chip for (uint32_t ii = 0; ii < _codeLength; ++ii) { _signal_E1[ii] = static_cast(_signal_E1_int[ii]); } + volk_gnsssdr_free(_signal_E1_int); } if (_fs != _samplesPerChip * _codeFreqBasis) { float* _resampled_signal = new float[_samplesPerCode]; + resampler(_signal_E1, _resampled_signal, _samplesPerChip * _codeFreqBasis, _fs, - _codeLength, _samplesPerCode); //resamples code to fs + _codeLength, _samplesPerCode); // resamples code to fs delete[] _signal_E1; _signal_E1 = _resampled_signal; @@ -221,6 +225,7 @@ void galileo_e1_code_gen_float_sampled(float* _dest, char _Signal[3], } delete[] _signal_E1; + volk_gnsssdr_free(primary_code_E1_chips); } @@ -229,7 +234,7 @@ void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signa bool _secondary_flag) { std::string _galileo_signal = _Signal; - const int32_t _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ; //Hz + const int32_t _codeFreqBasis = Galileo_E1_CODE_CHIP_RATE_HZ; // Hz uint32_t _samplesPerCode = static_cast(static_cast(_fs) / (static_cast(_codeFreqBasis) / static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS))); @@ -238,7 +243,7 @@ void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signa _samplesPerCode *= static_cast(Galileo_E1_C_SECONDARY_CODE_LENGTH); } - float real_code[_samplesPerCode]; + float* real_code = static_cast(volk_gnsssdr_malloc(_samplesPerCode * sizeof(float), volk_gnsssdr_get_alignment())); galileo_e1_code_gen_float_sampled(real_code, _Signal, _cboc, _prn, _fs, _chip_shift, _secondary_flag); @@ -246,6 +251,7 @@ void galileo_e1_code_gen_complex_sampled(std::complex* _dest, char _Signa { _dest[ii] = std::complex(real_code[ii], 0.0f); } + volk_gnsssdr_free(real_code); } diff --git a/src/algorithms/libs/gnss_signal_processing.cc b/src/algorithms/libs/gnss_signal_processing.cc index 52b0c25ea..0dea461e7 100644 --- a/src/algorithms/libs/gnss_signal_processing.cc +++ b/src/algorithms/libs/gnss_signal_processing.cc @@ -158,7 +158,7 @@ void hex_to_binary_converter(int32_t* _dest, char _from) } -void resampler(float* _from, float* _dest, float _fs_in, +void resampler(const float* _from, float* _dest, float _fs_in, float _fs_out, uint32_t _length_in, uint32_t _length_out) { uint32_t _codeValueIndex; @@ -182,7 +182,7 @@ void resampler(float* _from, float* _dest, float _fs_in, } -void resampler(std::complex* _from, std::complex* _dest, float _fs_in, +void resampler(const std::complex* _from, std::complex* _dest, float _fs_in, float _fs_out, uint32_t _length_in, uint32_t _length_out) { uint32_t _codeValueIndex; diff --git a/src/algorithms/libs/gnss_signal_processing.h b/src/algorithms/libs/gnss_signal_processing.h index 6a08d942e..3ed363e3e 100644 --- a/src/algorithms/libs/gnss_signal_processing.h +++ b/src/algorithms/libs/gnss_signal_processing.h @@ -65,14 +65,14 @@ void hex_to_binary_converter(int32_t* _dest, char _from); * \brief This function resamples a sequence of float values. * */ -void resampler(float* _from, float* _dest, +void resampler(const float* _from, float* _dest, float _fs_in, float _fs_out, uint32_t _length_in, uint32_t _length_out); /*! * \brief This function resamples a sequence of complex values. * */ -void resampler(std::complex* _from, std::complex* _dest, +void resampler(const std::complex* _from, std::complex* _dest, float _fs_in, float _fs_out, uint32_t _length_in, uint32_t _length_out); diff --git a/src/algorithms/libs/gps_sdr_signal_processing.cc b/src/algorithms/libs/gps_sdr_signal_processing.cc index 932c96ff2..b0a49fbd0 100644 --- a/src/algorithms/libs/gps_sdr_signal_processing.cc +++ b/src/algorithms/libs/gps_sdr_signal_processing.cc @@ -116,7 +116,7 @@ void gps_l1_ca_code_gen_int(int32_t* _dest, int32_t _prn, uint32_t _chip_shift) void gps_l1_ca_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift) { - uint32_t _code_length = 1023; + const uint32_t _code_length = 1023; int32_t ca_code_int[_code_length]; gps_l1_ca_code_gen_int(ca_code_int, _prn, _chip_shift); @@ -130,7 +130,7 @@ void gps_l1_ca_code_gen_float(float* _dest, int32_t _prn, uint32_t _chip_shift) void gps_l1_ca_code_gen_complex(std::complex* _dest, int32_t _prn, uint32_t _chip_shift) { - uint32_t _code_length = 1023; + const uint32_t _code_length = 1023; int32_t ca_code_int[_code_length]; gps_l1_ca_code_gen_int(ca_code_int, _prn, _chip_shift); diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index 5248c0f7c..7748c6a71 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -30,7 +30,6 @@ */ - #include "edc.h" #include "bits.h" #include "cnav_msg.h" @@ -48,26 +47,26 @@ * Block Viterbi decoding parameters. */ /** Viterbi decoder reversed polynomial A */ -#define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/ +#define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/ /** Viterbi decoder reversed polynomial B */ -#define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */ +#define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */ /* * GPS L2C message constants. */ /** GPS L2C preamble */ -#define GPS_CNAV_PREAMBLE1 (0b10001011u) +const u32 GPS_CNAV_PREAMBLE1 = 0x989A73u; /** Inverted GPS L2C preamble */ -#define GPS_CNAV_PREAMBLE2 (0b01110100u) +const u32 GPS_CNAV_PREAMBLE2 = 0x010F054u; /** GPS L2C preamble length in bits */ -#define GPS_CNAV_PREAMBLE_LENGTH (8) +#define GPS_CNAV_PREAMBLE_LENGTH (8) /** GPS L2C CNAV message length in bits */ -#define GPS_CNAV_MSG_LENGTH (300) +#define GPS_CNAV_MSG_LENGTH (300) /** GPS LC2 CNAV CRC length in bits */ -#define GPS_CNAV_MSG_CRC_LENGTH (24) +#define GPS_CNAV_MSG_CRC_LENGTH (24) /** GPS L2C CNAV message payload length in bits */ -#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH - GPS_CNAV_MSG_CRC_LENGTH) +#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH - GPS_CNAV_MSG_CRC_LENGTH) /** GPS L2C CNAV message lock detector threshold */ #define GPS_CNAV_LOCK_MAX_CRC_FAILS (10) @@ -85,7 +84,7 @@ static u32 _cnav_compute_crc(cnav_v27_part_t *part) { u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH, - part->invert); + part->invert); return crc; } @@ -104,7 +103,7 @@ static u32 _cnav_compute_crc(cnav_v27_part_t *part) static u32 _cnav_extract_crc(const cnav_v27_part_t *part) { u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH, - GPS_CNAV_MSG_CRC_LENGTH); + GPS_CNAV_MSG_CRC_LENGTH); if (part->invert) { crc ^= 0xFFFFFF; @@ -152,7 +151,7 @@ static void _cnav_rescan_preamble(cnav_v27_part_t *part) if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH) { bitshl(part->decoded, sizeof(part->decoded), - part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1); + part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1); part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1; } } @@ -200,11 +199,12 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) * - N - Number of bits to put into decoded buffer * - M - Number of bits in the tail to ignore. */ - unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS + - CHAR_BIT - 1) / CHAR_BIT]; + unsigned char tmp_bits[(GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS + + CHAR_BIT - 1) / + CHAR_BIT]; v27_chainback_likely(&part->dec, tmp_bits, - GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); + GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); /* Read decoded bits and add them to the decoded buffer */ bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS); @@ -238,10 +238,9 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) } if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded) { - /* We have collected 300 bits starting from message preamble. Now try * to compute CRC-24Q */ - u32 crc = _cnav_compute_crc(part); + u32 crc = _cnav_compute_crc(part); u32 crc2 = _cnav_extract_crc(part); if (part->message_lock) @@ -260,8 +259,8 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS) { /* CRC has failed too many times - drop the lock. */ - part->n_crc_fail = 0; - part->message_lock = false; + part->n_crc_fail = 0; + part->message_lock = false; part->preamble_seen = false; /* Try to find a new preamble, reuse data from buffer. */ retry = true; @@ -272,8 +271,8 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) { /* CRC match - message can be decoded */ part->message_lock = true; - part->crc_ok = true; - part->n_crc_fail = 0; + part->crc_ok = true; + part->n_crc_fail = 0; } else { @@ -346,13 +345,13 @@ static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay) _cnav_msg_invert(part); } - msg->prn = getbitu(part->decoded, 8, 6); + msg->prn = getbitu(part->decoded, 8, 6); msg->msg_id = getbitu(part->decoded, 14, 6); - msg->tow = getbitu(part->decoded, 20, 17); - msg->alert = getbitu(part->decoded, 37, 1) ? true : false; + msg->tow = getbitu(part->decoded, 20, 17); + msg->alert = getbitu(part->decoded, 37, 1) ? true : false; /* copy RAW message for GNSS-SDR */ - memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); + memcpy(msg->raw_msg, part->decoded, GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); *delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) * 2 + part->n_symbols; @@ -388,15 +387,15 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec) { memset(dec, 0, sizeof(*dec)); v27_init(&dec->part1.dec, - dec->part1.decisions, - GPS_L2_V27_HISTORY_LENGTH_BITS, - cnav_msg_decoder_get_poly(), - 0); + dec->part1.decisions, + GPS_L2_V27_HISTORY_LENGTH_BITS, + cnav_msg_decoder_get_poly(), + 0); v27_init(&dec->part2.dec, - dec->part2.decisions, - GPS_L2_V27_HISTORY_LENGTH_BITS, - cnav_msg_decoder_get_poly(), - 0); + dec->part2.decisions, + GPS_L2_V27_HISTORY_LENGTH_BITS, + cnav_msg_decoder_get_poly(), + 0); dec->part1.init = true; dec->part2.init = true; _cnav_add_symbol(&dec->part2, 0x80); @@ -426,9 +425,9 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec) * \retval false More data is required. */ bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec, - u8 symbol, - cnav_msg_t *msg, - u32 *pdelay) + u8 symbol, + cnav_msg_t *msg, + u32 *pdelay) { _cnav_add_symbol(&dec->part1, symbol); _cnav_add_symbol(&dec->part2, symbol); @@ -470,7 +469,7 @@ const v27_poly_t *cnav_msg_decoder_get_poly(void) if (!initialized) { /* Coefficients for polynomial object */ - const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B }; + const signed char coeffs[2] = {GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B}; /* Racing condition handling: the data can be potential initialized more * than once in case multiple threads request concurrent access. However, diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 2a017088e..24d3417ce 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -189,7 +189,9 @@ std::string Rtcm::bin_to_binary_data(const std::string& s) const { std::string s_aux; int32_t remainder = static_cast(std::fmod(s.length(), 8)); - uint8_t c[s.length()]; + std::vector c; + c.reserve(s.length()); + uint32_t k = 0; if (remainder != 0) { @@ -213,7 +215,7 @@ std::string Rtcm::bin_to_binary_data(const std::string& s) const k++; } - std::string ret(c, c + k / sizeof(c[0])); + std::string ret(c.begin(), c.begin() + k); return ret; }