diff --git a/embassy-nrf/src/pdm.rs b/embassy-nrf/src/pdm.rs index efa1fbcc..217884d1 100644 --- a/embassy-nrf/src/pdm.rs +++ b/embassy-nrf/src/pdm.rs @@ -8,12 +8,22 @@ use core::task::Poll; use embassy_hal_common::drop::OnDrop; use embassy_hal_common::{into_ref, PeripheralRef}; +use fixed::types::I7F1; use futures::future::poll_fn; use crate::chip::EASY_DMA_SIZE; use crate::gpio::sealed::Pin; use crate::gpio::{AnyPin, Pin as GpioPin}; use crate::interrupt::typelevel::Interrupt; +use crate::pac::pdm::mode::{EDGE_A, OPERATION_A}; +pub use crate::pac::pdm::pdmclkctrl::FREQ_A as Frequency; +#[cfg(any( + feature = "nrf52840", + feature = "nrf52833", + feature = "_nrf5340-app", + feature = "_nrf9160", +))] +pub use crate::pac::pdm::ratio::RATIO_A as Ratio; use crate::{interrupt, Peripheral}; /// Interrupt handler. @@ -23,7 +33,20 @@ pub struct InterruptHandler { impl interrupt::typelevel::Handler for InterruptHandler { unsafe fn on_interrupt() { - T::regs().intenclr.write(|w| w.end().clear()); + let r = T::regs(); + + if r.events_end.read().bits() != 0 { + r.intenclr.write(|w| w.end().clear()); + } + + if r.events_started.read().bits() != 0 { + r.intenclr.write(|w| w.started().clear()); + } + + if r.events_stopped.read().bits() != 0 { + r.intenclr.write(|w| w.stopped().clear()); + } + T::state().waker.wake(); } } @@ -44,10 +67,24 @@ pub enum Error { BufferZeroLength, /// PDM is not running NotRunning, + /// PDM is already running + AlreadyRunning, } static DUMMY_BUFFER: [i16; 1] = [0; 1]; +/// The state of a continuously running sampler. While it reflects +/// the progress of a sampler, it also signals what should be done +/// next. For example, if the sampler has stopped then the Pdm implementation +/// can then tear down its infrastructure. +#[derive(PartialEq)] +pub enum SamplerState { + /// The sampler processed the samples and is ready for more. + Sampled, + /// The sampler is done processing samples. + Stopped, +} + impl<'d, T: Instance> Pdm<'d, T> { /// Create PDM driver pub fn new( @@ -79,18 +116,24 @@ impl<'d, T: Instance> Pdm<'d, T> { r.psel.clk.write(|w| unsafe { w.bits(clk.psel_bits()) }); // configure - // use default for - // - gain right - // - gain left - // - clk - // - ratio + r.pdmclkctrl.write(|w| w.freq().variant(config.frequency)); + #[cfg(any( + feature = "nrf52840", + feature = "nrf52833", + feature = "_nrf5340-app", + feature = "_nrf9160", + ))] + r.ratio.write(|w| w.ratio().variant(config.ratio)); r.mode.write(|w| { - w.edge().bit(config.edge == Edge::LeftRising); - w.operation().bit(config.operation_mode == OperationMode::Mono); + w.operation().variant(config.operation_mode.into()); + w.edge().variant(config.edge.into()); w }); - r.gainl.write(|w| w.gainl().default_gain()); - r.gainr.write(|w| w.gainr().default_gain()); + + Self::_set_gain(r, config.gain_left, config.gain_right); + + // Disable all events interrupts + r.intenclr.write(|w| unsafe { w.bits(0x003F_FFFF) }); // IRQ T::Interrupt::unpend(); @@ -101,6 +144,25 @@ impl<'d, T: Instance> Pdm<'d, T> { Self { _peri: pdm } } + fn _set_gain(r: &crate::pac::pdm::RegisterBlock, gain_left: I7F1, gain_right: I7F1) { + let gain_left = gain_left + .saturating_add(I7F1::from_bits(40)) + .saturating_to_num::() + .clamp(0, 0x50); + let gain_right = gain_right + .saturating_add(I7F1::from_bits(40)) + .saturating_to_num::() + .clamp(0, 0x50); + + r.gainl.write(|w| unsafe { w.gainl().bits(gain_left) }); + r.gainr.write(|w| unsafe { w.gainr().bits(gain_right) }); + } + + /// Adjust the gain of the PDM microphone on the fly + pub fn set_gain(&mut self, gain_left: I7F1, gain_right: I7F1) { + Self::_set_gain(T::regs(), gain_left, gain_right) + } + /// Start sampling microphon data into a dummy buffer /// Usefull to start the microphon and keep it active between recording samples pub async fn start(&mut self) { @@ -198,6 +260,108 @@ impl<'d, T: Instance> Pdm<'d, T> { compiler_fence(Ordering::SeqCst); } + + /// Continuous sampling with double buffers. + /// + /// A sampler closure is provided that receives the buffer of samples, noting + /// that the size of this buffer can be less than the original buffer's size. + /// A command is return from the closure that indicates whether the sampling + /// should continue or stop. + /// + /// NOTE: The time spent within the callback supplied should not exceed the time + /// taken to acquire the samples into a single buffer. You should measure the + /// time taken by the callback and set the sample buffer size accordingly. + /// Exceeding this time can lead to samples becoming dropped. + pub async fn run_task_sampler( + &mut self, + bufs: &mut [[i16; N]; 2], + mut sampler: S, + ) -> Result<(), Error> + where + S: FnMut(&[i16; N]) -> SamplerState, + { + let r = T::regs(); + + if r.events_started.read().bits() != 0 { + return Err(Error::AlreadyRunning); + } + + r.sample + .ptr + .write(|w| unsafe { w.sampleptr().bits(bufs[0].as_mut_ptr() as u32) }); + r.sample.maxcnt.write(|w| unsafe { w.buffsize().bits(N as _) }); + + // Reset and enable the events + r.events_end.reset(); + r.events_started.reset(); + r.events_stopped.reset(); + r.intenset.write(|w| { + w.end().set(); + w.started().set(); + w.stopped().set(); + w + }); + + // Don't reorder the start event before the previous writes. Hopefully self + // wouldn't happen anyway. + compiler_fence(Ordering::SeqCst); + + r.tasks_start.write(|w| unsafe { w.bits(1) }); + + let mut current_buffer = 0; + + let mut done = false; + + let drop = OnDrop::new(|| { + r.tasks_stop.write(|w| unsafe { w.bits(1) }); + // N.B. It would be better if this were async, but Drop only support sync code. + while r.events_stopped.read().bits() != 0 {} + }); + + // Wait for events and complete when the sampler indicates it has had enough. + poll_fn(|cx| { + let r = T::regs(); + + T::state().waker.register(cx.waker()); + + if r.events_end.read().bits() != 0 { + compiler_fence(Ordering::SeqCst); + + r.events_end.reset(); + r.intenset.write(|w| w.end().set()); + + if !done { + // Discard the last buffer after the user requested a stop. + if sampler(&bufs[current_buffer]) == SamplerState::Sampled { + let next_buffer = 1 - current_buffer; + current_buffer = next_buffer; + } else { + r.tasks_stop.write(|w| unsafe { w.bits(1) }); + done = true; + }; + }; + } + + if r.events_started.read().bits() != 0 { + r.events_started.reset(); + r.intenset.write(|w| w.started().set()); + + let next_buffer = 1 - current_buffer; + r.sample + .ptr + .write(|w| unsafe { w.sampleptr().bits(bufs[next_buffer].as_mut_ptr() as u32) }); + } + + if r.events_stopped.read().bits() != 0 { + return Poll::Ready(()); + } + + Poll::Pending + }) + .await; + drop.defuse(); + Ok(()) + } } /// PDM microphone driver Config @@ -206,6 +370,20 @@ pub struct Config { pub operation_mode: OperationMode, /// On which edge the left channel should be samples pub edge: Edge, + /// Clock frequency + pub frequency: Frequency, + /// Clock ratio + #[cfg(any( + feature = "nrf52840", + feature = "nrf52833", + feature = "_nrf5340-app", + feature = "_nrf9160", + ))] + pub ratio: Ratio, + /// Gain left in dB + pub gain_left: I7F1, + /// Gain right in dB + pub gain_right: I7F1, } impl Default for Config { @@ -213,6 +391,16 @@ impl Default for Config { Self { operation_mode: OperationMode::Mono, edge: Edge::LeftFalling, + frequency: Frequency::DEFAULT, + #[cfg(any( + feature = "nrf52840", + feature = "nrf52833", + feature = "_nrf5340-app", + feature = "_nrf9160", + ))] + ratio: Ratio::RATIO80, + gain_left: I7F1::ZERO, + gain_right: I7F1::ZERO, } } } @@ -226,6 +414,15 @@ pub enum OperationMode { Stereo, } +impl From for OPERATION_A { + fn from(mode: OperationMode) -> Self { + match mode { + OperationMode::Mono => OPERATION_A::MONO, + OperationMode::Stereo => OPERATION_A::STEREO, + } + } +} + /// PDM edge polarity #[derive(PartialEq)] pub enum Edge { @@ -235,6 +432,15 @@ pub enum Edge { LeftFalling, } +impl From for EDGE_A { + fn from(edge: Edge) -> Self { + match edge { + Edge::LeftRising => EDGE_A::LEFT_RISING, + Edge::LeftFalling => EDGE_A::LEFT_FALLING, + } + } +} + impl<'d, T: Instance> Drop for Pdm<'d, T> { fn drop(&mut self) { let r = T::regs(); diff --git a/examples/nrf52840/Cargo.toml b/examples/nrf52840/Cargo.toml index 7b9c371b..9b41ec5a 100644 --- a/examples/nrf52840/Cargo.toml +++ b/examples/nrf52840/Cargo.toml @@ -43,6 +43,7 @@ embassy-net-esp-hosted = { version = "0.1.0", path = "../../embassy-net-esp-host defmt = "0.3" defmt-rtt = "0.4" +fixed = "1.10.0" static_cell = "1.1" cortex-m = { version = "0.7.6", features = ["inline-asm", "critical-section-single-core"] } cortex-m-rt = "0.7.0" @@ -53,6 +54,8 @@ embedded-storage = "0.3.0" usbd-hid = "0.6.0" serde = { version = "1.0.136", default-features = false } embedded-hal-async = { version = "0.2.0-alpha.2", optional = true } +num-integer = { version = "0.1.45", default-features = false } +microfft = "0.5.0" [patch.crates-io] lora-phy = { git = "https://github.com/embassy-rs/lora-phy", rev = "ad289428fd44b02788e2fa2116445cc8f640a265" } diff --git a/examples/nrf52840/src/bin/pdm.rs b/examples/nrf52840/src/bin/pdm.rs index 6b41320c..444b9137 100644 --- a/examples/nrf52840/src/bin/pdm.rs +++ b/examples/nrf52840/src/bin/pdm.rs @@ -7,6 +7,8 @@ use embassy_executor::Spawner; use embassy_nrf::pdm::{self, Config, Pdm}; use embassy_nrf::{bind_interrupts, peripherals}; use embassy_time::{Duration, Timer}; +use fixed::types::I7F1; +use num_integer::Roots; use {defmt_rtt as _, panic_probe as _}; bind_interrupts!(struct Irqs { @@ -20,18 +22,36 @@ async fn main(_p: Spawner) { let mut pdm = Pdm::new(p.PDM, Irqs, p.P0_01, p.P0_00, config); loop { - pdm.start().await; + for gain in [I7F1::from_num(-20), I7F1::from_num(0), I7F1::from_num(20)] { + pdm.set_gain(gain, gain); + info!("Gain = {} dB", defmt::Debug2Format(&gain)); + pdm.start().await; - // wait some time till the microphon settled - Timer::after(Duration::from_millis(1000)).await; + // wait some time till the microphon settled + Timer::after(Duration::from_millis(1000)).await; - const SAMPLES: usize = 2048; - let mut buf = [0i16; SAMPLES]; - pdm.sample(&mut buf).await.unwrap(); + const SAMPLES: usize = 2048; + let mut buf = [0i16; SAMPLES]; + pdm.sample(&mut buf).await.unwrap(); - info!("samples: {:?}", &buf); + let mean = (buf.iter().map(|v| i32::from(*v)).sum::() / buf.len() as i32) as i16; + info!( + "{} samples, min {=i16}, max {=i16}, mean {=i16}, AC RMS {=i16}", + buf.len(), + buf.iter().min().unwrap(), + buf.iter().max().unwrap(), + mean, + (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, + ); - pdm.stop().await; - Timer::after(Duration::from_millis(100)).await; + info!("samples: {:?}", &buf); + + pdm.stop().await; + Timer::after(Duration::from_millis(100)).await; + } } } diff --git a/examples/nrf52840/src/bin/pdm_continuous.rs b/examples/nrf52840/src/bin/pdm_continuous.rs new file mode 100644 index 00000000..7d853147 --- /dev/null +++ b/examples/nrf52840/src/bin/pdm_continuous.rs @@ -0,0 +1,81 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use core::cmp::Ordering; + +use defmt::info; +use embassy_executor::Spawner; +use embassy_nrf::pdm::{self, Config, Frequency, OperationMode, Pdm, Ratio, SamplerState}; +use embassy_nrf::{bind_interrupts, peripherals}; +use fixed::types::I7F1; +use microfft::real::rfft_1024; +use num_integer::Roots; +use {defmt_rtt as _, panic_probe as _}; + +// Demonstrates both continuous sampling and scanning multiple channels driven by a PPI linked timer + +bind_interrupts!(struct Irqs { + PDM => pdm::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_p: Spawner) { + let mut p = embassy_nrf::init(Default::default()); + let mut config = Config::default(); + // Pins are correct for the onboard microphone on the Feather nRF52840 Sense. + config.frequency = Frequency::_1280K; // 16 kHz sample rate + config.ratio = Ratio::RATIO80; + config.operation_mode = OperationMode::Mono; + config.gain_left = I7F1::from_bits(5); // 2.5 dB + let mut pdm = Pdm::new(p.PDM, Irqs, &mut p.P0_00, &mut p.P0_01, config); + + let mut bufs = [[0; 1024]; 2]; + + pdm.run_task_sampler(&mut bufs, move |buf| { + // NOTE: It is important that the time spent within this callback + // does not exceed the time taken to acquire the 1500 samples we + // have in this example, which would be 10us + 2us per + // sample * 1500 = 18ms. You need to measure the time taken here + // 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}, peak {} @ {} Hz", + buf.len(), + buf.iter().min().unwrap(), + buf.iter().max().unwrap(), + mean, + (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 + .unwrap(); +} + +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()) + .enumerate() + .max_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(Ordering::Equal)) + .map(|(i, v)| (i, ((v * 32768.0) as u32).sqrt())) + .unwrap() +}