piotr | 4089c1a | 2014-08-06 14:10:56 +0200 | [diff] [blame] | 1 | #!/usr/bin/env python |
| 2 | # -*- coding: utf-8 -*- |
| 3 | # |
| 4 | # Copyright 2014 <+YOU OR YOUR COMPANY+>. |
| 5 | # |
| 6 | # This is free software; you can redistribute it and/or modify |
| 7 | # it under the terms of the GNU General Public License as published by |
| 8 | # the Free Software Foundation; either version 3, or (at your option) |
| 9 | # any later version. |
| 10 | # |
| 11 | # This software is distributed in the hope that it will be useful, |
| 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 14 | # GNU General Public License for more details. |
| 15 | # |
| 16 | # You should have received a copy of the GNU General Public License |
| 17 | # along with this software; see the file COPYING. If not, write to |
| 18 | # the Free Software Foundation, Inc., 51 Franklin Street, |
| 19 | # Boston, MA 02110-1301, USA. |
| 20 | # |
| 21 | |
| 22 | from numpy import * |
| 23 | from gnuradio import gr |
| 24 | import pmt |
Piotr K | 8cfe4c4 | 2014-08-07 17:03:10 +0200 | [diff] [blame^] | 25 | from threading import Timer |
piotr | 4089c1a | 2014-08-06 14:10:56 +0200 | [diff] [blame] | 26 | |
| 27 | class clock_offset_control(gr.basic_block): |
| 28 | """ |
| 29 | docstring for block clock_offset_control |
| 30 | """ |
| 31 | def __init__(self, fc, samp_rate): |
| 32 | gr.basic_block.__init__(self, |
| 33 | name="clock_offset_control", |
| 34 | in_sig=[], |
| 35 | out_sig=[]) |
| 36 | self.fc = fc |
| 37 | self.samp_rate = samp_rate |
| 38 | self.message_port_register_in(pmt.intern("measurements")) |
| 39 | self.set_msg_handler(pmt.intern("measurements"), self.process_measurement) |
| 40 | self.message_port_register_out(pmt.intern("ppm")) |
| 41 | self.alfa = 0.6 |
| 42 | self.ppm_estimate = -1e6 |
| 43 | self.first_measurement = True |
| 44 | self.counter = 0 |
Piotr K | 8cfe4c4 | 2014-08-07 17:03:10 +0200 | [diff] [blame^] | 45 | self.last_state = "" |
| 46 | self.timer = Timer(0.5, self.conditional_reset) |
piotr | 4089c1a | 2014-08-06 14:10:56 +0200 | [diff] [blame] | 47 | |
| 48 | def process_measurement(self,msg): |
| 49 | if pmt.is_tuple(msg): |
| 50 | key = pmt.symbol_to_string(pmt.tuple_ref(msg,0)) |
| 51 | if key == "freq_offset": |
| 52 | freq_offset = pmt.to_double(pmt.tuple_ref(msg,1)) |
| 53 | ppm = -freq_offset/self.fc*1.0e6 |
| 54 | state = pmt.symbol_to_string(pmt.tuple_ref(msg,2)) |
piotr | 9b59e82 | 2014-08-06 20:14:46 +0200 | [diff] [blame] | 55 | |
Piotr K | 8cfe4c4 | 2014-08-07 17:03:10 +0200 | [diff] [blame^] | 56 | self.last_state = state |
| 57 | |
| 58 | if abs(ppm) > 100: #safeguard against flawed measurements |
piotr | 9b59e82 | 2014-08-06 20:14:46 +0200 | [diff] [blame] | 59 | ppm = 0 |
| 60 | self.reset() |
| 61 | |
piotr | d6d6687 | 2014-08-06 15:20:33 +0200 | [diff] [blame] | 62 | if state == "fcch_search": |
piotr | 4089c1a | 2014-08-06 14:10:56 +0200 | [diff] [blame] | 63 | msg_ppm = pmt.from_double(ppm) |
| 64 | self.message_port_pub(pmt.intern("ppm"), msg_ppm) |
Piotr K | 8cfe4c4 | 2014-08-07 17:03:10 +0200 | [diff] [blame^] | 65 | self.timer.cancel() |
| 66 | self.timer = Timer(0.5, self.conditional_reset) |
| 67 | self.timer.start() |
| 68 | elif state == "synchronized": |
piotr | 4089c1a | 2014-08-06 14:10:56 +0200 | [diff] [blame] | 69 | if self.first_measurement: |
| 70 | self.ppm_estimate = ppm |
| 71 | self.first_measurement = False |
| 72 | else: |
| 73 | self.ppm_estimate = (1-self.alfa)*self.ppm_estimate+self.alfa*ppm |
| 74 | |
| 75 | if self.counter == 5: |
| 76 | self.counter = 0 |
| 77 | msg_ppm = pmt.from_double(ppm) |
| 78 | self.message_port_pub(pmt.intern("ppm"), msg_ppm) |
| 79 | else: |
| 80 | self.counter=self.counter+1 |
Piotr K | 8cfe4c4 | 2014-08-07 17:03:10 +0200 | [diff] [blame^] | 81 | elif state == "sync_loss": |
piotr | 9b59e82 | 2014-08-06 20:14:46 +0200 | [diff] [blame] | 82 | self.reset() |
piotr | 4089c1a | 2014-08-06 14:10:56 +0200 | [diff] [blame] | 83 | msg_ppm = pmt.from_double(0.0) |
| 84 | self.message_port_pub(pmt.intern("ppm"), msg_ppm) |
Piotr K | 8cfe4c4 | 2014-08-07 17:03:10 +0200 | [diff] [blame^] | 85 | |
| 86 | def conditional_reset(self): |
| 87 | if self.last_state != "synchronized": |
| 88 | # print "conditional reset" |
| 89 | self.reset() |
| 90 | msg_ppm = pmt.from_double(0.0) |
| 91 | self.message_port_pub(pmt.intern("ppm"), msg_ppm) |
| 92 | |
piotr | 9b59e82 | 2014-08-06 20:14:46 +0200 | [diff] [blame] | 93 | def reset(self): |
| 94 | self.ppm_estimate = -1e6 |
| 95 | self.counter = 0 |
| 96 | self.first_measurement = True |