Added new blocks for clock freqeuncy correction

5 files changed, 162 insertions, 1 deletions

diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 651baa5..1b24d40 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -36,7 +36,7 @@ GR_PYTHON_INSTALL(
     sch_detector.py
     fcch_detector.py 
     chirpz.py
-    DESTINATION ${GR_PYTHON_DIR}/gsm
+    clock_offset_control.py DESTINATION ${GR_PYTHON_DIR}/gsm
 )
 
 ########################################################################
diff --git a/python/__init__.py b/python/__init__.py
index cf25d1c..3428109 100644
--- a/python/__init__.py
+++ b/python/__init__.py
@@ -50,6 +50,7 @@ from receiver_hier import receiver_hier
 from fcch_burst_tagger import fcch_burst_tagger
 from sch_detector import sch_detector
 from fcch_detector import fcch_detector
+from clock_offset_control import clock_offset_control
 
 #
 
diff --git a/python/clock_offset_control.py b/python/clock_offset_control.py
new file mode 100644
index 0000000..133368c
--- /dev/null
+++ b/python/clock_offset_control.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# 
+# Copyright 2014 <+YOU OR YOUR COMPANY+>.
+# 
+# This 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.
+# 
+# This software 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 this software; see the file COPYING.  If not, write to
+# the Free Software Foundation, Inc., 51 Franklin Street,
+# Boston, MA 02110-1301, USA.
+# 
+
+from numpy import *
+from gnuradio import gr
+import pmt
+
+class clock_offset_control(gr.basic_block):
+    """
+    docstring for block clock_offset_control
+    """
+    def __init__(self, fc, samp_rate):
+        gr.basic_block.__init__(self,
+            name="clock_offset_control",
+            in_sig=[],
+            out_sig=[])
+        self.fc = fc
+        self.samp_rate = samp_rate
+        self.message_port_register_in(pmt.intern("measurements"))
+        self.set_msg_handler(pmt.intern("measurements"), self.process_measurement)
+        self.message_port_register_out(pmt.intern("ppm"))
+        self.alfa = 0.6
+        self.ppm_estimate = -1e6
+        self.first_measurement = True
+        self.counter = 0
+        
+    def process_measurement(self,msg):
+        if pmt.is_tuple(msg):
+            key = pmt.symbol_to_string(pmt.tuple_ref(msg,0))
+            if key == "freq_offset":
+                freq_offset = pmt.to_double(pmt.tuple_ref(msg,1))
+                ppm = -freq_offset/self.fc*1.0e6
+                state = pmt.symbol_to_string(pmt.tuple_ref(msg,2))
+
+                if state == "first_fcch_search" or state == "next_fcch_search":
+                    msg_ppm = pmt.from_double(ppm)
+                    self.message_port_pub(pmt.intern("ppm"), msg_ppm)
+                
+                if state == "synchronized":
+                    if self.first_measurement:
+                        self.ppm_estimate = ppm
+                        self.first_measurement = False
+                    else:
+                        self.ppm_estimate = (1-self.alfa)*self.ppm_estimate+self.alfa*ppm
+                    
+                    if self.counter == 5:
+                        self.counter = 0
+                        msg_ppm = pmt.from_double(ppm)
+                        self.message_port_pub(pmt.intern("ppm"), msg_ppm)
+                    else:
+                        self.counter=self.counter+1
+                    
+                if state == "sync_loss":
+                    self.ppm_estimate = -1e6
+                    self.counter = 0
+                    self.first_measurement = True
+                    msg_ppm = pmt.from_double(0.0)
+                    self.message_port_pub(pmt.intern("ppm"), msg_ppm)
diff --git a/python/clock_offset_corrector.py b/python/clock_offset_corrector.py
new file mode 100644
index 0000000..df97cca
--- /dev/null
+++ b/python/clock_offset_corrector.py
@@ -0,0 +1,82 @@
+#!/usr/bin/env python
+##################################################
+# Gnuradio Python Flow Graph
+# Title: Clock offset corrector
+# Author: Piotr Krysik
+# Generated: Wed Aug  6 13:47:38 2014
+##################################################
+
+from gnuradio import blocks
+from gnuradio import filter
+from gnuradio import gr
+from gnuradio.filter import firdes
+import gsm
+import math
+
+class clock_offset_corrector(gr.hier_block2):
+
+    def __init__(self, ppm=0, samp_rate=1625000.0/6.0*4.0, fc=936.6e6):
+        gr.hier_block2.__init__(
+            self, "Clock offset corrector",
+            gr.io_signature(1, 1, gr.sizeof_gr_complex*1),
+            gr.io_signature(1, 1, gr.sizeof_gr_complex*1),
+        )
+
+        ##################################################
+        # Parameters
+        ##################################################
+        self.ppm = ppm
+        self.samp_rate = samp_rate
+        self.fc = fc
+
+        ##################################################
+        # Blocks
+        ##################################################
+        self.ppm_msg = None;self.message_port_register_hier_out("ppm_msg")
+        self.gsm_controlled_rotator_cc_0 = gsm.controlled_rotator_cc(0,samp_rate)
+        self.gsm_controlled_const_source_f_0 = gsm.controlled_const_source_f(ppm)
+        self.fractional_resampler_xx_0 = filter.fractional_resampler_cc(0, 1)
+        self.blocks_multiply_const_vxx_0_0 = blocks.multiply_const_vff((1.0/1.0e6, ))
+        self.blocks_multiply_const_vxx_0 = blocks.multiply_const_vff((fc/samp_rate*(2*math.pi), ))
+        self.blocks_add_const_vxx_0 = blocks.add_const_vff((1, ))
+
+        ##################################################
+        # Connections
+        ##################################################
+        self.connect((self.gsm_controlled_const_source_f_0, 0), (self.blocks_multiply_const_vxx_0_0, 0))
+        self.connect((self, 0), (self.gsm_controlled_rotator_cc_0, 0))
+        self.connect((self.blocks_multiply_const_vxx_0_0, 0), (self.blocks_multiply_const_vxx_0, 0))
+        self.connect((self.blocks_add_const_vxx_0, 0), (self.fractional_resampler_xx_0, 1))
+        self.connect((self.gsm_controlled_rotator_cc_0, 0), (self.fractional_resampler_xx_0, 0))
+        self.connect((self.blocks_multiply_const_vxx_0, 0), (self.gsm_controlled_rotator_cc_0, 1))
+        self.connect((self.blocks_multiply_const_vxx_0_0, 0), (self.blocks_add_const_vxx_0, 0))
+        self.connect((self.fractional_resampler_xx_0, 0), (self, 0))
+
+        ##################################################
+        # Asynch Message Connections
+        ##################################################
+        self.msg_connect(self, "ppm_msg", self.gsm_controlled_const_source_f_0, "constant_msg")
+
+
+    def get_ppm(self):
+        return self.ppm
+
+    def set_ppm(self, ppm):
+        self.ppm = ppm
+        self.gsm_controlled_const_source_f_0.set_constant(self.ppm)
+
+    def get_samp_rate(self):
+        return self.samp_rate
+
+    def set_samp_rate(self, samp_rate):
+        self.samp_rate = samp_rate
+        self.blocks_multiply_const_vxx_0.set_k((self.fc/self.samp_rate*(2*math.pi), ))
+        self.gsm_controlled_rotator_cc_0.set_samp_rate(self.samp_rate)
+
+    def get_fc(self):
+        return self.fc
+
+    def set_fc(self, fc):
+        self.fc = fc
+        self.blocks_multiply_const_vxx_0.set_k((self.fc/self.samp_rate*(2*math.pi), ))
+
diff --git a/python/receiver_hier.py b/python/receiver_hier.py
index 6a0b77f..fe9c636 100644
--- a/python/receiver_hier.py
+++ b/python/receiver_hier.py
@@ -25,6 +25,7 @@ class receiver_hier(gr.hier_block2):
         gsm_symb_rate = 1625000/6.0
 
         self.message_port_register_hier_in("bursts")
+        self.message_port_register_hier_in("measurements")
 
         self.input_rate = input_rate
         self.osr = osr
@@ -40,6 +41,7 @@ class receiver_hier(gr.hier_block2):
         self.connect(self, self.filtr,  self.interpolator, self.receiver,  self)
 #        self.connect(self, self.interpolator, self.receiver,  self)
         self.msg_connect(self.receiver, "bursts", weakref.proxy(self), "bursts")
+        self.msg_connect(self.receiver, "measurements", weakref.proxy(self), "measurements")
 
     def _set_filter(self):
         filter_cutoff   = 125e3