Added fft-beacon-finder block
This commit is contained in:
parent
effea96230
commit
b37ce97c4e
|
@ -1,13 +1,16 @@
|
|||
all: .depends costas.o
|
||||
all: .depends costas-beacon-sync.o fft-beacon-finder.o
|
||||
|
||||
CXX=clang++
|
||||
CXXFLAGS=-Wall
|
||||
LDFLAGS=-lliquid
|
||||
|
||||
.depends: *.cpp
|
||||
$(CXX) -o .depends -M *.cpp
|
||||
$(CXX) -M *.cpp > .depends
|
||||
|
||||
%.o : %.cpp
|
||||
$(CXX) $(CFLAGS) -c $< -o $@
|
||||
|
||||
clean:
|
||||
rm *.o
|
||||
|
||||
include .depends
|
||||
|
|
|
@ -1,4 +1,4 @@
|
|||
#include "costas.h"
|
||||
#include "costas-beacon-sync.h"
|
||||
|
||||
#include<cmath>
|
||||
#include <liquid/liquid.h>
|
||||
|
@ -26,46 +26,45 @@ CostasBeaconSync::CostasBeaconSync(float agc_bw, float loop_bw, float min_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;
|
||||
alpha = (4 * damping * loop_bw) / denom;
|
||||
beta = (4 * loop_bw * loop_bw) / denom;
|
||||
|
||||
this->loop_freq = 0.0;
|
||||
this->loop_phase = 0.0;
|
||||
loop_freq = 0.0;
|
||||
loop_phase = 0.0;
|
||||
|
||||
this->bandpass = firfilt_crcf_create(bandpass_taps,banpass_len);
|
||||
bandpass = firfilt_crcf_create(bandpass_taps,banpass_len);
|
||||
|
||||
this->agc = agc_crcf_create();
|
||||
agc_crcf_set_bandwidth(this->agc, agc_bw);
|
||||
agc = agc_crcf_create();
|
||||
agc_crcf_set_bandwidth(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);
|
||||
firfilt_crcf_push(bandpass, in); // push input sample
|
||||
firfilt_crcf_execute(bandpass,&filtered);
|
||||
|
||||
std::complex<float> out = std::complex<float>(cos(-this->loop_phase),sin(-this->loop_phase));
|
||||
std::complex<float> out = std::complex<float>(cos(-loop_phase),sin(-loop_phase));
|
||||
|
||||
agc_crcf_execute(this->agc, filtered, &filtered);
|
||||
agc_crcf_execute(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;
|
||||
loop_freq += beta * error;
|
||||
loop_phase += 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;
|
||||
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;
|
||||
}
|
|
@ -5,6 +5,7 @@
|
|||
#include <liquid/liquid.h>
|
||||
|
||||
class CostasBeaconSync {
|
||||
private:
|
||||
float alpha;
|
||||
float beta;
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
#include "fft-beacon-finder.h"
|
||||
|
||||
FFTBeaconFinder::FFTBeaconFinder(int samplingrate) {
|
||||
this->samplingrate = samplingrate;
|
||||
|
||||
coarse_correction = nco_crcf_create(LIQUID_NCO);
|
||||
nco_crcf_set_frequency(coarse_correction, 0.0f);
|
||||
|
||||
fft = fft_create_plan(FFT_LEN, fft_in, fft_out,
|
||||
LIQUID_FFT_FORWARD, 0);
|
||||
|
||||
pos = 0;
|
||||
next_fft_in = 0;
|
||||
}
|
||||
|
||||
std::complex<float> FFTBeaconFinder::work(std::complex<float> in) {
|
||||
if(next_fft_in <= 0) {
|
||||
fft_in[pos] = in;
|
||||
pos += 1;
|
||||
if(pos == FFT_LEN) {
|
||||
pos = 0;
|
||||
fft_execute(fft);
|
||||
|
||||
float fft_max = std::abs(fft_out[0]);
|
||||
for(int i = 0; i < FFT_LEN; i++) {
|
||||
float mag = std::abs(fft_out[i]);
|
||||
if(mag > fft_max) {
|
||||
fft_max = mag;
|
||||
}
|
||||
}
|
||||
|
||||
float max_levels = 0;
|
||||
int max_center = 0;
|
||||
for(int bin = -50; bin <= 50; bin++) {
|
||||
int center_idx = spectral_bin_to_fft_idx(bin);
|
||||
float center_val = std::abs(fft_out[center_idx]) / fft_max;
|
||||
if(center_val > 0.25) {
|
||||
printf("Found peak candidate at %d\n", bin);
|
||||
int left_idx = spectral_bin_to_fft_idx(bin - 127);
|
||||
int right_idx = spectral_bin_to_fft_idx(bin + 127);
|
||||
float left_val = std::abs(fft_out[left_idx]) / fft_max;
|
||||
float right_val = std::abs(fft_out[right_idx]) / fft_max;
|
||||
|
||||
if(center_val + left_val + right_val > max_levels) {
|
||||
max_levels = center_val + left_val + right_val;
|
||||
max_center = bin;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(max_levels > 0.0) {
|
||||
float center_freq = max_center * samplingrate / FFT_LEN;
|
||||
printf("Found center at %f\n", center_freq);
|
||||
nco_crcf_set_frequency(coarse_correction, -(2 * M_PI * center_freq) / samplingrate);
|
||||
}
|
||||
|
||||
next_fft_in = samplingrate / 4;
|
||||
}
|
||||
} else {
|
||||
next_fft_in--;
|
||||
}
|
||||
|
||||
std::complex<float> y;
|
||||
// increment internal phase
|
||||
nco_crcf_step(coarse_correction);
|
||||
// compute complex exponential
|
||||
nco_crcf_cexpf(coarse_correction, &y);
|
||||
|
||||
return y * in;
|
||||
}
|
||||
|
||||
|
||||
int FFTBeaconFinder::spectral_bin_to_fft_idx(int bin) {
|
||||
if(bin == 0) {
|
||||
return FFT_LEN/2;
|
||||
} else if(bin > 0) {
|
||||
return bin - 1;
|
||||
} else {
|
||||
return bin + FFT_LEN;
|
||||
}
|
||||
}
|
|
@ -0,0 +1,31 @@
|
|||
#ifndef _FFT_BEACON_FINDER_
|
||||
#define _FFT_BEACON_FINDER_
|
||||
|
||||
#include <complex>
|
||||
#include <liquid/liquid.h>
|
||||
|
||||
const size_t FFT_LEN = 512;
|
||||
|
||||
class FFTBeaconFinder {
|
||||
private:
|
||||
int samplingrate;
|
||||
|
||||
nco_crcf coarse_correction;
|
||||
fftplan fft;
|
||||
|
||||
std::complex<float> fft_in[FFT_LEN];
|
||||
std::complex<float> fft_out[FFT_LEN];
|
||||
|
||||
size_t pos;
|
||||
int next_fft_in;
|
||||
|
||||
int spectral_bin_to_fft_idx(int);
|
||||
|
||||
public:
|
||||
FFTBeaconFinder(int);
|
||||
|
||||
std::complex<float> work(std::complex<float>);
|
||||
};
|
||||
|
||||
|
||||
#endif
|
Loading…
Reference in New Issue