5 files changed, 174 insertions, 54 deletions
diff --git a/gr-digital/grc/digital_costas_loop_cc.xml b/gr-digital/grc/digital_costas_loop_cc.xml
index 668c43dec6..40db3a4a6f 100644
--- a/gr-digital/grc/digital_costas_loop_cc.xml
+++ b/gr-digital/grc/digital_costas_loop_cc.xml
@@ -5,34 +5,60 @@
 ###################################################
  -->
 <block>
-	<name>Costas Loop</name>
-	<key>digital_costas_loop_cc</key>
-	<import>from gnuradio import digital</import>
-	<make>digital.costas_loop_cc($w, $order)</make>
-	<callback>set_loop_bandwidth($w)</callback>
-	<param>
-		<name>Loop Bandwidth</name>
-		<key>w</key>
-		<type>real</type>
-	</param>
-	<param>
-		<name>Order</name>
-		<key>order</key>
-		<type>int</type>
-	</param>
-	<sink>
-		<name>in</name>
-		<type>complex</type>
-	</sink>
-	<source>
-		<name>out</name>
-		<type>complex</type>
-	</source>
+  <name>Costas Loop</name>
+  <key>digital_costas_loop_cc</key>
+  <import>from gnuradio import digital</import>
+  <make>digital.costas_loop_cc($w, $order, $use_snr)</make>
+  <callback>set_loop_bandwidth($w)</callback>
 
-	<!-- Optional Outputs -->
-	<source>
-		<name>frequency</name>
-		<type>float</type>
-		<optional>1</optional>
-	</source>
+  <param>
+    <name>Loop Bandwidth</name>
+    <key>w</key>
+    <type>real</type>
+  </param>
+
+  <param>
+    <name>Order</name>
+    <key>order</key>
+    <type>int</type>
+  </param>
+
+  <param>
+    <name>Use SNR</name>
+    <key>use_snr</key>
+    <value>False</value>
+    <type>enum</type>
+    <hide>part</hide>
+    <option>
+      <name>Yes</name>
+      <key>True</key>
+    </option>
+    <option>
+      <name>No</name>
+      <key>False</key>
+    </option>
+  </param>
+
+  <sink>
+    <name>in</name>
+    <type>complex</type>
+  </sink>
+
+  <sink>
+    <name>noise</name>
+    <type>message</type>
+    <optional>1</optional>
+  </sink>
+
+  <source>
+    <name>out</name>
+    <type>complex</type>
+  </source>
+
+  <!-- Optional Outputs -->
+  <source>
+    <name>frequency</name>
+    <type>float</type>
+    <optional>1</optional>
+  </source>
 </block>
diff --git a/gr-digital/include/gnuradio/digital/costas_loop_cc.h b/gr-digital/include/gnuradio/digital/costas_loop_cc.h
index d924d969b7..ff5b9b993a 100644
--- a/gr-digital/include/gnuradio/digital/costas_loop_cc.h
+++ b/gr-digital/include/gnuradio/digital/costas_loop_cc.h
@@ -1,6 +1,6 @@
 /* -*- c++ -*- */
 /*
- * Copyright 2006,2011,2012 Free Software Foundation, Inc.
+ * Copyright 2006,2011,2012,2014 Free Software Foundation, Inc.
  *
  * This file is part of GNU Radio
  *
@@ -58,6 +58,9 @@ namespace gr {
      * The Costas loop can have two output streams:
      * \li stream 1 (required) is the baseband I and Q;
      * \li stream 2 (optional) is the normalized frequency of the loop
+     *
+     * There is a single optional message input:
+     * \li noise: A noise floor estimate used to calculate the SNR of a sample.
      */
     class DIGITAL_API costas_loop_cc
       : virtual public sync_block,
@@ -72,8 +75,10 @@ namespace gr {
        *
        * \param loop_bw  internal 2nd order loop bandwidth (~ 2pi/100)
        * \param order the loop order, either 2, 4, or 8
+       * \param use_snr Use or ignore SNR estimates (from noise message port)
+       *        in measurements; also uses tanh instead of slicing.
        */
-      static sptr make(float loop_bw, int order);
+      static sptr make(float loop_bw, int order, bool use_snr=false);
 
       /*!
        * Returns the current value of the loop error.
diff --git a/gr-digital/lib/costas_loop_cc_impl.cc b/gr-digital/lib/costas_loop_cc_impl.cc
index 36f95ac022..a53045ac55 100644
--- a/gr-digital/lib/costas_loop_cc_impl.cc
+++ b/gr-digital/lib/costas_loop_cc_impl.cc
@@ -35,37 +35,52 @@ namespace gr {
   namespace digital {
 
     costas_loop_cc::sptr
-    costas_loop_cc::make(float loop_bw, int order)
+    costas_loop_cc::make(float loop_bw, int order, bool use_snr)
     {
       return gnuradio::get_initial_sptr
-	(new costas_loop_cc_impl(loop_bw, order));
+	(new costas_loop_cc_impl(loop_bw, order, use_snr));
     }
 
-    costas_loop_cc_impl::costas_loop_cc_impl(float loop_bw, int order)
+    costas_loop_cc_impl::costas_loop_cc_impl(float loop_bw, int order, bool use_snr)
       : sync_block("costas_loop_cc",
                    io_signature::make(1, 1, sizeof(gr_complex)),
                    io_signature::make2(1, 2, sizeof(gr_complex), sizeof(float))),
 	blocks::control_loop(loop_bw, 1.0, -1.0),
-	d_order(order), d_error(0), d_phase_detector(NULL)
+	d_order(order), d_error(0), d_noise(1.0), d_phase_detector(NULL)
     {
       // Set up the phase detector to use based on the constellation order
       switch(d_order) {
       case 2:
-	d_phase_detector = &costas_loop_cc_impl::phase_detector_2;
+        if(use_snr)
+          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_2;
+	else
+          d_phase_detector = &costas_loop_cc_impl::phase_detector_2;
 	break;
 
       case 4:
-	d_phase_detector = &costas_loop_cc_impl::phase_detector_4;
+        if(use_snr)
+          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_4;
+	else
+          d_phase_detector = &costas_loop_cc_impl::phase_detector_4;
 	break;
 
       case 8:
-	d_phase_detector = &costas_loop_cc_impl::phase_detector_8;
+        if(use_snr)
+          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_8;
+	else
+          d_phase_detector = &costas_loop_cc_impl::phase_detector_8;
 	break;
 
       default:
 	throw std::invalid_argument("order must be 2, 4, or 8");
 	break;
       }
+
+      message_port_register_in(pmt::mp("noise"));
+      set_msg_handler(
+        pmt::mp("noise"),
+        boost::bind(&costas_loop_cc_impl::handle_set_noise,
+                    this, _1));
     }
 
     costas_loop_cc_impl::~costas_loop_cc_impl()
@@ -114,11 +129,50 @@ namespace gr {
     }
 
     float
+    costas_loop_cc_impl::phase_detector_snr_8(gr_complex sample) const
+    {
+      float K = (sqrt(2.0) - 1);
+      float snr = abs(sample)*abs(sample) / d_noise;
+      if(fabsf(sample.real()) >= fabsf(sample.imag())) {
+	return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag()) -
+          (blocks::tanhf_lut(snr*sample.imag()) * sample.real() * K));
+      }
+      else {
+	return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag() * K) -
+          (blocks::tanhf_lut(snr*sample.imag()) * sample.real()));
+      }
+    }
+
+    float
+    costas_loop_cc_impl::phase_detector_snr_4(gr_complex sample) const
+    {
+      float snr = abs(sample)*abs(sample) / d_noise;
+      return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag()) -
+              (blocks::tanhf_lut(snr*sample.imag()) * sample.real()));
+    }
+
+    float
+    costas_loop_cc_impl::phase_detector_snr_2(gr_complex sample) const
+    {
+      float snr = abs(sample)*abs(sample) / d_noise;
+      return blocks::tanhf_lut(snr*sample.real()) * sample.imag();
+    }
+
+    float
     costas_loop_cc_impl::error() const
     {
       return d_error;
     }
 
+    void
+    costas_loop_cc_impl::handle_set_noise(pmt::pmt_t msg)
+    {
+      if(pmt::is_real(msg)) {
+        d_noise = pmt::to_double(msg);
+        d_noise = powf(10.0f, d_noise/10.0f);
+      }
+    }
+
     int
     costas_loop_cc_impl::work(int noutput_items,
 			      gr_vector_const_void_star &input_items,
diff --git a/gr-digital/lib/costas_loop_cc_impl.h b/gr-digital/lib/costas_loop_cc_impl.h
index 665724236b..ebd05e2e93 100644
--- a/gr-digital/lib/costas_loop_cc_impl.h
+++ b/gr-digital/lib/costas_loop_cc_impl.h
@@ -1,6 +1,6 @@
 /* -*- c++ -*- */
 /*
- * Copyright 2006,2011,2012 Free Software Foundation, Inc.
+ * Copyright 2006,2011,2012,2014 Free Software Foundation, Inc.
  *
  * This file is part of GNU Radio
  *
@@ -34,33 +34,68 @@ namespace gr {
     private:
       int d_order;
       float d_error;
+      float d_noise;
 
-      /*! \brief the phase detector circuit for 8th-order PSK loops
+       /*! \brief the phase detector circuit for 8th-order PSK loops.
+       *
        *  \param sample complex sample
        *  \return the phase error
        */
       float phase_detector_8(gr_complex sample) const;    // for 8PSK
 
-      /*! \brief the phase detector circuit for fourth-order loops
+      /*! \brief the phase detector circuit for fourth-order loops.
+       *
        *  \param sample complex sample
        *  \return the phase error
        */
       float phase_detector_4(gr_complex sample) const;    // for QPSK
 
-      /*! \brief the phase detector circuit for second-order loops
+      /*! \brief the phase detector circuit for second-order loops.
+       *
        *  \param sample a complex sample
        *  \return the phase error
        */
       float phase_detector_2(gr_complex sample) const;    // for BPSK
 
+
+      /*! \brief the phase detector circuit for 8th-order PSK
+       *  loops. Uses tanh instead of slicing and the noise estimate
+       *  from the message port to estimated SNR of the samples.
+       *
+       *  \param sample complex sample
+       *  \return the phase error
+       */
+      float phase_detector_snr_8(gr_complex sample) const;    // for 8PSK
+
+      /*! \brief the phase detector circuit for fourth-order
+       *  loops. Uses tanh instead of slicing and the noise estimate
+       *  from the message port to estimated SNR of the samples.
+       *
+       *  \param sample complex sample
+       *  \return the phase error
+       */
+      float phase_detector_snr_4(gr_complex sample) const;    // for QPSK
+
+      /*! \brief the phase detector circuit for second-order
+       *  loops. Uses tanh instead of slicing and the noise estimate
+       *  from the message port to estimated SNR of the samples.
+       *
+       *  \param sample a complex sample
+       *  \return the phase error
+       */
+      float phase_detector_snr_2(gr_complex sample) const;    // for BPSK
+
+
       float (costas_loop_cc_impl::*d_phase_detector)(gr_complex sample) const;
 
     public:
-      costas_loop_cc_impl(float loop_bw, int order);
+      costas_loop_cc_impl(float loop_bw, int order, bool use_snr=false);
       ~costas_loop_cc_impl();
 
       float error() const;
 
+      void handle_set_noise(pmt::pmt_t msg);
+
       void setup_rpc();
 
       int work(int noutput_items,
diff --git a/gr-digital/python/digital/qa_costas_loop_cc.py b/gr-digital/python/digital/qa_costas_loop_cc.py
index 9ecb017599..e48f45cc22 100755
--- a/gr-digital/python/digital/qa_costas_loop_cc.py
+++ b/gr-digital/python/digital/qa_costas_loop_cc.py
@@ -1,24 +1,24 @@
 #!/usr/bin/env python
 #
 # Copyright 2011,2013 Free Software Foundation, Inc.
-# 
+#
 # This file is part of GNU Radio
-# 
+#
 # GNU Radio is free software; you can redistribute it and/or modify
 # it under the terms of the GNU General Public License as published by
 # the Free Software Foundation; either version 3, or (at your option)
 # any later version.
-# 
+#
 # GNU Radio is distributed in the hope that it will be useful,
 # but WITHOUT ANY WARRANTY; without even the implied warranty of
 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 # GNU General Public License for more details.
-# 
+#
 # You should have received a copy of the GNU General Public License
 # along with GNU Radio; see the file COPYING.  If not, write to
 # the Free Software Foundation, Inc., 51 Franklin Street,
 # Boston, MA 02110-1301, USA.
-# 
+#
 
 import random
 import cmath
@@ -46,7 +46,7 @@ class test_costas_loop_cc(gr_unittest.TestCase):
 
         self.tb.connect(self.src, self.test, self.snk)
         self.tb.run()
-        
+
         expected_result = data
         dst_data = self.snk.data()
         self.assertComplexTuplesAlmostEqual(expected_result, dst_data, 5)
@@ -77,7 +77,7 @@ class test_costas_loop_cc(gr_unittest.TestCase):
 
         rot = cmath.exp(0.2j) # some small rotation
         data = [complex(2*random.randint(0,1)-1, 0) for i in xrange(100)]
-        
+
         N = 40 # settling time
         expected_result = data[N:]
         data = [rot*d for d in data]
@@ -89,7 +89,7 @@ class test_costas_loop_cc(gr_unittest.TestCase):
         self.tb.run()
 
         dst_data = self.snk.data()[N:]
-        
+
         # generously compare results; the loop will converge near to, but
         # not exactly on, the target data
         self.assertComplexTuplesAlmostEqual(expected_result, dst_data, 2)
@@ -103,7 +103,7 @@ class test_costas_loop_cc(gr_unittest.TestCase):
         rot = cmath.exp(0.2j) # some small rotation
         data = [complex(2*random.randint(0,1)-1, 2*random.randint(0,1)-1)
                 for i in xrange(100)]
-        
+
         N = 40 # settling time
         expected_result = data[N:]
         data = [rot*d for d in data]
@@ -130,7 +130,7 @@ class test_costas_loop_cc(gr_unittest.TestCase):
         const = psk.psk_constellation(order)
         data = [random.randint(0,7) for i in xrange(100)]
         data = [2*rot*const.points()[d] for d in data]
-        
+
         N = 40 # settling time
         expected_result = data[N:]
 
@@ -144,7 +144,7 @@ class test_costas_loop_cc(gr_unittest.TestCase):
         self.tb.run()
 
         dst_data = self.snk.data()[N:]
-        
+
 	# generously compare results; the loop will converge near to, but
         # not exactly on, the target data
         self.assertComplexTuplesAlmostEqual(expected_result, dst_data, 2)