mirror of
https://github.com/SatDump/SatDump
synced 2026-08-13 17:47:30 -04:00
41 lines
973 B
C++
41 lines
973 B
C++
|
#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);
|
||
|
|
}
|
||
|
|
}
|