72 lines
2.7 KiB
C++
72 lines
2.7 KiB
C++
#include "costas.h"
|
|
|
|
#include<cmath>
|
|
#include <liquid/liquid.h>
|
|
|
|
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 agc_bw, 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);
|
|
this->alpha = (4 * damping * loop_bw) / denom;
|
|
this->beta = (4 * loop_bw * loop_bw) / denom;
|
|
|
|
this->loop_freq = 0.0;
|
|
this->loop_phase = 0.0;
|
|
|
|
this->bandpass = firfilt_crcf_create(bandpass_taps,banpass_len);
|
|
|
|
this->agc = agc_crcf_create();
|
|
agc_crcf_set_bandwidth(this->agc, agc_bw);
|
|
}
|
|
|
|
|
|
std::complex<float> CostasBeaconSync::work(std::complex<float> in) {
|
|
|
|
std::complex<float> filtered;
|
|
firfilt_crcf_push(this->bandpass, in); // push input sample
|
|
firfilt_crcf_execute(this->bandpass,&filtered);
|
|
|
|
std::complex<float> out = std::complex<float>(cos(-this->loop_phase),sin(-this->loop_phase));
|
|
|
|
agc_crcf_execute(this->agc, filtered, &filtered);
|
|
|
|
std::complex<float> costas_sample = filtered * out;
|
|
|
|
float error = costas_sample.imag() * costas_sample.real();
|
|
|
|
this->loop_freq += beta * error;
|
|
this->loop_phase += this->loop_freq + alpha * error;
|
|
|
|
while(this->loop_phase > (2 * M_PI))
|
|
this->loop_phase -= 2 * M_PI;
|
|
while(this->loop_phase < (-2 * M_PI))
|
|
this->loop_phase += 2 * M_PI;
|
|
|
|
if(this->loop_freq > this->max_freq)
|
|
this->loop_freq = this->max_freq;
|
|
else if(this->loop_freq < this->min_freq)
|
|
this->loop_freq = this->min_freq;
|
|
|
|
|
|
return out;
|
|
}
|