diff --git a/blocks/costas.cpp b/blocks/costas.cpp
new file mode 100644
index 0000000..5a2fc96
--- /dev/null
+++ b/blocks/costas.cpp
@@ -0,0 +1,71 @@
+#include "costas.h"
+
+#include<cmath>
+#include <liquid/liquid.h>
+
+const size_t banpass_len = 47;
+
+// 512000 ksps, pass end 10kHz, stop start 25khz, 40dB
+float bandpass_taps[banpass_len] = {
+    0.0023692550603300333,0.002578944666311145,0.003062514355406165,0.0038352811243385077,
+    0.004904961679130793,0.00627117557451129,0.007925170473754406,0.009849807247519493,
+    0.012019773945212364,0.014402060769498348,0.016956664621829987,0.01963750831782818,
+    0.022393573075532913,0.025170154869556427,0.027910303324460983,0.03055628389120102,
+    0.03305114805698395,0.03534023091197014,0.03737267851829529,0.039102789014577866,
+    0.04049133509397507,0.04150659218430519,0.04212525859475136,0.042333073914051056,
+    0.04212525859475136,0.04150659218430519,0.04049133509397507,0.039102789014577866,
+    0.03737267851829529,0.03534023091197014,0.03305114805698395,0.03055628389120102,
+    0.027910303324460983,0.025170154869556427,0.022393573075532913,0.01963750831782818,
+    0.016956664621829987,0.014402060769498348,0.012019773945212364,0.009849807247519493,
+    0.007925170473754406,0.00627117557451129,0.004904961679130793,0.0038352811243385077,
+    0.003062514355406165,0.002578944666311145,0.0023692550603300333};
+
+CostasBeaconSync::CostasBeaconSync(float agc_bw, float loop_bw, float min_freq, float max_freq) {
+    this->min_freq = min_freq;
+    this->max_freq = max_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;
+
+    this->loop_freq = 0.0;
+    this->loop_phase = 0.0;
+
+    this->bandpass = firfilt_crcf_create(bandpass_taps,banpass_len);
+
+    this->agc = agc_crcf_create();
+    agc_crcf_set_bandwidth(this->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);
+
+    std::complex<float> out = std::complex<float>(cos(-this->loop_phase),sin(-this->loop_phase));
+
+    agc_crcf_execute(this->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;
+
+
+    return out;
+}
diff --git a/blocks/costas.h b/blocks/costas.h
new file mode 100644
index 0000000..bfabf47
--- /dev/null
+++ b/blocks/costas.h
@@ -0,0 +1,30 @@
+#ifndef _COSTAS_
+#define _COSTAS_
+
+#include <complex>
+#include <liquid/liquid.h>
+
+class CostasBeaconSync {
+
+
+    float alpha;
+    float beta;
+
+    float min_freq;
+    float max_freq;
+
+    float loop_freq;
+    float loop_phase;
+
+    firfilt_crcf bandpass;
+
+    agc_crcf agc;
+
+    public:
+        CostasBeaconSync(float, float, float, float);
+
+        std::complex<float> work(liquid_float_complex);
+};
+
+
+#endif
diff --git a/fft-beacon-finder/test_fft.c b/fft-beacon-finder/test_fft.c
new file mode 100644
index 0000000..9f8ad80
--- /dev/null
+++ b/fft-beacon-finder/test_fft.c
@@ -0,0 +1,63 @@
+#include <liquid/liquid.h>
+#include <math.h>
+#include <complex.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+const float SAMPLINGRATE = 1000000.0;
+const float TEST_TONE = 0.0;
+
+const int FFT_SIZE = 1024;
+
+int main() {
+
+    nco_crcf test_tone = nco_crcf_create(LIQUID_NCO);
+    nco_crcf_set_phase(test_tone, 0.0f);
+    float freq = (2 * M_PI * TEST_TONE) / SAMPLINGRATE;
+    nco_crcf_set_frequency(test_tone, freq);
+
+
+    // allocated memory arrays
+    float complex * x = (float complex*) malloc(FFT_SIZE * sizeof(float complex));
+    float complex * y = (float complex*) malloc(FFT_SIZE * sizeof(float complex));
+
+    // create FFT plan
+    fftplan fft = fft_create_plan(FFT_SIZE, x, y, LIQUID_FFT_FORWARD, 0);
+
+    for(int i = 0; i < FFT_SIZE; i++) {
+        nco_crcf_step(test_tone);
+        nco_crcf_cexpf(test_tone, &x[i]);
+    }
+
+    fft_execute(fft);
+
+    float mag_max = cabsf(y[0]);
+    for(int i = 0; i < FFT_SIZE; i++) {
+        float mag = cabsf(y[i]);
+        if(mag >  mag_max) {
+            mag_max = mag;
+        }
+    }
+
+    for(int i = 0; i < FFT_SIZE; i++) {
+        float mag = cabsf(y[i]);
+        mag = mag / mag_max;
+
+        float freq;
+        if(i < FFT_SIZE/2) {
+            freq = i * SAMPLINGRATE / FFT_SIZE;
+        } else {
+            freq = (i-FFT_SIZE/2) * SAMPLINGRATE / FFT_SIZE - SAMPLINGRATE/2;
+        }
+        printf("%f %f\n", freq, mag);
+    }
+
+
+    nco_crcf_destroy(test_tone);
+    fft_destroy_plan(fft);
+
+    free(x);
+    free(y);
+
+    return 0;
+}