From c1855f6a08fe7a0401a1282c7974751387acf3ac Mon Sep 17 00:00:00 2001
From: linuswck <linuswck@m-labs.hk>
Date: Mon, 27 Jan 2025 12:40:01 +0800
Subject: [PATCH] pd_mon: Add iir filter to voltage readings - analog wdg
 behaviour is unchanged

---
 src/laser_diode/ld_pwr_exc_protector.rs | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/laser_diode/ld_pwr_exc_protector.rs b/src/laser_diode/ld_pwr_exc_protector.rs
index 4a9259d..41c5e85 100644
--- a/src/laser_diode/ld_pwr_exc_protector.rs
+++ b/src/laser_diode/ld_pwr_exc_protector.rs
@@ -47,6 +47,7 @@ pub struct LdPwrExcProtector {
     calibrated_vdda: u32,
     offset: u32,
     prev_samples: [u16; 32],
+    prev_iir_sample: u16,
     ptr: u16,
 }
 
@@ -148,6 +149,7 @@ impl LdPwrExcProtector {
                 calibrated_vdda: 3300,
                 offset: offset,
                 prev_samples: [0; 32],
+                prev_iir_sample: 0,
                 ptr: 0,
             });
         }
@@ -198,7 +200,9 @@ impl LdPwrExcProtector {
                     samples += wdg.prev_samples[idx] as u32;
                 }
                 samples = samples >> 5;
-                wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(samples as u16);
+
+                wdg.prev_iir_sample = (7 * wdg.prev_iir_sample + samples as u16) >> 3;
+                wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(wdg.prev_iir_sample);
             }
             return wdg.alarm_status.v;
         }