Added fft relock on demand

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<float> CostasBeaconSync::work(std::complex<float> 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<float> CostasBeaconSync::work(std::complex<float> in) {
     return out;
 }
 
+bool CostasBeaconSync::has_lock() {
+    return locked;
+}
+
 CostasBeaconSync::~CostasBeaconSync() {
     firfilt_crcf_destroy(bandpass);
 }

blocks/costas-beacon-sync.h

@@ -18,11 +18,15 @@ class CostasBeaconSync {
 
         firfilt_crcf bandpass;
 
+        bool locked;
+
     public:
         CostasBeaconSync(float, float, float);
         ~CostasBeaconSync();
 
         std::complex<float> work(liquid_float_complex);
+
+        bool has_lock();
 };
 
 #endif

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<float> FFTBeaconFinder::work(std::complex<float> in) {
-    if (next_fft_in <= 0) {
+    if (next_fft_in <= 0 && !has_sync) {
-        fft_in[pos] = in;
+            fft_in[pos] = in;
-        pos += 1;
+            pos += 1;
-        if (pos == FFT_LEN) {
+            if (pos == FFT_LEN) {
-            pos = 0;
+                pos = 0;
-            fft_execute(fft);
+                fft_execute(fft);
 
-            float fft_max = std::abs(fft_out[0]);
+                float fft_max = std::abs(fft_out[0]);
-            for (int i = 0; i < FFT_LEN; i++) {
+                for (int i = 0; i < FFT_LEN; i++) {
-                float mag = std::abs(fft_out[i]);
+                    float mag = std::abs(fft_out[i]);
-                if (mag > fft_max) {
+                    if (mag > fft_max) {
-                    fft_max = mag;
+                        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;
-                        }
                     }
                 }
 
-                if (max_levels > 0.0) {
+                std::cout << std::endl;
-                    last_center = max_center;
 
-                    float center_freq =
+                int last_center_idx = spectral_bin_to_fft_idx(last_center);
-                        float(max_center) / FFT_LEN * samplingrate;
+                float last_center_val =
-                    nco_crcf_set_frequency(
+                    std::abs(fft_out[last_center_idx]) / fft_max;
-                        coarse_correction,
-                        -(2 * M_PI * center_freq) / samplingrate);
 
-                    std::cout << "New center " << center_freq << " "
+                if (last_center_val < 0.25) {
-                              << max_center << std::endl;
+                    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;
+                next_fft_in = samplingrate / 4;
-        }
+            }
-    } else {
+    } 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);

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<float> work(std::complex<float>);
 };
 

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'

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<float>), buffer_size, input);
     while (len > 0) {
         for (size_t i = 0; i < len; i++) {
-            std::complex<float> coarse_synced = finder.work(in_buffer[i]);
+            std::complex<float> coarse_synced =
+                finder.work(in_buffer[i]);
             std::complex<float> 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;
         }