Added fft-beacon-finder block

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

blocks/costas-beacon-sync.cpp

@@ -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;
-
-    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;
 }

blocks/costas-beacon-sync.h

@@ -5,18 +5,19 @@
 #include <liquid/liquid.h>
 
 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);

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<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;
+    }
+}

blocks/fft-beacon-finder.h

@@ -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