diff --git a/src/algorithms/libs/gps_l5_signal.cc b/src/algorithms/libs/gps_l5_signal.cc index c1f234506..c5fc6e13c 100644 --- a/src/algorithms/libs/gps_l5_signal.cc +++ b/src/algorithms/libs/gps_l5_signal.cc @@ -32,13 +32,10 @@ #include "gps_l5_signal.h" #include "GPS_L5.h" -#include -#include -#include #include -std::deque l5i_xa_shift(std::deque xa) +std::deque l5i_xa_shift(std::deque xa) // GPS-IS-705E Figure 3-4 pp. 15 { if (xa == std::deque{true, true, true, true, true, true, true, true, true, true, true, false, true}) { @@ -62,7 +59,7 @@ std::deque l5q_xa_shift(std::deque xa) } -std::deque l5i_xb_shift(std::deque xb) +std::deque l5i_xb_shift(std::deque xb) // GPS-IS-705E Figure 3-5 pp. 16 { std::deque out(xb.begin(), xb.end() - 1); out.push_front(xb[12] xor xb[11] xor xb[7] xor xb[6] xor xb[5] xor xb[3] xor xb[2] xor xb[0]); @@ -146,7 +143,7 @@ void make_l5i(int32_t* _dest, int32_t prn) { xb_shift[n] = xb[(xb_offset + n) % GPS_L5I_CODE_LENGTH_CHIPS]; } - std::deque out_code(GPS_L5I_CODE_LENGTH_CHIPS, false); + for (int32_t n = 0; n < GPS_L5I_CODE_LENGTH_CHIPS; n++) { _dest[n] = xa[n] xor xb_shift[n]; @@ -166,7 +163,7 @@ void make_l5q(int32_t* _dest, int32_t prn) { xb_shift[n] = xb[(xb_offset + n) % GPS_L5Q_CODE_LENGTH_CHIPS]; } - std::deque out_code(GPS_L5Q_CODE_LENGTH_CHIPS, false); + for (int32_t n = 0; n < GPS_L5Q_CODE_LENGTH_CHIPS; n++) { _dest[n] = xa[n] xor xb_shift[n]; @@ -231,28 +228,24 @@ void gps_l5i_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, //--- Find time constants -------------------------------------------------- _ts = 1.0 / static_cast(_fs); // Sampling period in sec - _tc = 1.0 / static_cast(GPS_L5I_CODE_RATE_HZ); // C/A chip period in sec + _tc = 1.0 / static_cast(GPS_L5I_CODE_RATE_HZ); // L5I primary chip period in sec - //float aux; for (int32_t i = 0; i < _samplesPerCode; i++) { //=== Digitizing ======================================================= //--- Make index array to read L5 code values ------------------------- - //TODO: Check this formula! Seems to start with an extra sample - _codeValueIndex = std::ceil((_ts * (static_cast(i) + 1)) / _tc) - 1; - //aux = (_ts * (i + 1)) / _tc; - //_codeValueIndex = static_cast (static_cast(aux)) - 1; + _codeValueIndex = static_cast(std::ceil(_ts * static_cast(i + 1) / _tc)) - 1; - //--- Make the digitized version of the L2C code ----------------------- + //--- Make the digitized version of the L5I code ----------------------- if (i == _samplesPerCode - 1) { //--- Correct the last index (due to number rounding issues) ----------- - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeLength - 1], 0); + _dest[i] = std::complex(1.0 - 2.0 * _code[_codeLength - 1], 0.0); } else { - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample + _dest[i] = std::complex(1.0 - 2.0 * _code[_codeValueIndex], 0.0); // repeat the chip -> upsample } } delete[] _code; @@ -296,7 +289,7 @@ void gps_l5q_code_gen_float(float* _dest, uint32_t _prn) /* - * Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency + * Generates complex GPS L5Q code for the desired SV ID and sampled to specific sampling frequency */ void gps_l5q_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs) { @@ -316,7 +309,7 @@ void gps_l5q_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, //--- Find time constants -------------------------------------------------- _ts = 1.0 / static_cast(_fs); // Sampling period in sec - _tc = 1.0 / static_cast(GPS_L5Q_CODE_RATE_HZ); // C/A chip period in sec + _tc = 1.0 / static_cast(GPS_L5Q_CODE_RATE_HZ); // L5Q chip period in sec //float aux; for (int32_t i = 0; i < _samplesPerCode; i++) @@ -324,12 +317,9 @@ void gps_l5q_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, //=== Digitizing ======================================================= //--- Make index array to read L5 code values ------------------------- - //TODO: Check this formula! Seems to start with an extra sample - _codeValueIndex = std::ceil((_ts * (static_cast(i) + 1)) / _tc) - 1; - //aux = (_ts * (i + 1)) / _tc; - //_codeValueIndex = static_cast (static_cast(aux)) - 1; + _codeValueIndex = static_cast(std::ceil(_ts * static_cast(i + 1) / _tc)) - 1; - //--- Make the digitized version of the L2C code ----------------------- + //--- Make the digitized version of the L5Q code ----------------------- if (i == _samplesPerCode - 1) { //--- Correct the last index (due to number rounding issues) ----------- @@ -337,7 +327,7 @@ void gps_l5q_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, } else { - _dest[i] = std::complex(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample + _dest[i] = std::complex(1.0 - 2.0 * _code[_codeValueIndex], 0); // repeat the chip -> upsample } } delete[] _code; diff --git a/src/algorithms/libs/gps_l5_signal.h b/src/algorithms/libs/gps_l5_signal.h index b0b24d2a5..de8e44c0d 100644 --- a/src/algorithms/libs/gps_l5_signal.h +++ b/src/algorithms/libs/gps_l5_signal.h @@ -36,18 +36,22 @@ #include #include -//!Generates complex GPS L5i M code for the desired SV ID +//! Generates complex GPS L5I code for the desired SV ID void gps_l5i_code_gen_complex(std::complex* _dest, uint32_t _prn); + +//! Generates real GPS L5I code for the desired SV ID void gps_l5i_code_gen_float(float* _dest, uint32_t _prn); -//!Generates complex GPS L5q M code for the desired SV ID +//! Generates complex GPS L5Q code for the desired SV ID void gps_l5q_code_gen_complex(std::complex* _dest, uint32_t _prn); + +//! Generates real GPS L5Q code for the desired SV ID void gps_l5q_code_gen_float(float* _dest, uint32_t _prn); -//! Generates complex GPS L5i M code for the desired SV ID, and sampled to specific sampling frequency +//! Generates complex GPS L5I code for the desired SV ID, and sampled to specific sampling frequency void gps_l5i_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs); -//! Generates complex GPS L5q M code for the desired SV ID, and sampled to specific sampling frequency +//! Generates complex GPS L5Q code for the desired SV ID, and sampled to specific sampling frequency void gps_l5q_code_gen_complex_sampled(std::complex* _dest, uint32_t _prn, int32_t _fs); 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 4851235ba..00b20a8fd 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 @@ -97,7 +97,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_frame_length_symbols = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; CodeLength = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS; DataLength = (CodeLength / nn) - mm; - d_max_symbols_without_valid_frame = GALILEO_INAV_PAGE_PART_SYMBOLS * 10; //rise alarm 10 seconds without valid tlm + d_max_symbols_without_valid_frame = GALILEO_INAV_PAGE_PART_SYMBOLS * 30; //rise alarm 30 seconds without valid tlm break; } @@ -127,7 +127,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs( d_secondary_code_samples[i] = -1; } } - d_max_symbols_without_valid_frame = GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_FNAV_SYMBOLS_PER_PAGE * 10; //rise alarm 10 seconds without valid tlm + d_max_symbols_without_valid_frame = GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_FNAV_SYMBOLS_PER_PAGE * 30; //rise alarm 30 seconds without valid tlm break; } default: @@ -532,13 +532,14 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( corr_value += d_preamble_samples[i]; } } + if (abs(corr_value) >= d_samples_per_preamble) + { + d_preamble_index = d_sample_counter; // record the preamble sample stamp + DLOG(INFO) << "Preamble detection for Galileo satellite " << this->d_satellite; + d_stat = 1; // enter into frame pre-detection status + } } - if (abs(corr_value) >= d_samples_per_preamble) - { - d_preamble_index = d_sample_counter; // record the preamble sample stamp - DLOG(INFO) << "Preamble detection for Galileo satellite " << this->d_satellite; - d_stat = 1; // enter into frame pre-detection status - } + break; } case 1: // possible preamble lock @@ -560,25 +561,32 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( corr_value += d_preamble_samples[i]; } } - } - if (abs(corr_value) >= d_samples_per_preamble) - { - // check preamble separation - preamble_diff = static_cast(d_sample_counter - d_preamble_index); - if (abs(preamble_diff - d_preamble_period_symbols) == 0) + if (abs(corr_value) >= d_samples_per_preamble) { - // try to decode frame - DLOG(INFO) << "Starting page decoder for Galileo satellite " << this->d_satellite; - d_preamble_index = d_sample_counter; // record the preamble sample stamp - d_CRC_error_counter = 0; - if (corr_value < 0) flag_PLL_180_deg_phase_locked = true; - d_stat = 2; - } - else - { - if (preamble_diff > d_preamble_period_symbols) + // check preamble separation + preamble_diff = static_cast(d_sample_counter - d_preamble_index); + if (abs(preamble_diff - d_preamble_period_symbols) == 0) { - d_stat = 0; // start again + // try to decode frame + DLOG(INFO) << "Starting page decoder for Galileo satellite " << this->d_satellite; + d_preamble_index = d_sample_counter; // record the preamble sample stamp + d_CRC_error_counter = 0; + if (corr_value < 0) + { + flag_PLL_180_deg_phase_locked = true; + } + else + { + flag_PLL_180_deg_phase_locked = false; + } + d_stat = 2; + } + else + { + if (preamble_diff > d_preamble_period_symbols) + { + d_stat = 0; // start again + } } } }