mirror of
https://github.com/SatDump/SatDump
synced 2026-08-13 17:47:30 -04:00
64 lines
No EOL
2.3 KiB
C++
64 lines
No EOL
2.3 KiB
C++
#include "costas.h"
|
|
#include "common/dsp/block.h"
|
|
|
|
namespace satdump
|
|
{
|
|
namespace ndsp
|
|
{
|
|
CostasBlock::CostasBlock() : BlockSimple("costas_cc", {{"in", DSP_SAMPLE_TYPE_CF32}}, {{"out", DSP_SAMPLE_TYPE_CF32}}) {}
|
|
|
|
CostasBlock::~CostasBlock() {}
|
|
|
|
uint32_t CostasBlock::process(complex_t *input, uint32_t nsamples, complex_t *output)
|
|
{
|
|
complex_t *ibuf = input;
|
|
complex_t *obuf = output;
|
|
|
|
for (uint32_t i = 0; i < nsamples; i++)
|
|
{
|
|
// Mix input & VCO
|
|
tmp_val = ibuf[i] * complex_t(cosf(-phase), sinf(-phase));
|
|
obuf[i] = tmp_val;
|
|
|
|
// Calculate error
|
|
switch (order)
|
|
{
|
|
case 2: // Order 2, BPSK
|
|
error = tmp_val.real * tmp_val.imag;
|
|
break;
|
|
case 4: // Order 4, QPSK
|
|
error = (tmp_val.real > 0.0f ? 1.0f : -1.0f) * tmp_val.imag - (tmp_val.imag > 0.0f ? 1.0f : -1.0f) * tmp_val.real;
|
|
break;
|
|
case 8: // Order 8, 8-PSK
|
|
const float K = (sqrtf(2.0) - 1);
|
|
if (fabsf(tmp_val.real) >= fabsf(tmp_val.imag))
|
|
error = ((tmp_val.real > 0.0f ? 1.0f : -1.0f) * tmp_val.imag - (tmp_val.imag > 0.0f ? 1.0f : -1.0f) * tmp_val.real * K);
|
|
else
|
|
error = ((tmp_val.real > 0.0f ? 1.0f : -1.0f) * tmp_val.imag * K - (tmp_val.imag > 0.0f ? 1.0f : -1.0f) * tmp_val.real);
|
|
break;
|
|
}
|
|
|
|
// Clip error
|
|
error = dsp::branched_clip(error, 1.0);
|
|
|
|
// Compute new freq and phase.
|
|
freq += beta * error;
|
|
phase += freq + alpha * error;
|
|
|
|
// Wrap phase
|
|
while (phase > (2 * M_PI))
|
|
phase -= 2 * M_PI;
|
|
while (phase < (-2 * M_PI))
|
|
phase += 2 * M_PI;
|
|
|
|
// Clamp freq
|
|
if (freq > freq_limit_max)
|
|
freq = freq_limit_max;
|
|
if (freq < freq_limit_min)
|
|
freq = freq_limit_min;
|
|
}
|
|
|
|
return nsamples;
|
|
}
|
|
} // namespace ndsp
|
|
} // namespace satdump
|