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
|
#include "vco.h"
#include <volk/volk.h>
#include <cmath>
#ifndef M_PI
#define M_PI 3.14159265358979323846 /* pi */
#endif
namespace dsp
{
VCOBlock::VCOBlock(std::shared_ptr<dsp::stream<float>> input, float k, float amp) : Block(input), d_k(k), d_amp(amp)
{
}
void VCOBlock::work()
{
int nsamples = input_stream->read();
if (nsamples <= 0)
{
input_stream->flush();
return;
}
for (int i = 0; i < nsamples; i++)
{
output_stream->writeBuf[i].real = cosf(d_phase) * d_amp;
output_stream->writeBuf[i].imag = sinf(d_phase) * d_amp;
d_phase += input_stream->readBuf[i] * d_k;
// Wrap phase
while (d_phase > (2 * M_PI))
d_phase -= 2 * M_PI;
while (d_phase < (-2 * M_PI))
d_phase += 2 * M_PI;
}
input_stream->flush();
output_stream->swap(nsamples);
}
}
|
