Implementation of Differential Arctangent Discriminator for FLL

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,8 +43,9 @@
         ONE_PI_TWO_PX = (1/Pi)*2^X
 */
 
-const double PI = 3.1415926535897932;  //!<  pi
+const double HALF_PI = 1.570796326794897;  //!< pi/2
-const double PI_2 = 2.0 * PI;          //!<  2 * pi
+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