1 /* -*- c++ -*- */
2 /*
3 * Copyright 2009-2013 Free Software Foundation, Inc.
4 *
5 * This file is part of GNU Radio
6 *
7 * GNU Radio 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 * GNU Radio 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 GNU Radio; 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 #ifdef HAVE_CONFIG_H
24 #include "config.h"
25 #endif
26
27 #include <algorithm>
28 #include <cmath>
29 #include <cstdio>
30
31 #include "pfb_clock_sync_ccf_impl.h"
32 #include <gnuradio/io_signature.h>
33 #include <gnuradio/math.h>
34 #include <boost/format.hpp>
35 #include <boost/math/special_functions/round.hpp>
36
37 namespace gr {
38 namespace digital {
39
make(double sps,float loop_bw,const std::vector<float> & taps,unsigned int filter_size,float init_phase,float max_rate_deviation,int osps)40 pfb_clock_sync_ccf::sptr pfb_clock_sync_ccf::make(double sps,
41 float loop_bw,
42 const std::vector<float>& taps,
43 unsigned int filter_size,
44 float init_phase,
45 float max_rate_deviation,
46 int osps)
47 {
48 return gnuradio::get_initial_sptr(new pfb_clock_sync_ccf_impl(
49 sps, loop_bw, taps, filter_size, init_phase, max_rate_deviation, osps));
50 }
51
52 static int ios[] = { sizeof(gr_complex), sizeof(float), sizeof(float), sizeof(float) };
53 static std::vector<int> iosig(ios, ios + sizeof(ios) / sizeof(int));
pfb_clock_sync_ccf_impl(double sps,float loop_bw,const std::vector<float> & taps,unsigned int filter_size,float init_phase,float max_rate_deviation,int osps)54 pfb_clock_sync_ccf_impl::pfb_clock_sync_ccf_impl(double sps,
55 float loop_bw,
56 const std::vector<float>& taps,
57 unsigned int filter_size,
58 float init_phase,
59 float max_rate_deviation,
60 int osps)
61 : block("pfb_clock_sync_ccf",
62 io_signature::make(1, 1, sizeof(gr_complex)),
63 io_signature::makev(1, 4, iosig)),
64 d_updated(false),
65 d_nfilters(filter_size),
66 d_max_dev(max_rate_deviation),
67 d_osps(osps),
68 d_error(0),
69 d_out_idx(0)
70 {
71 if (taps.empty())
72 throw std::runtime_error("pfb_clock_sync_ccf: please specify a filter.\n");
73
74 // Let scheduler adjust our relative_rate.
75 // enable_update_rate(true);
76 set_tag_propagation_policy(TPP_DONT);
77
78 d_nfilters = filter_size;
79 d_sps = floor(sps);
80
81 // Set the damping factor for a critically damped system
82 d_damping = 2 * d_nfilters;
83
84 // Set the bandwidth, which will then call update_gains()
85 set_loop_bandwidth(loop_bw);
86
87 // Store the last filter between calls to work
88 // The accumulator keeps track of overflow to increment the stride correctly.
89 // set it here to the fractional difference based on the initial phaes
90 d_k = init_phase;
91 d_rate = (sps - floor(sps)) * (double)d_nfilters;
92 d_rate_i = (int)floor(d_rate);
93 d_rate_f = d_rate - (float)d_rate_i;
94 d_filtnum = (int)floor(d_k);
95
96 d_filters = std::vector<kernel::fir_filter_ccf*>(d_nfilters);
97 d_diff_filters = std::vector<kernel::fir_filter_ccf*>(d_nfilters);
98
99 // Create an FIR filter for each channel and zero out the taps
100 std::vector<float> vtaps(1, 0);
101 for (int i = 0; i < d_nfilters; i++) {
102 d_filters[i] = new kernel::fir_filter_ccf(1, vtaps);
103 d_diff_filters[i] = new kernel::fir_filter_ccf(1, vtaps);
104 }
105
106 // Now, actually set the filters' taps
107 std::vector<float> dtaps;
108 create_diff_taps(taps, dtaps);
109 set_taps(taps, d_taps, d_filters);
110 set_taps(dtaps, d_dtaps, d_diff_filters);
111
112 d_old_in = 0;
113 d_new_in = 0;
114 d_last_out = 0;
115
116 set_relative_rate((uint64_t)d_osps, (uint64_t)d_sps);
117 }
118
~pfb_clock_sync_ccf_impl()119 pfb_clock_sync_ccf_impl::~pfb_clock_sync_ccf_impl()
120 {
121 for (int i = 0; i < d_nfilters; i++) {
122 delete d_filters[i];
123 delete d_diff_filters[i];
124 }
125 }
126
check_topology(int ninputs,int noutputs)127 bool pfb_clock_sync_ccf_impl::check_topology(int ninputs, int noutputs)
128 {
129 return noutputs == 1 || noutputs == 4;
130 }
131
forecast(int noutput_items,gr_vector_int & ninput_items_required)132 void pfb_clock_sync_ccf_impl::forecast(int noutput_items,
133 gr_vector_int& ninput_items_required)
134 {
135 unsigned ninputs = ninput_items_required.size();
136 for (unsigned i = 0; i < ninputs; i++)
137 ninput_items_required[i] = (noutput_items + history()) * (d_sps / d_osps);
138 }
139
update_taps(const std::vector<float> & taps)140 void pfb_clock_sync_ccf_impl::update_taps(const std::vector<float>& taps)
141 {
142 d_updated_taps = taps;
143 d_updated = true;
144 }
145
146
147 /*******************************************************************
148 SET FUNCTIONS
149 *******************************************************************/
150
set_loop_bandwidth(float bw)151 void pfb_clock_sync_ccf_impl::set_loop_bandwidth(float bw)
152 {
153 if (bw < 0) {
154 throw std::out_of_range("pfb_clock_sync_ccf: invalid bandwidth. Must be >= 0.");
155 }
156
157 d_loop_bw = bw;
158 update_gains();
159 }
160
set_damping_factor(float df)161 void pfb_clock_sync_ccf_impl::set_damping_factor(float df)
162 {
163 if (df < 0 || df > 1.0) {
164 throw std::out_of_range(
165 "pfb_clock_sync_ccf: invalid damping factor. Must be in [0,1].");
166 }
167
168 d_damping = df;
169 update_gains();
170 }
171
set_alpha(float alpha)172 void pfb_clock_sync_ccf_impl::set_alpha(float alpha)
173 {
174 if (alpha < 0 || alpha > 1.0) {
175 throw std::out_of_range("pfb_clock_sync_ccf: invalid alpha. Must be in [0,1].");
176 }
177 d_alpha = alpha;
178 }
179
set_beta(float beta)180 void pfb_clock_sync_ccf_impl::set_beta(float beta)
181 {
182 if (beta < 0 || beta > 1.0) {
183 throw std::out_of_range("pfb_clock_sync_ccf: invalid beta. Must be in [0,1].");
184 }
185 d_beta = beta;
186 }
187
188 /*******************************************************************
189 GET FUNCTIONS
190 *******************************************************************/
191
loop_bandwidth() const192 float pfb_clock_sync_ccf_impl::loop_bandwidth() const { return d_loop_bw; }
193
damping_factor() const194 float pfb_clock_sync_ccf_impl::damping_factor() const { return d_damping; }
195
alpha() const196 float pfb_clock_sync_ccf_impl::alpha() const { return d_alpha; }
197
beta() const198 float pfb_clock_sync_ccf_impl::beta() const { return d_beta; }
199
clock_rate() const200 float pfb_clock_sync_ccf_impl::clock_rate() const { return d_rate_f; }
201
error() const202 float pfb_clock_sync_ccf_impl::error() const { return d_error; }
203
rate() const204 float pfb_clock_sync_ccf_impl::rate() const { return d_rate_f; }
205
phase() const206 float pfb_clock_sync_ccf_impl::phase() const { return d_k; }
207
208 /*******************************************************************
209 *******************************************************************/
210
update_gains()211 void pfb_clock_sync_ccf_impl::update_gains()
212 {
213 float denom = (1.0 + 2.0 * d_damping * d_loop_bw + d_loop_bw * d_loop_bw);
214 d_alpha = (4 * d_damping * d_loop_bw) / denom;
215 d_beta = (4 * d_loop_bw * d_loop_bw) / denom;
216 }
217
set_taps(const std::vector<float> & newtaps,std::vector<std::vector<float>> & ourtaps,std::vector<kernel::fir_filter_ccf * > & ourfilter)218 void pfb_clock_sync_ccf_impl::set_taps(const std::vector<float>& newtaps,
219 std::vector<std::vector<float>>& ourtaps,
220 std::vector<kernel::fir_filter_ccf*>& ourfilter)
221 {
222 int i, j;
223
224 unsigned int ntaps = newtaps.size();
225 d_taps_per_filter = (unsigned int)ceil((double)ntaps / (double)d_nfilters);
226
227 // Create d_numchan vectors to store each channel's taps
228 ourtaps.resize(d_nfilters);
229
230 // Make a vector of the taps plus fill it out with 0's to fill
231 // each polyphase filter with exactly d_taps_per_filter
232 std::vector<float> tmp_taps;
233 tmp_taps = newtaps;
234 while ((float)(tmp_taps.size()) < d_nfilters * d_taps_per_filter) {
235 tmp_taps.push_back(0.0);
236 }
237
238 // Partition the filter
239 for (i = 0; i < d_nfilters; i++) {
240 // Each channel uses all d_taps_per_filter with 0's if not enough taps to fill out
241 ourtaps[i] = std::vector<float>(d_taps_per_filter, 0);
242 for (j = 0; j < d_taps_per_filter; j++) {
243 ourtaps[i][j] = tmp_taps[i + j * d_nfilters];
244 }
245
246 // Build a filter for each channel and add it's taps to it
247 ourfilter[i]->set_taps(ourtaps[i]);
248 }
249
250 // Set the history to ensure enough input items for each filter
251 set_history(d_taps_per_filter + d_sps + d_sps);
252
253 // Make sure there is enough output space for d_osps outputs/input.
254 set_output_multiple(d_osps);
255 }
256
create_diff_taps(const std::vector<float> & newtaps,std::vector<float> & difftaps)257 void pfb_clock_sync_ccf_impl::create_diff_taps(const std::vector<float>& newtaps,
258 std::vector<float>& difftaps)
259 {
260 std::vector<float> diff_filter(3);
261 diff_filter[0] = -1;
262 diff_filter[1] = 0;
263 diff_filter[2] = 1;
264
265 float pwr = 0;
266 difftaps.clear();
267 difftaps.push_back(0);
268 for (unsigned int i = 0; i < newtaps.size() - 2; i++) {
269 float tap = 0;
270 for (unsigned int j = 0; j < diff_filter.size(); j++) {
271 tap += diff_filter[j] * newtaps[i + j];
272 }
273 difftaps.push_back(tap);
274 pwr += fabsf(tap);
275 }
276 difftaps.push_back(0);
277
278 // Normalize the taps
279 for (unsigned int i = 0; i < difftaps.size(); i++) {
280 difftaps[i] *= d_nfilters / pwr;
281 if (difftaps[i] != difftaps[i]) {
282 throw std::runtime_error(
283 "pfb_clock_sync_ccf::create_diff_taps produced NaN.");
284 }
285 }
286 }
287
taps_as_string() const288 std::string pfb_clock_sync_ccf_impl::taps_as_string() const
289 {
290 int i, j;
291 std::stringstream str;
292 str.precision(4);
293 str.setf(std::ios::scientific);
294
295 str << "[ ";
296 for (i = 0; i < d_nfilters; i++) {
297 str << "[" << d_taps[i][0] << ", ";
298 for (j = 1; j < d_taps_per_filter - 1; j++) {
299 str << d_taps[i][j] << ", ";
300 }
301 str << d_taps[i][j] << "],";
302 }
303 str << " ]" << std::endl;
304
305 return str.str();
306 }
307
diff_taps_as_string() const308 std::string pfb_clock_sync_ccf_impl::diff_taps_as_string() const
309 {
310 int i, j;
311 std::stringstream str;
312 str.precision(4);
313 str.setf(std::ios::scientific);
314
315 str << "[ ";
316 for (i = 0; i < d_nfilters; i++) {
317 str << "[" << d_dtaps[i][0] << ", ";
318 for (j = 1; j < d_taps_per_filter - 1; j++) {
319 str << d_dtaps[i][j] << ", ";
320 }
321 str << d_dtaps[i][j] << "],";
322 }
323 str << " ]" << std::endl;
324
325 return str.str();
326 }
327
taps() const328 std::vector<std::vector<float>> pfb_clock_sync_ccf_impl::taps() const { return d_taps; }
329
diff_taps() const330 std::vector<std::vector<float>> pfb_clock_sync_ccf_impl::diff_taps() const
331 {
332 return d_dtaps;
333 }
334
channel_taps(int channel) const335 std::vector<float> pfb_clock_sync_ccf_impl::channel_taps(int channel) const
336 {
337 std::vector<float> taps;
338 for (int i = 0; i < d_taps_per_filter; i++) {
339 taps.push_back(d_taps[channel][i]);
340 }
341 return taps;
342 }
343
diff_channel_taps(int channel) const344 std::vector<float> pfb_clock_sync_ccf_impl::diff_channel_taps(int channel) const
345 {
346 std::vector<float> taps;
347 for (int i = 0; i < d_taps_per_filter; i++) {
348 taps.push_back(d_dtaps[channel][i]);
349 }
350 return taps;
351 }
352
general_work(int noutput_items,gr_vector_int & ninput_items,gr_vector_const_void_star & input_items,gr_vector_void_star & output_items)353 int pfb_clock_sync_ccf_impl::general_work(int noutput_items,
354 gr_vector_int& ninput_items,
355 gr_vector_const_void_star& input_items,
356 gr_vector_void_star& output_items)
357 {
358 gr_complex* in = (gr_complex*)input_items[0];
359 gr_complex* out = (gr_complex*)output_items[0];
360
361 if (d_updated) {
362 std::vector<float> dtaps;
363 create_diff_taps(d_updated_taps, dtaps);
364 set_taps(d_updated_taps, d_taps, d_filters);
365 set_taps(dtaps, d_dtaps, d_diff_filters);
366 d_updated = false;
367 return 0; // history requirements may have changed.
368 }
369
370 float *err = NULL, *outrate = NULL, *outk = NULL;
371 if (output_items.size() == 4) {
372 err = (float*)output_items[1];
373 outrate = (float*)output_items[2];
374 outk = (float*)output_items[3];
375 }
376
377 std::vector<tag_t> tags;
378 get_tags_in_window(tags, 0, 0, d_sps * noutput_items, pmt::intern("time_est"));
379
380 int i = 0, count = 0;
381 float error_r, error_i;
382
383 // produce output as long as we can and there are enough input samples
384 while (i < noutput_items) {
385 if (!tags.empty()) {
386 size_t offset = tags[0].offset - nitems_read(0);
387 if ((offset >= (size_t)count) && (offset < (size_t)(count + d_sps))) {
388 float center = (float)pmt::to_double(tags[0].value);
389 d_k = d_nfilters * (center + (offset - count));
390
391 tags.erase(tags.begin());
392 }
393 }
394
395 while (d_out_idx < d_osps) {
396
397 d_filtnum = (int)floor(d_k);
398
399 // Keep the current filter number in [0, d_nfilters]
400 // If we've run beyond the last filter, wrap around and go to next sample
401 // If we've gone below 0, wrap around and go to previous sample
402 while (d_filtnum >= d_nfilters) {
403 d_k -= d_nfilters;
404 d_filtnum -= d_nfilters;
405 count += 1;
406 }
407 while (d_filtnum < 0) {
408 d_k += d_nfilters;
409 d_filtnum += d_nfilters;
410 count -= 1;
411 }
412
413 out[i + d_out_idx] = d_filters[d_filtnum]->filter(&in[count + d_out_idx]);
414 d_k = d_k + d_rate_i + d_rate_f; // update phase
415
416
417 // Manage Tags
418 std::vector<tag_t> xtags;
419 std::vector<tag_t>::iterator itags;
420 d_new_in = nitems_read(0) + count + d_out_idx + d_sps;
421 get_tags_in_range(xtags, 0, d_old_in, d_new_in);
422 for (itags = xtags.begin(); itags != xtags.end(); itags++) {
423 tag_t new_tag = *itags;
424 // new_tag.offset = d_last_out + d_taps_per_filter/(2*d_sps) - 2;
425 new_tag.offset = d_last_out + d_taps_per_filter / 4 - 2;
426 add_item_tag(0, new_tag);
427 }
428 d_old_in = d_new_in;
429 d_last_out = nitems_written(0) + i + d_out_idx;
430
431 d_out_idx++;
432
433 if (output_items.size() == 4) {
434 err[i] = d_error;
435 outrate[i] = d_rate_f;
436 outk[i] = d_k;
437 }
438
439 // We've run out of output items we can create; return now.
440 if (i + d_out_idx >= noutput_items) {
441 consume_each(count);
442 return i;
443 }
444 }
445
446 // reset here; if we didn't complete a full osps samples last time,
447 // the early return would take care of it.
448 d_out_idx = 0;
449
450 // Update the phase and rate estimates for this symbol
451 gr_complex diff = d_diff_filters[d_filtnum]->filter(&in[count]);
452 error_r = out[i].real() * diff.real();
453 error_i = out[i].imag() * diff.imag();
454 d_error = (error_i + error_r) / 2.0; // average error from I&Q channel
455
456 // Run the control loop to update the current phase (k) and
457 // tracking rate estimates based on the error value
458 // Interpolating here to update rates for ever sps.
459 for (int s = 0; s < d_sps; s++) {
460 d_rate_f = d_rate_f + d_beta * d_error;
461 d_k = d_k + d_rate_f + d_alpha * d_error;
462 }
463
464 // Keep our rate within a good range
465 d_rate_f = gr::branchless_clip(d_rate_f, d_max_dev);
466
467 i += d_osps;
468 count += (int)floor(d_sps);
469 }
470
471 consume_each(count);
472 return i;
473 }
474
setup_rpc()475 void pfb_clock_sync_ccf_impl::setup_rpc()
476 {
477 #ifdef GR_CTRLPORT
478 // Getters
479 add_rpc_variable(rpcbasic_sptr(
480 new rpcbasic_register_get<pfb_clock_sync_ccf, float>(alias(),
481 "error",
482 &pfb_clock_sync_ccf::error,
483 pmt::mp(-2.0f),
484 pmt::mp(2.0f),
485 pmt::mp(0.0f),
486 "",
487 "Error signal of loop",
488 RPC_PRIVLVL_MIN,
489 DISPTIME | DISPOPTSTRIP)));
490
491 add_rpc_variable(rpcbasic_sptr(
492 new rpcbasic_register_get<pfb_clock_sync_ccf, float>(alias(),
493 "rate",
494 &pfb_clock_sync_ccf::rate,
495 pmt::mp(-2.0f),
496 pmt::mp(2.0f),
497 pmt::mp(0.0f),
498 "",
499 "Rate change of phase",
500 RPC_PRIVLVL_MIN,
501 DISPTIME | DISPOPTSTRIP)));
502
503 add_rpc_variable(rpcbasic_sptr(
504 new rpcbasic_register_get<pfb_clock_sync_ccf, float>(alias(),
505 "phase",
506 &pfb_clock_sync_ccf::phase,
507 pmt::mp(0),
508 pmt::mp((int)d_nfilters),
509 pmt::mp(0),
510 "",
511 "Current filter phase arm",
512 RPC_PRIVLVL_MIN,
513 DISPTIME | DISPOPTSTRIP)));
514
515 add_rpc_variable(rpcbasic_sptr(new rpcbasic_register_get<pfb_clock_sync_ccf, float>(
516 alias(),
517 "loop bw",
518 &pfb_clock_sync_ccf::loop_bandwidth,
519 pmt::mp(0.0f),
520 pmt::mp(1.0f),
521 pmt::mp(0.0f),
522 "",
523 "Loop bandwidth",
524 RPC_PRIVLVL_MIN,
525 DISPNULL)));
526
527 // Setters
528 add_rpc_variable(rpcbasic_sptr(new rpcbasic_register_set<pfb_clock_sync_ccf, float>(
529 alias(),
530 "loop bw",
531 &pfb_clock_sync_ccf::set_loop_bandwidth,
532 pmt::mp(0.0f),
533 pmt::mp(1.0f),
534 pmt::mp(0.0f),
535 "",
536 "Loop bandwidth",
537 RPC_PRIVLVL_MIN,
538 DISPNULL)));
539 #endif /* GR_CTRLPORT */
540 }
541
542 } /* namespace digital */
543 } /* namespace gr */
544