From 64154fec8cd7497992ef0d93f319b98215b8a84e Mon Sep 17 00:00:00 2001
From: Quentin Smith <quentin@mit.edu>
Date: Sun, 21 Aug 2022 02:43:13 -0400
Subject: [PATCH] Demonstrate FFT in example

---
 examples/nrf/Cargo.toml                |  1 +
 examples/nrf/src/bin/pdm_continuous.rs | 24 +++++++++++++++++++++++-
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/examples/nrf/Cargo.toml b/examples/nrf/Cargo.toml
index d0567ba8..876dcf73 100644
--- a/examples/nrf/Cargo.toml
+++ b/examples/nrf/Cargo.toml
@@ -32,3 +32,4 @@ embedded-storage = "0.3.0"
 usbd-hid = "0.5.2"
 serde = { version = "1.0.136", default-features = false }
 num-integer = { version = "0.1.45", default-features = false }
+microfft = "0.5.0"
diff --git a/examples/nrf/src/bin/pdm_continuous.rs b/examples/nrf/src/bin/pdm_continuous.rs
index 33ba1e27..e78bc40d 100644
--- a/examples/nrf/src/bin/pdm_continuous.rs
+++ b/examples/nrf/src/bin/pdm_continuous.rs
@@ -3,11 +3,13 @@
 #![feature(type_alias_impl_trait)]
 
 use defmt::info;
+use core::cmp::Ordering;
 use embassy_executor::Spawner;
 use embassy_nrf::interrupt;
 use embassy_nrf::pdm::{Config, Channels, Pdm, SamplerState, Frequency, Ratio};
 use fixed::types::I7F1;
 use num_integer::Roots;
+use microfft::real::rfft_1024;
 use {defmt_rtt as _, panic_probe as _};
 
 // Demonstrates both continuous sampling and scanning multiple channels driven by a PPI linked timer
@@ -36,8 +38,10 @@ async fn main(_p: Spawner) {
                 // and set the sample buffer size accordingly. Exceeding this
                 // time can lead to the peripheral re-writing the other buffer.
                 let mean = (buf.iter().map(|v| i32::from(*v)).sum::<i32>() / buf.len() as i32) as i16;
+                let (peak_freq_index, peak_mag) = fft_peak_freq(&buf);
+                let peak_freq = peak_freq_index * 16000 / buf.len();
                 info!(
-                    "{} samples, min {=i16}, max {=i16}, mean {=i16}, AC RMS {=i16}",
+                    "{} samples, min {=i16}, max {=i16}, mean {=i16}, AC RMS {=i16}, peak {} @ {} Hz",
                     buf.len(),
                     buf.iter().min().unwrap(),
                     buf.iter().max().unwrap(),
@@ -45,9 +49,27 @@ async fn main(_p: Spawner) {
                     (
                         buf.iter().map(|v| i32::from(*v - mean).pow(2)).fold(0i32, |a,b| a.saturating_add(b))
                     / buf.len() as i32).sqrt() as i16,
+                    peak_mag, peak_freq,
                 );
                 SamplerState::Sampled
             },
         )
         .await;
 }
+
+fn fft_peak_freq(input: &[i16; 1024]) -> (usize, u32) {
+    let mut f = [0f32; 1024];
+    for i in 0..input.len() {
+        f[i] = (input[i] as f32) / 32768.0;
+    }
+    // N.B. rfft_1024 does the FFT in-place so result is actually also a reference to f.
+    let result = rfft_1024(&mut f);
+    result[0].im = 0.0;
+
+    result
+        .iter()
+        .map(|c| ((c.norm_sqr()*32768.0) as u32).sqrt())
+        .enumerate()
+        .max_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(Ordering::Equal))
+        .unwrap()
+}
\ No newline at end of file