From c09da0db98f3218e4f3a15079c061fc65780c67d Mon Sep 17 00:00:00 2001
From: linuswck <linuswck@m-labs.hk>
Date: Thu, 13 Jun 2024 15:36:17 +0800
Subject: [PATCH] ld_pwr_exc_protector: Apply 32-pt avg to pd_v - Reduce noise
 amplitude from STM32f4 ADC

---
 src/laser_diode/laser_diode.rs          |  4 ++++
 src/laser_diode/ld_pwr_exc_protector.rs | 25 ++++++++++++++++++++++++-
 src/main.rs                             |  1 +
 3 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/src/laser_diode/laser_diode.rs b/src/laser_diode/laser_diode.rs
index b91fd46..ace4c9a 100644
--- a/src/laser_diode/laser_diode.rs
+++ b/src/laser_diode/laser_diode.rs
@@ -157,6 +157,10 @@ impl LdDrive {
         LdCurrentOutCtrlTimer::set_target_i_and_listen_irq(self.settings.ld_drive_current, self.ctrl.get_i_set());
     }
 
+    pub fn poll_pd_mon_v(&mut self) -> ElectricPotential {
+        LdPwrExcProtector::poll_pd_mon_v()
+    }
+
     pub fn poll_and_update_output_current(&mut self) -> ElectricCurrent {
         match LdCurrentOutCtrlTimer::get_irq_status() {
             Some(i_set) => {
diff --git a/src/laser_diode/ld_pwr_exc_protector.rs b/src/laser_diode/ld_pwr_exc_protector.rs
index 362e53c..7dfbddf 100644
--- a/src/laser_diode/ld_pwr_exc_protector.rs
+++ b/src/laser_diode/ld_pwr_exc_protector.rs
@@ -1,3 +1,4 @@
+use num_traits::Zero;
 use stm32f4xx_hal::{adc::{config::{self, AdcConfig},
                           Adc},
                     gpio::{gpioa::PA3, gpiod::PD9, Analog, Output, PushPull},
@@ -45,6 +46,8 @@ pub struct LdPwrExcProtector {
     alarm_status: Status,
     calibrated_vdda: u32,
     offset: u32,
+    prev_samples: [u16; 32],
+    ptr: u16,
 }
 
 impl LdPwrExcProtector {
@@ -143,6 +146,8 @@ impl LdPwrExcProtector {
                 alarm_status: Status::default(),
                 calibrated_vdda: 3300,
                 offset: offset,
+                prev_samples: [0; 32],
+                ptr: 0,
             });
         }
     }
@@ -177,9 +182,27 @@ impl LdPwrExcProtector {
         }
     }
 
+    pub fn poll_pd_mon_v() -> ElectricPotential {
+        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
+            if wdg.pac.sr.read().eoc().bit() {
+                wdg.prev_samples[wdg.ptr as usize] = wdg.pac.dr.read().data().bits();
+                wdg.ptr = (wdg.ptr + 1) % 32 as u16;
+
+                let mut samples: u32 = 0;
+                for idx in 0..32 {
+                    samples += wdg.prev_samples[idx] as u32;
+                }
+                samples = samples >> 5;
+                wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(samples as u16);
+            }
+            return wdg.alarm_status.v;
+        }
+        ElectricPotential::zero()
+    }
+
     pub fn get_status() -> Status {
         if let Some(ref mut wdg) = LdPwrExcProtector::get() {
-            wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(wdg.pac.dr.read().data().bits());
+            LdPwrExcProtector::poll_pd_mon_v();
             return wdg.alarm_status.clone();
         }
         Status::default()
diff --git a/src/main.rs b/src/main.rs
index dee2807..ec71424 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -153,6 +153,7 @@ fn main() -> ! {
             }
             State::MainLoop => {
                 laser.poll_and_update_output_current();
+                laser.poll_pd_mon_v();
 
                 net::net::eth_update_iface_poll_timer();