From be5ffe6b6748f00a57e0a9272de65dbc37bd16fb Mon Sep 17 00:00:00 2001
From: Javier Arribas <javiarribas@gmail.com>
Date: Wed, 15 May 2019 17:38:45 +0200
Subject: [PATCH] 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 <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:
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