From 97e782c9ce3c33a0f2844e7a4653e88f0ed030f6 Mon Sep 17 00:00:00 2001
From: Sebastian <sebastian@sebastians-site.de>
Date: Thu, 15 Jun 2023 00:04:44 +0200
Subject: [PATCH] Added FFT

---
 Cargo.toml  |  1 +
 src/main.rs | 59 ++++++++++++++++++++++++++++++++++++++++++-----------
 2 files changed, 48 insertions(+), 12 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index 45dfdf0..356319e 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -17,6 +17,7 @@ arrayvec = {version = "0.7.0", default-features = false}
 systick-monotonic = "1.0.0"
 num-traits = { version = "0.2", default-features = false, features = ["libm"] }
 num = {version = "0.4", default-features = false}
+microfft = "0.5.1"
 
 
 [features]
diff --git a/src/main.rs b/src/main.rs
index 0c82ab0..ec516b1 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -41,7 +41,8 @@ mod app {
     };
 
     use arrayvec::ArrayString;
-    use num_traits::float::Float;
+    use microfft::complex::cfft_128;
+    use num_traits::{float::Float, Pow};
 
     use crate::filters;
     use crate::si5153;
@@ -209,31 +210,65 @@ mod app {
         )
     }
 
-    #[task(local=[board_led, iq_buffer, i_offset, q_offset, board_led, usb_filter])]
+    #[task(local=[board_led, iq_buffer, board_led, usb_filter])]
     async fn receiver_task(ctx: receiver_task::Context) {
         defmt::info!("Start receiver_task!");
 
+        let mut i_offset = 0.0;
+        let mut q_offset = 0.0;
+
         let mut expected_half = dma::Half::First;
 
         loop {
             while ctx.local.iq_buffer.readable_half().unwrap() != expected_half {}
             ctx.local.board_led.set_low();
 
-            let half = ctx.local.iq_buffer.peek(|half, _| *half).unwrap();
+            let samples = ctx
+                .local
+                .iq_buffer
+                .peek(|half, _| {
+                    let mut samples = [Complex::<f32>::default(); 128];
+                    for idx in 0..half.len() / 2 {
+                        let q_raw = half[idx * 2];
+                        let i_raw = half[idx * 2 + 1];
 
-            for idx in 0..half.len() / 2 {
-                let q_raw = half[idx * 2];
-                let i_raw = half[idx * 2 + 1];
+                        i_offset = 0.95 * i_offset + 0.05 * (i_raw as f32);
+                        q_offset = 0.95 * q_offset + 0.05 * (q_raw as f32);
 
-                *ctx.local.i_offset = 0.95 * *ctx.local.i_offset + 0.05 * (i_raw as f32);
-                *ctx.local.q_offset = 0.95 * *ctx.local.q_offset + 0.05 * (q_raw as f32);
+                        let i_sample = (i_raw as f32) - i_offset;
+                        let q_sample = (q_raw as f32) - q_offset;
 
-                let i_sample = (i_raw as f32) - *ctx.local.i_offset;
-                let q_sample = (q_raw as f32) - *ctx.local.q_offset;
+                        samples[idx] =
+                            Complex::new(i_sample as f32 / 4096.0, q_sample as f32 / 4096.0);
+                    }
+                    samples
+                })
+                .unwrap();
 
-                let sample = Complex::new(i_sample as f32 / 4096.0, q_sample as f32 / 4096.0);
-                let _filtered = ctx.local.usb_filter.compute(sample) * 2.0;
+            let mut fft_input = [Complex::<f32>::default(); 128];
+            for idx in 0..samples.len() / 2 {
+                let _filtered = ctx.local.usb_filter.compute(samples[idx]) * 2.0;
+                fft_input[idx] = samples[idx];
             }
+
+            let spectrum = cfft_128(&mut fft_input);
+
+            let mut max_idx: usize = 0;
+            let mut max_mag = 0.0;
+            for idx in 0..spectrum.len() {
+                let mag_cur = ((spectrum[idx].re.pow(2) + spectrum[idx].im.pow(2)) as f32).sqrt();
+
+                if mag_cur > max_mag {
+                    max_idx = idx;
+                    max_mag = mag_cur;
+                }
+            }
+            defmt::debug!(
+                "Max at {}kHz: {}",
+                max_idx as f32 * (8.0 / 128.0) - 4.0,
+                max_mag
+            );
+
             ctx.local.board_led.set_high();
 
             expected_half = if expected_half == dma::Half::First {