87 lines
2.9 KiB
C++
87 lines
2.9 KiB
C++
#include "costas-beacon-sync.h"
|
|
|
|
#include <iostream>
|
|
|
|
const size_t banpass_len = 47;
|
|
|
|
// 512000 ksps, pass end 10kHz, stop start 25khz, 40dB
|
|
float bandpass_taps[banpass_len] = {
|
|
0.0023692550603300333, 0.002578944666311145, 0.003062514355406165,
|
|
0.0038352811243385077, 0.004904961679130793, 0.00627117557451129,
|
|
0.007925170473754406, 0.009849807247519493, 0.012019773945212364,
|
|
0.014402060769498348, 0.016956664621829987, 0.01963750831782818,
|
|
0.022393573075532913, 0.025170154869556427, 0.027910303324460983,
|
|
0.03055628389120102, 0.03305114805698395, 0.03534023091197014,
|
|
0.03737267851829529, 0.039102789014577866, 0.04049133509397507,
|
|
0.04150659218430519, 0.04212525859475136, 0.042333073914051056,
|
|
0.04212525859475136, 0.04150659218430519, 0.04049133509397507,
|
|
0.039102789014577866, 0.03737267851829529, 0.03534023091197014,
|
|
0.03305114805698395, 0.03055628389120102, 0.027910303324460983,
|
|
0.025170154869556427, 0.022393573075532913, 0.01963750831782818,
|
|
0.016956664621829987, 0.014402060769498348, 0.012019773945212364,
|
|
0.009849807247519493, 0.007925170473754406, 0.00627117557451129,
|
|
0.004904961679130793, 0.0038352811243385077, 0.003062514355406165,
|
|
0.002578944666311145, 0.0023692550603300333};
|
|
|
|
CostasBeaconSync::CostasBeaconSync(float loop_bw,
|
|
float min_freq,
|
|
float max_freq) {
|
|
this->min_freq = min_freq;
|
|
this->max_freq = max_freq;
|
|
|
|
float damping = sqrtf(2.0f) / 2.0f;
|
|
float denom = (1.0 + 2.0 * damping * loop_bw + loop_bw * loop_bw);
|
|
alpha = (4 * damping * loop_bw) / denom;
|
|
beta = (4 * loop_bw * loop_bw) / denom;
|
|
|
|
std::cout << "Alpha: " << alpha << std::endl;
|
|
std::cout << "Beta: " << beta << std::endl;
|
|
std::cout << "Min f: " << min_freq << std::endl;
|
|
std::cout << "Max f: " << max_freq << std::endl;
|
|
|
|
loop_freq = 0.0;
|
|
loop_phase = 0.0;
|
|
|
|
locked = false;
|
|
|
|
bandpass = firfilt_crcf_create(bandpass_taps, banpass_len);
|
|
}
|
|
|
|
std::complex<float> CostasBeaconSync::work(std::complex<float> in) {
|
|
std::complex<float> filtered;
|
|
firfilt_crcf_push(bandpass, in); // push input sample
|
|
firfilt_crcf_execute(bandpass, &filtered);
|
|
|
|
std::complex<float> out =
|
|
std::complex<float>(cos(-loop_phase), sin(-loop_phase));
|
|
|
|
std::complex<float> costas_sample = filtered * out;
|
|
|
|
float error = costas_sample.imag() * costas_sample.real();
|
|
|
|
locked = std::abs(error) < 0.001;
|
|
|
|
loop_freq += beta * error;
|
|
loop_phase += loop_freq + alpha * error;
|
|
|
|
while (loop_phase > (2 * M_PI))
|
|
loop_phase -= 2 * M_PI;
|
|
while (loop_phase < (-2 * M_PI))
|
|
loop_phase += 2 * M_PI;
|
|
|
|
if (loop_freq > max_freq)
|
|
loop_freq = max_freq;
|
|
else if (loop_freq < min_freq)
|
|
loop_freq = min_freq;
|
|
|
|
return out;
|
|
}
|
|
|
|
bool CostasBeaconSync::has_lock() {
|
|
return locked;
|
|
}
|
|
|
|
CostasBeaconSync::~CostasBeaconSync() {
|
|
firfilt_crcf_destroy(bandpass);
|
|
}
|