Piotr Krysik | a6268a5 | 2017-08-23 16:02:19 +0200 | [diff] [blame] | 1 | #!/usr/bin/env python2 |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 2 | # -*- coding: utf-8 -*- |
Piotr Krysik | a6268a5 | 2017-08-23 16:02:19 +0200 | [diff] [blame] | 3 | # @file |
| 4 | # @author (C) 2014 by Piotr Krysik <ptrkrysik@gmail.com> |
| 5 | # @section LICENSE |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 6 | # |
| 7 | # This is free software; you can redistribute it and/or modify |
| 8 | # it under the terms of the GNU General Public License as published by |
| 9 | # the Free Software Foundation; either version 3, or (at your option) |
| 10 | # any later version. |
| 11 | # |
| 12 | # This software is distributed in the hope that it will be useful, |
| 13 | # but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 14 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 15 | # GNU General Public License for more details. |
| 16 | # |
| 17 | # You should have received a copy of the GNU General Public License |
| 18 | # along with this software; see the file COPYING. If not, write to |
| 19 | # the Free Software Foundation, Inc., 51 Franklin Street, |
| 20 | # Boston, MA 02110-1301, USA. |
| 21 | # |
| 22 | |
| 23 | from numpy import * |
Piotr Krysik | d6cc069 | 2014-12-06 20:56:19 +0100 | [diff] [blame] | 24 | #from pylab import * |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 25 | from gnuradio import gr |
| 26 | import pmt |
Vasil Velichkov | b479fb4 | 2018-02-21 01:06:16 +0200 | [diff] [blame] | 27 | from chirpz import ZoomFFT |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 28 | |
| 29 | class fcch_burst_tagger(gr.sync_block): |
| 30 | """ |
| 31 | docstring for block fcch_burst_tagger |
| 32 | """ |
| 33 | def __init__(self, OSR): |
| 34 | gr.sync_block.__init__(self, |
| 35 | name="fcch_burst_tagger", |
| 36 | in_sig=[complex64, float32], |
| 37 | out_sig=[complex64]) |
| 38 | |
| 39 | self.state=False |
| 40 | self.symbol_rate = 1625000/6 |
| 41 | self.OSR=OSR |
| 42 | self.samp_rate = self.symbol_rate*OSR |
| 43 | self.burst_size = int(156.25*self.OSR) |
| 44 | self.guard_period = int(round(8.25*self.OSR)) |
| 45 | self.block_size = self.burst_size+self.guard_period |
| 46 | self.processed_block_size = int(142*self.OSR) |
piotr | 75c2f5c | 2014-07-20 23:51:28 +0200 | [diff] [blame] | 47 | self.set_history(self.block_size) |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 48 | self.set_output_multiple(self.guard_period) |
| 49 | self.prev_offset=0 |
| 50 | |
| 51 | #parameters of zoomfft frequency estimator |
| 52 | f1 = self.symbol_rate/4*0.9 |
| 53 | f2 = self.symbol_rate/4*1.1 |
| 54 | m=5000*self.OSR |
| 55 | self.zoomfft = ZoomFFT(self.processed_block_size, f1, f2, m, Fs=self.samp_rate) |
| 56 | self.f_axis = linspace(f1,f2,m) |
| 57 | |
| 58 | def work(self, input_items, output_items): |
| 59 | in0=input_items[0] |
| 60 | output_items[0][:] = in0[self.history()-1:] |
| 61 | |
| 62 | threshold = input_items[1][self.history()-1:] |
| 63 | threshold_diff = diff(concatenate([[0],threshold])) |
| 64 | up_to_high_indexes = nonzero(threshold_diff>0)[0] |
| 65 | |
| 66 | up_to_high_idx=[] |
| 67 | |
piotr | 75c2f5c | 2014-07-20 23:51:28 +0200 | [diff] [blame] | 68 | for up_to_high_idx in up_to_high_indexes: #look for "high" value at the trigger |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 69 | if up_to_high_idx==0 and self.state==True: #if it's not transition from "low" to "high" |
piotr | 75c2f5c | 2014-07-20 23:51:28 +0200 | [diff] [blame] | 70 | continue #then continue |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 71 | self.state=True #if found - change state |
| 72 | |
| 73 | if self.state==True and up_to_high_idx and any(threshold_diff<0): #and look for transition from high to low |
| 74 | last_up_to_high_idx = up_to_high_idx |
| 75 | last_high_to_low_idx = nonzero(threshold_diff<0)[0][-1] |
| 76 | |
| 77 | if last_high_to_low_idx-last_up_to_high_idx>0: |
piotr | 75c2f5c | 2014-07-20 23:51:28 +0200 | [diff] [blame] | 78 | coarse_idx = int(last_high_to_low_idx+self.history()-self.block_size) |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 79 | inst_freq = angle(in0[coarse_idx:coarse_idx+self.block_size]*in0[coarse_idx-self.OSR:coarse_idx+self.block_size-self.OSR].conj())/(2*pi)*self.symbol_rate #instantaneus frequency estimate |
| 80 | precise_idx = self.find_best_position(inst_freq) |
| 81 | # measured_freq = mean(inst_freq[precise_idx:precise_idx+self.processed_block_size]) |
| 82 | expected_freq = self.symbol_rate/4 |
piotr | 75c2f5c | 2014-07-20 23:51:28 +0200 | [diff] [blame] | 83 | |
| 84 | print "input_items:",len(in0) |
| 85 | print "coarse_idx",coarse_idx |
| 86 | print "coarse_idx+precise_idx",coarse_idx+precise_idx |
| 87 | |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 88 | zoomed_spectrum = abs(self.zoomfft(in0[coarse_idx+precise_idx:coarse_idx+precise_idx+self.processed_block_size])) |
| 89 | measured_freq = self.f_axis[argmax(zoomed_spectrum)] |
| 90 | freq_offset = measured_freq - expected_freq |
| 91 | offset = self.nitems_written(0) + coarse_idx + precise_idx - self.guard_period |
| 92 | key = pmt.string_to_symbol("fcch") |
| 93 | value = pmt.from_double(freq_offset) |
| 94 | self.add_item_tag(0,offset, key, value) |
| 95 | self.state=False |
| 96 | |
| 97 | # Some additional plots and prints for debugging |
| 98 | # print "coarse_idx+precise_idx",coarse_idx+precise_idx |
| 99 | # print "offset-self.nitems_written(0):",offset-self.nitems_written(0) |
| 100 | print offset-self.prev_offset |
| 101 | self.prev_offset=offset |
| 102 | print "freq offset", freq_offset |
| 103 | # freq_offset = measured_freq - expected_freq |
| 104 | # plot(self.f_axis, zoomed_spectrum) |
| 105 | # show() |
| 106 | # plot(inst_freq[precise_idx:precise_idx+self.burst_size]) |
| 107 | # show() |
| 108 | # plot(unwrap(angle(in0[coarse_idx+precise_idx:coarse_idx+precise_idx+self.burst_size]))) |
| 109 | # show() |
| 110 | # |
| 111 | return len(output_items[0]) |
| 112 | |
| 113 | def find_best_position(self, inst_freq): |
| 114 | lowest_max_min_diff = 1e6 #1e6 - just some large value |
| 115 | start_pos = 0 |
| 116 | |
piotr | 75c2f5c | 2014-07-20 23:51:28 +0200 | [diff] [blame] | 117 | for ii in xrange(0,int(2*self.guard_period)): |
piotr | 6b78abc | 2014-07-08 23:29:13 +0200 | [diff] [blame] | 118 | min_inst_freq = min(inst_freq[ii:self.processed_block_size+ii-1]); |
| 119 | max_inst_freq = max(inst_freq[ii:self.processed_block_size+ii-1]); |
| 120 | |
| 121 | if (lowest_max_min_diff > max_inst_freq - min_inst_freq): |
| 122 | lowest_max_min_diff = max_inst_freq - min_inst_freq; |
| 123 | start_pos = ii |
| 124 | # print 'start_pos',start_pos |
| 125 | |
| 126 | # plot(xrange(start_pos,start_pos+self.processed_block_size),inst_freq[start_pos:start_pos+self.processed_block_size],'r.') |
| 127 | # hold(True) |
| 128 | # plot(inst_freq) |
| 129 | # show() |
| 130 | |
| 131 | return start_pos |