From b37ce97c4ea8a797bfc2ac13461d8b597005cae4 Mon Sep 17 00:00:00 2001 From: Sebastian Date: Wed, 15 Dec 2021 23:24:42 +0100 Subject: [PATCH] Added fft-beacon-finder block --- blocks/Makefile | 7 +- blocks/{costas.cpp => costas-beacon-sync.cpp} | 47 ++++++----- blocks/{costas.h => costas-beacon-sync.h} | 17 ++-- blocks/fft-beacon-finder.cpp | 81 +++++++++++++++++++ blocks/fft-beacon-finder.h | 31 +++++++ 5 files changed, 149 insertions(+), 34 deletions(-) rename blocks/{costas.cpp => costas-beacon-sync.cpp} (62%) rename blocks/{costas.h => costas-beacon-sync.h} (54%) create mode 100644 blocks/fft-beacon-finder.cpp create mode 100644 blocks/fft-beacon-finder.h diff --git a/blocks/Makefile b/blocks/Makefile index 0d5ff69..29499a4 100644 --- a/blocks/Makefile +++ b/blocks/Makefile @@ -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 diff --git a/blocks/costas.cpp b/blocks/costas-beacon-sync.cpp similarity index 62% rename from blocks/costas.cpp rename to blocks/costas-beacon-sync.cpp index 5a2fc96..503d2ff 100644 --- a/blocks/costas.cpp +++ b/blocks/costas-beacon-sync.cpp @@ -1,4 +1,4 @@ -#include "costas.h" +#include "costas-beacon-sync.h" #include #include @@ -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 CostasBeaconSync::work(std::complex in) { std::complex 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 out = std::complex(cos(-this->loop_phase),sin(-this->loop_phase)); + std::complex out = std::complex(cos(-loop_phase),sin(-loop_phase)); - agc_crcf_execute(this->agc, filtered, &filtered); + agc_crcf_execute(agc, filtered, &filtered); std::complex 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; + 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; } diff --git a/blocks/costas.h b/blocks/costas-beacon-sync.h similarity index 54% rename from blocks/costas.h rename to blocks/costas-beacon-sync.h index 055c041..03f6a95 100644 --- a/blocks/costas.h +++ b/blocks/costas-beacon-sync.h @@ -5,18 +5,19 @@ #include class CostasBeaconSync { - float alpha; - float beta; + private: + float alpha; + float beta; - float min_freq; - float max_freq; + float min_freq; + float max_freq; - float loop_freq; - float loop_phase; + float loop_freq; + float loop_phase; - firfilt_crcf bandpass; + firfilt_crcf bandpass; - agc_crcf agc; + agc_crcf agc; public: CostasBeaconSync(float, float, float, float); diff --git a/blocks/fft-beacon-finder.cpp b/blocks/fft-beacon-finder.cpp new file mode 100644 index 0000000..a8f9d5d --- /dev/null +++ b/blocks/fft-beacon-finder.cpp @@ -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 FFTBeaconFinder::work(std::complex 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 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; + } +} diff --git a/blocks/fft-beacon-finder.h b/blocks/fft-beacon-finder.h new file mode 100644 index 0000000..68caf1f --- /dev/null +++ b/blocks/fft-beacon-finder.h @@ -0,0 +1,31 @@ +#ifndef _FFT_BEACON_FINDER_ +#define _FFT_BEACON_FINDER_ + +#include +#include + +const size_t FFT_LEN = 512; + +class FFTBeaconFinder { + private: + int samplingrate; + + nco_crcf coarse_correction; + fftplan fft; + + std::complex fft_in[FFT_LEN]; + std::complex fft_out[FFT_LEN]; + + size_t pos; + int next_fft_in; + + int spectral_bin_to_fft_idx(int); + + public: + FFTBeaconFinder(int); + + std::complex work(std::complex); +}; + + +#endif