From 3d26573c6b36c670a170279db49e7adc5e37466b Mon Sep 17 00:00:00 2001
From: Quentin Smith <quentin@mit.edu>
Date: Sun, 21 Aug 2022 01:44:04 -0400
Subject: [PATCH] Discard the first N samples due to transients

---
 embassy-nrf/src/pdm.rs      | 22 +++++++++++++++++++---
 examples/nrf/src/bin/pdm.rs |  8 +++++---
 2 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/embassy-nrf/src/pdm.rs b/embassy-nrf/src/pdm.rs
index db4c74afd..c8375b1e3 100644
--- a/embassy-nrf/src/pdm.rs
+++ b/embassy-nrf/src/pdm.rs
@@ -139,7 +139,8 @@ impl<'d> Pdm<'d> {
     }
 
     /// One shot sampling. If the PDM is configured for multiple channels, the samples will be interleaved.
-    pub async fn sample<const N: usize>(&mut self, buf: &mut [i16; N]) {
+    /// The first samples from the PDM peripheral and microphone usually contain garbage data, so the discard parameter sets the number of complete buffers to discard before returning.
+    pub async fn sample<const N: usize>(&mut self, mut discard: usize, buf: &mut [i16; N]) {
         let r = Self::regs();
 
         // Set up the DMA
@@ -148,9 +149,11 @@ impl<'d> Pdm<'d> {
 
         // 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
         });
@@ -168,13 +171,26 @@ impl<'d> Pdm<'d> {
             WAKER.register(cx.waker());
 
             if r.events_end.read().bits() != 0 {
+                compiler_fence(Ordering::SeqCst);
                 // END means the whole buffer has been received.
                 r.events_end.reset();
-                // Note that the beginning of the buffer might be overwritten before the task fully stops :(
-                r.tasks_stop.write(|w| w.tasks_stop().set_bit());
+                r.intenset.write(|w| w.end().set());
+
+                if discard > 0 {
+                    discard -= 1;
+                } else {
+                    // Note that the beginning of the buffer might be overwritten before the task fully stops :(
+                    r.tasks_stop.write(|w| w.tasks_stop().set_bit());
+                }
+            }
+
+            if r.events_started.read().bits() != 0 {
+                compiler_fence(Ordering::SeqCst);
+                r.events_started.reset();
             }
 
             if r.events_stopped.read().bits() != 0 {
+                compiler_fence(Ordering::SeqCst);
                 r.events_stopped.reset();
                 return Poll::Ready(());
             }
diff --git a/examples/nrf/src/bin/pdm.rs b/examples/nrf/src/bin/pdm.rs
index 85a59a529..605eca59e 100644
--- a/examples/nrf/src/bin/pdm.rs
+++ b/examples/nrf/src/bin/pdm.rs
@@ -26,14 +26,16 @@ async fn main(_p: Spawner) {
             info!("Gain = {} dB", defmt::Debug2Format(&gain));
             for _ in 0..10 {
                 let mut buf = [0; 1500];
-                pdm.sample(&mut buf).await;
+                pdm.sample(5, &mut buf).await;
+                let mean = (buf.iter().map(|v| i32::from(*v)).sum::<i32>() / buf.len() as i32) as i16;
                 info!(
-                    "{} samples, min {=i16}, max {=i16}, RMS {=i16}",
+                    "{} 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).pow(2)).fold(0i32, |a,b| a.saturating_add(b))
+                        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,
                 );
                 info!("samples = {}", &buf);