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::() / 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