From 93bea1591bbe79de805ef35c2ed6da4868205a5b Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 15 May 2019 17:35:10 +0200 Subject: [PATCH 1/2] Bug fix in GPS L1 CA telemetry decoder, increased subframe realiability --- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 2859ae975..2fb25b023 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -160,10 +160,10 @@ bool gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(uint32_t gpsword) parity = parity & 0x3F; if (parity == (gpsword & 0x3F)) { - return (true); + return true; } - return (false); + return false; } @@ -215,7 +215,7 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe() int32_t word_index = 0; uint32_t GPS_frame_4bytes = 0; float symbol_accumulator = 0; - bool subframe_synchro_confirmation = false; + bool subframe_synchro_confirmation = true; for (float subframe_symbol : d_symbol_history) { // ******* SYMBOL TO BIT ******* @@ -261,9 +261,10 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe() { GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR) } - if (gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(GPS_frame_4bytes)) + //check parity. If ANY word inside the subframe fails the parity, set subframe_synchro_confirmation = false + if (not gps_l1_ca_telemetry_decoder_gs::gps_word_parityCheck(GPS_frame_4bytes)) { - subframe_synchro_confirmation = true; + subframe_synchro_confirmation = false; } // add word to subframe // insert the word in the correct position of the subframe From be5ffe6b6748f00a57e0a9272de65dbc37bd16fb Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 15 May 2019 17:38:45 +0200 Subject: [PATCH 2/2] Implementation of Differential Arctangent Discriminator for FLL --- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 4 ++- .../tracking/libs/tracking_discriminators.cc | 32 +++++++++++++++++++ .../tracking/libs/tracking_discriminators.h | 13 ++++++++ src/core/system_parameters/MATH_CONSTANTS.h | 5 +-- 4 files changed, 51 insertions(+), 3 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 57c19c994..61e407656 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -938,7 +938,9 @@ void dll_pll_veml_tracking::run_dll_pll() if ((d_pull_in_transitory == true and trk_parameters.enable_fll_pull_in == true) or trk_parameters.enable_fll_steady_state) { // FLL discriminator - d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + //d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI; + d_P_accu_old = d_P_accu; //std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl; // Carrier discriminator filter diff --git a/src/algorithms/tracking/libs/tracking_discriminators.cc b/src/algorithms/tracking/libs/tracking_discriminators.cc index 994709bcb..fa7868209 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.cc +++ b/src/algorithms/tracking/libs/tracking_discriminators.cc @@ -33,10 +33,26 @@ */ #include "tracking_discriminators.h" +#include "MATH_CONSTANTS.h" #include // All the outputs are in RADIANS +double phase_unwrap(double phase_rad) +{ + if (phase_rad >= HALF_PI) + { + return phase_rad - PI; + } + else if (phase_rad <= -HALF_PI) + { + return phase_rad + PI; + } + else + { + return phase_rad; + } +} /* * FLL four quadrant arctan discriminator: * \f{equation} @@ -54,6 +70,22 @@ double fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, double return atan2(cross, dot) / (t2 - t1); } +/* + * FLL differential arctan discriminator: + * \f{equation} + * e_{atan}(k)=\frac{1}{t_1-t_2}\text{phase_unwrap}(\tan^-1(\frac{Q(k)}{I(k)})-\tan^-1(\frac{Q(k-1)}{I(k-1)})) + * \f} + * The output is in [radians/second]. + */ +double fll_diff_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2) +{ + double diff_atan = atan(prompt_s2.imag() / prompt_s2.real()) - atan(prompt_s1.imag() / prompt_s1.real()); + if (std::isnan(diff_atan)) + { + diff_atan = 0; + } + return phase_unwrap(diff_atan) / (t2 - t1); +} /* * PLL four quadrant arctan discriminator: diff --git a/src/algorithms/tracking/libs/tracking_discriminators.h b/src/algorithms/tracking/libs/tracking_discriminators.h index 302633783..8358ba075 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.h +++ b/src/algorithms/tracking/libs/tracking_discriminators.h @@ -52,6 +52,19 @@ */ double fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2); +/* + * FLL differential arctan discriminator: + * \f{equation} + * e_{atan}(k)=\frac{1}{t_1-t_2}\text{phase_unwrap}(\tan^-1(\frac{Q(k)}{I(k)})-\tan^-1(\frac{Q(k-1)}{I(k-1)})) + * \f} + * The output is in [radians/second]. + */ +double fll_diff_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2); + +/*! \brief Phase unwrapping function, input is [rad] + * + */ +double phase_unwrap(double phase_rad); /*! \brief PLL four quadrant arctan discriminator * diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index dec9eef93..41fc3221d 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -43,8 +43,9 @@ ONE_PI_TWO_PX = (1/Pi)*2^X */ -const double PI = 3.1415926535897932; //!< pi -const double PI_2 = 2.0 * PI; //!< 2 * pi +const double HALF_PI = 1.570796326794897; //!< pi/2 +const double PI = 3.1415926535897932; //!< pi +const double PI_2 = 2.0 * PI; //!< 2 * pi const double TWO_P3 = (8); //!< 2^3 const double TWO_P4 = (16); //!< 2^4