1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
|
/* -*- c++ -*- */
/*
* Copyright 2014 Free Software Foundation, Inc.
*
* This file is part of GNU Radio
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "rotator_cc_impl.h"
#include <gnuradio/io_signature.h>
#include <cmath>
namespace gr {
namespace blocks {
rotator_cc::sptr rotator_cc::make(double phase_inc, bool tag_inc_updates)
{
return gnuradio::make_block_sptr<rotator_cc_impl>(phase_inc, tag_inc_updates);
}
rotator_cc_impl::rotator_cc_impl(double phase_inc, bool tag_inc_updates)
: sync_block("rotator_cc",
io_signature::make(1, 1, sizeof(gr_complex)),
io_signature::make(1, 1, sizeof(gr_complex))),
d_tag_inc_updates(tag_inc_updates),
d_inc_update_queue(cmp_phase_inc_update_offset)
{
set_phase_inc(phase_inc);
const pmt::pmt_t port_id = pmt::mp("cmd");
message_port_register_in(port_id);
set_msg_handler(port_id, [this](pmt::pmt_t msg) { this->handle_cmd_msg(msg); });
}
rotator_cc_impl::~rotator_cc_impl() {}
void rotator_cc_impl::set_phase_inc(double phase_inc)
{
d_r.set_phase_incr(exp(gr_complex(0, phase_inc)));
}
void rotator_cc_impl::handle_cmd_msg(pmt::pmt_t msg)
{
gr::thread::scoped_lock l(d_mutex);
static const pmt::pmt_t inc_key = pmt::intern("inc");
static const pmt::pmt_t offset_key = pmt::intern("offset");
if (!pmt::is_dict(msg)) {
throw std::runtime_error("rotator_cc: Rotator command message "
"must be a PMT dictionary");
}
bool handled = false;
if (pmt::dict_has_key(msg, inc_key)) {
/* Prepare to update the phase increment on a specific absolute sample
* offset. If the offset is not defined in the command message,
* configure the phase increment update to occur immediately. */
phase_inc_update_t update{};
update.phase_inc = pmt::to_double(pmt::dict_ref(msg, inc_key, pmt::PMT_NIL));
update.offset = pmt::dict_has_key(msg, offset_key)
? pmt::to_uint64(pmt::dict_ref(msg, offset_key, pmt::PMT_NIL))
: nitems_written(0);
d_inc_update_queue.push(update);
handled = true;
}
if (!handled) {
throw std::runtime_error("rotator_cc: Unsupported command message");
}
}
int rotator_cc_impl::work(int noutput_items,
gr_vector_const_void_star& input_items,
gr_vector_void_star& output_items)
{
gr::thread::scoped_lock l(d_mutex);
const gr_complex* in = (const gr_complex*)input_items[0];
gr_complex* out = (gr_complex*)output_items[0];
const uint64_t n_written = nitems_written(0);
static const pmt::pmt_t phase_inc_tag_key = pmt::intern("rot_phase_inc");
if (d_inc_update_queue.empty()) {
d_r.rotateN(out, in, noutput_items);
} else {
/* If there are phase increment updates scheduled for now, handle the
* rotation in steps and update the phase increment in between. */
int nprocessed_items = 0;
while (!d_inc_update_queue.empty()) {
auto next_update = d_inc_update_queue.top();
if (next_update.offset < (n_written + nprocessed_items)) {
d_inc_update_queue.pop();
continue; // we didn't process this update on time - drop it
}
if (next_update.offset >= (n_written + noutput_items)) {
break; // the update is for a future batch of samples
}
// The update is scheduled for this batch of samples. Apply it now.
d_inc_update_queue.pop();
// Process all samples until the scheduled phase increment update
int items_before_update = next_update.offset - n_written - nprocessed_items;
d_r.rotateN(out, in, items_before_update);
nprocessed_items += items_before_update;
set_phase_inc(next_update.phase_inc);
if (d_tag_inc_updates) {
add_item_tag(0,
next_update.offset,
phase_inc_tag_key,
pmt::from_float(next_update.phase_inc));
}
}
// Rotate the remaining samples
d_r.rotateN(out + nprocessed_items,
in + nprocessed_items,
(noutput_items - nprocessed_items));
}
return noutput_items;
}
} /* namespace blocks */
} /* namespace gr */
|
