From 530f192acceb5a10c416e1823dc27a749e68b7dc Mon Sep 17 00:00:00 2001
From: Quentin Smith <quentin@mit.edu>
Date: Sat, 20 Aug 2022 17:08:29 -0400
Subject: [PATCH] Set gain at runtime

---
 embassy-nrf/src/pdm.rs      | 21 +++++++++++++++++++++
 examples/nrf/Cargo.toml     |  1 +
 examples/nrf/src/bin/pdm.rs | 29 +++++++++++++++++++----------
 3 files changed, 41 insertions(+), 10 deletions(-)

diff --git a/embassy-nrf/src/pdm.rs b/embassy-nrf/src/pdm.rs
index 629eab99f..b3cc87603 100644
--- a/embassy-nrf/src/pdm.rs
+++ b/embassy-nrf/src/pdm.rs
@@ -88,6 +88,8 @@ impl<'d> Pdm<'d> {
             w
         });
 
+        Self::_set_gain(r, gain_left, gain_right);
+
         r.psel.din.write(|w| unsafe { w.bits(data.psel_bits()) });
         r.psel.clk.write(|w| unsafe { w.bits(clock.psel_bits()) });
 
@@ -115,6 +117,18 @@ impl<'d> Pdm<'d> {
         }
     }
 
+    fn _set_gain(r: &pdm::RegisterBlock, gain_left: I7F1, gain_right: I7F1) {
+        let gain_left = gain_left.saturating_add(I7F1::from_bits(40)).saturating_to_num::<u8>().clamp(0, 0x50);
+        let gain_right = gain_right.saturating_add(I7F1::from_bits(40)).saturating_to_num::<u8>().clamp(0, 0x50);
+
+        r.gainl.write(|w| unsafe { w.gainl().bits(gain_left) });
+        r.gainr.write(|w| unsafe { w.gainr().bits(gain_right) });
+    }
+
+    pub fn set_gain(&mut self, gain_left: I7F1, gain_right: I7F1) {
+        Self::_set_gain(Self::regs(), gain_left, gain_right)
+    }
+
     fn regs() -> &'static pdm::RegisterBlock {
         unsafe { &*PDM::ptr() }
     }
@@ -154,6 +168,13 @@ impl<'d> Pdm<'d> {
     }
 }
 
+impl<'d> Drop for Pdm<'d> {
+    fn drop(&mut self) {
+        let r = Self::regs();
+        r.enable.write(|w| w.enable().disabled());
+    }
+}
+
 #[derive(Clone, Copy, PartialEq)]
 pub enum Edge {
     FallingEdge,
diff --git a/examples/nrf/Cargo.toml b/examples/nrf/Cargo.toml
index 673bcfc6b..d0567ba8e 100644
--- a/examples/nrf/Cargo.toml
+++ b/examples/nrf/Cargo.toml
@@ -31,3 +31,4 @@ fixed = "1.10.0"
 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 }
diff --git a/examples/nrf/src/bin/pdm.rs b/examples/nrf/src/bin/pdm.rs
index d5e90e27a..a73d01fb9 100644
--- a/examples/nrf/src/bin/pdm.rs
+++ b/examples/nrf/src/bin/pdm.rs
@@ -8,6 +8,7 @@ use embassy_nrf::interrupt;
 use embassy_nrf::pdm::{Config, Channels, Pdm};
 use embassy_time::{Duration, Timer};
 use fixed::types::I7F1;
+use num_integer::Roots;
 use {defmt_rtt as _, panic_probe as _};
 
 #[embassy_executor::main]
@@ -20,15 +21,23 @@ async fn main(_p: Spawner) {
     let mut pdm = Pdm::new(p.PDM, interrupt::take!(PDM), &mut p.P0_00, &mut p.P0_01, config);
 
     loop {
-        let mut buf = [0; 128];
-        pdm.sample(&mut buf).await;
-        info!(
-            "{} samples, min {=i16}, max {=i16}, mean {=i16}",
-            buf.len(),
-            buf.iter().min().unwrap(),
-            buf.iter().max().unwrap(),
-            (buf.iter().map(|v| i32::from(*v)).sum::<i32>() / buf.len() as i32) as i16,
-        );
-        Timer::after(Duration::from_millis(100)).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));
+            for _ in 0..10 {
+                let mut buf = [0; 128];
+                pdm.sample(&mut buf).await;
+                info!(
+                    "{} samples, min {=i16}, max {=i16}, RMS {=i16}",
+                    buf.len(),
+                    buf.iter().min().unwrap(),
+                    buf.iter().max().unwrap(),
+                    (
+                        buf.iter().map(|v| i32::from(*v).pow(2)).fold(0i32, |a,b| a.saturating_add(b))
+                    / buf.len() as i32).sqrt() as i16,
+                );
+                Timer::after(Duration::from_millis(100)).await;
+            }
+        }
     }
 }