Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

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

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

src/algorithms/tracking/libs/tracking_discriminators.cc

@@ -33,10 +33,26 @@
  */
 
 #include "tracking_discriminators.h"
+#include "MATH_CONSTANTS.h"
 #include <cmath>
 
 //  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:

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
  *

src/core/system_parameters/MATH_CONSTANTS.h

@@ -43,6 +43,7 @@
         ONE_PI_TWO_PX = (1/Pi)*2^X
 */
 
+const double HALF_PI = 1.570796326794897;  //!< pi/2
 const double PI = 3.1415926535897932;      //!<  pi
 const double PI_2 = 2.0 * PI;              //!<  2 * pi