1 files changed, 20 insertions, 3 deletions
diff --git a/gr-analog/lib/pll_carriertracking_cc_impl.h b/gr-analog/lib/pll_carriertracking_cc_impl.h
index 3cb7146da9..82a2f1c21c 100644
--- a/gr-analog/lib/pll_carriertracking_cc_impl.h
+++ b/gr-analog/lib/pll_carriertracking_cc_impl.h
@@ -12,6 +12,7 @@
 #define INCLUDED_ANALOG_PLL_CARRIERTRACKING_CC_IMPL_H
 
 #include <gnuradio/analog/pll_carriertracking_cc.h>
+#include <gnuradio/math.h>
 
 namespace gr {
 namespace analog {
@@ -22,14 +23,30 @@ private:
     float d_locksig, d_lock_threshold;
     bool d_squelch_enable;
 
-    float mod_2pi(float in);
-    float phase_detector(gr_complex sample, float ref_phase);
+    float mod_2pi(float in)
+    {
+        if (in > GR_M_PI)
+            return in - (2.0 * GR_M_PI);
+        else if (in < -GR_M_PI)
+            return in + (2.0 * GR_M_PI);
+        else
+            return in;
+    }
+
+    float phase_detector(gr_complex sample, float ref_phase)
+    {
+        float sample_phase;
+        //  sample_phase = atan2(sample.imag(),sample.real());
+        sample_phase = gr::fast_atan2f(sample.imag(), sample.real());
+        return mod_2pi(sample_phase - ref_phase);
+    }
 
 public:
     pll_carriertracking_cc_impl(float loop_bw, float max_freq, float min_freq);
     ~pll_carriertracking_cc_impl();
 
-    bool lock_detector(void);
+    bool lock_detector(void) { return (fabsf(d_locksig) > d_lock_threshold); }
+
     bool squelch_enable(bool);
     float set_lock_threshold(float);