From 66665622bc6562a8d9085c27f2b09ef025935fd8 Mon Sep 17 00:00:00 2001 From: LongHairedHacker Date: Wed, 29 Dec 2021 14:21:10 +0100 Subject: [PATCH] Added fft relock on demand --- blocks/costas-beacon-sync.cpp | 8 +++ blocks/costas-beacon-sync.h | 4 ++ blocks/fft-beacon-finder.cpp | 125 ++++++++++++++++++---------------- blocks/fft-beacon-finder.h | 4 ++ blocks/generate-testdata.grc | 4 +- blocks/test.cpp | 9 ++- 6 files changed, 94 insertions(+), 60 deletions(-) diff --git a/blocks/costas-beacon-sync.cpp b/blocks/costas-beacon-sync.cpp index 7d1f119..ae96d01 100644 --- a/blocks/costas-beacon-sync.cpp +++ b/blocks/costas-beacon-sync.cpp @@ -42,6 +42,8 @@ CostasBeaconSync::CostasBeaconSync(float loop_bw, loop_freq = 0.0; loop_phase = 0.0; + locked = false; + bandpass = firfilt_crcf_create(bandpass_taps, banpass_len); } @@ -57,6 +59,8 @@ std::complex CostasBeaconSync::work(std::complex in) { float error = costas_sample.imag() * costas_sample.real(); + locked = std::abs(error) < 0.001; + loop_freq += beta * error; loop_phase += loop_freq + alpha * error; @@ -73,6 +77,10 @@ std::complex CostasBeaconSync::work(std::complex in) { return out; } +bool CostasBeaconSync::has_lock() { + return locked; +} + CostasBeaconSync::~CostasBeaconSync() { firfilt_crcf_destroy(bandpass); } diff --git a/blocks/costas-beacon-sync.h b/blocks/costas-beacon-sync.h index 2755c86..00d31dd 100644 --- a/blocks/costas-beacon-sync.h +++ b/blocks/costas-beacon-sync.h @@ -18,11 +18,15 @@ class CostasBeaconSync { firfilt_crcf bandpass; + bool locked; + public: CostasBeaconSync(float, float, float); ~CostasBeaconSync(); std::complex work(liquid_float_complex); + + bool has_lock(); }; #endif diff --git a/blocks/fft-beacon-finder.cpp b/blocks/fft-beacon-finder.cpp index 5030299..038f7c1 100644 --- a/blocks/fft-beacon-finder.cpp +++ b/blocks/fft-beacon-finder.cpp @@ -15,75 +15,82 @@ FFTBeaconFinder::FFTBeaconFinder(int samplingrate) { last_center = 0; last_center_level = 0.0; + + has_sync = false; } 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); + if (next_fft_in <= 0 && !has_sync) { + 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; - } - } - - std::cout << std::endl; - - int last_center_idx = spectral_bin_to_fft_idx(last_center); - float last_center_val = - std::abs(fft_out[last_center_idx]) / fft_max; - - if (last_center_val < 0.25) { - 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) { - 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; - - std::cout << bin << " " << left_val << " | " - << center_val << " | " << right_val - << std::endl; - - if (center_val + left_val + right_val > max_levels) { - max_levels = center_val + left_val + right_val; - max_center = bin; - } + 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; } } - if (max_levels > 0.0) { - last_center = max_center; + std::cout << std::endl; - float center_freq = - float(max_center) / FFT_LEN * samplingrate; - nco_crcf_set_frequency( - coarse_correction, - -(2 * M_PI * center_freq) / samplingrate); + int last_center_idx = spectral_bin_to_fft_idx(last_center); + float last_center_val = + std::abs(fft_out[last_center_idx]) / fft_max; - std::cout << "New center " << center_freq << " " - << max_center << std::endl; + if (last_center_val < 0.25) { + 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) { + 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; + + std::cout << bin << " " << left_val << " | " + << center_val << " | " << right_val + << std::endl; + + 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) { + last_center = max_center; + + float center_freq = + float(max_center) / FFT_LEN * samplingrate; + nco_crcf_set_frequency( + coarse_correction, + -(2 * M_PI * center_freq) / samplingrate); + + std::cout << "New center " << center_freq << " " + << max_center << std::endl; + has_sync = true; + } else { + std::cout << "No center found!" << std::endl; + } } else { - std::cout << "No center found!" << std::endl; + std::cout << "Last center still valid" << std::endl; + has_sync = true; } - } else { - std::cout << "Last center still valid" << std::endl; - } - next_fft_in = samplingrate / 4; - } - } else { + next_fft_in = samplingrate / 4; + } + } else if(next_fft_in > 0){ next_fft_in--; } @@ -106,6 +113,10 @@ int FFTBeaconFinder::spectral_bin_to_fft_idx(int bin) { } } +void FFTBeaconFinder::reset_sync() { + has_sync = false; +} + FFTBeaconFinder::~FFTBeaconFinder() { nco_crcf_destroy(coarse_correction); fft_destroy_plan(fft); diff --git a/blocks/fft-beacon-finder.h b/blocks/fft-beacon-finder.h index c9d79de..fd9f138 100644 --- a/blocks/fft-beacon-finder.h +++ b/blocks/fft-beacon-finder.h @@ -23,12 +23,16 @@ class FFTBeaconFinder { int last_center; float last_center_level; + bool has_sync; + int spectral_bin_to_fft_idx(int); public: FFTBeaconFinder(int); ~FFTBeaconFinder(); + void reset_sync(); + std::complex work(std::complex); }; diff --git a/blocks/generate-testdata.grc b/blocks/generate-testdata.grc index d710189..bcc9b67 100644 --- a/blocks/generate-testdata.grc +++ b/blocks/generate-testdata.grc @@ -70,7 +70,7 @@ blocks: alias: '' amp: '1' comment: '' - freq: '0.1' + freq: '0.01' maxoutbuf: '0' minoutbuf: '0' offset: '0' @@ -158,7 +158,7 @@ blocks: alias: '' amp: '1' comment: '' - freq: '-10000' + freq: '-35000' maxoutbuf: '0' minoutbuf: '0' offset: '0' diff --git a/blocks/test.cpp b/blocks/test.cpp index 6b4d48f..26af4e6 100644 --- a/blocks/test.cpp +++ b/blocks/test.cpp @@ -15,6 +15,8 @@ int main(int argc, char const* argv[]) { FFTBeaconFinder finder(1000000); CostasBeaconSync sync(0.03, -0.1, 0.1); + bool locked = false; + int sample_count = 0; clock_t start_time = clock(); @@ -25,8 +27,13 @@ int main(int argc, char const* argv[]) { fread(in_buffer, sizeof(std::complex), buffer_size, input); while (len > 0) { for (size_t i = 0; i < len; i++) { - std::complex coarse_synced = finder.work(in_buffer[i]); + std::complex coarse_synced = + finder.work(in_buffer[i]); std::complex sync_correction = sync.work(coarse_synced); + if(!sync.has_lock()) { + //std::cout << "Resetting lock" << std::endl; + finder.reset_sync(); + } out_buffer[i] = coarse_synced * sync_correction; }