From d1660c60900d280842103a523c2c364c144221ef Mon Sep 17 00:00:00 2001
From: linuswck <linuswck@m-labs.hk>
Date: Wed, 12 Jun 2024 18:16:44 +0800
Subject: [PATCH] ld_pwr_exc_protector: cal volt offset for pd mon - hw change:
 A voltage offset is added onto the pd mon pin

---
 src/laser_diode/ld_pwr_exc_protector.rs | 31 +++++++++++++++++++++----
 1 file changed, 26 insertions(+), 5 deletions(-)

diff --git a/src/laser_diode/ld_pwr_exc_protector.rs b/src/laser_diode/ld_pwr_exc_protector.rs
index 9586a39..362e53c 100644
--- a/src/laser_diode/ld_pwr_exc_protector.rs
+++ b/src/laser_diode/ld_pwr_exc_protector.rs
@@ -1,6 +1,8 @@
-use stm32f4xx_hal::{gpio::{gpioa::PA3, gpiod::PD9, Analog, Output, PushPull},
+use stm32f4xx_hal::{adc::{config::{self, AdcConfig},
+                          Adc},
+                    gpio::{gpioa::PA3, gpiod::PD9, Analog, Output, PushPull},
                     interrupt, pac,
-                    pac::{ADC3, NVIC},
+                    pac::{Peripherals, ADC3, NVIC},
                     rcc::Enable};
 use uom::si::{electric_potential::millivolt, f32::ElectricPotential, ratio::ratio};
 
@@ -42,6 +44,7 @@ pub struct LdPwrExcProtector {
     phy: LdPwrExcProtectorPhy,
     alarm_status: Status,
     calibrated_vdda: u32,
+    offset: u32,
 }
 
 impl LdPwrExcProtector {
@@ -49,6 +52,20 @@ impl LdPwrExcProtector {
     /// ADC is configured to start continuous conversion without using DMA immediately.
     /// Interrupt is disabled by default.
     pub fn setup(pac_adc: ADC3, mut phy: LdPwrExcProtectorPhy) {
+        let mut offset = 0;
+        unsafe {
+            let adc_config = AdcConfig::default()
+                .clock(config::Clock::Pclk2_div_8)
+                .default_sample_time(config::SampleTime::Cycles_480);
+            let mut tmp_adc = Adc::adc3(Peripherals::steal().ADC3, false, adc_config);
+
+            for _ in (0..1024).rev() {
+                offset += tmp_adc.convert(&phy._pd_mon_ch0, stm32f4xx_hal::adc::config::SampleTime::Cycles_480) as u32;
+            }
+            offset /= 1024 as u32
+        }
+
+        // Do not set reset RCCs as it causes other ADCs' clock to be disabled
         unsafe {
             // All ADCs share the same reset interface.
             // NOTE(unsafe) this reference will only be used for atomic writes with no side effects.
@@ -125,6 +142,7 @@ impl LdPwrExcProtector {
                 phy: phy,
                 alarm_status: Status::default(),
                 calibrated_vdda: 3300,
+                offset: offset,
             });
         }
     }
@@ -136,7 +154,8 @@ impl LdPwrExcProtector {
     fn convert_sample_to_volt(sample: u16) -> ElectricPotential {
         if let Some(ref mut wdg) = LdPwrExcProtector::get() {
             return ElectricPotential::new::<millivolt>(
-                ((u32::from(sample) * wdg.calibrated_vdda) / u32::from(MAX_SAMPLE)) as f32,
+                (((i32::from(sample) - wdg.offset as i32).max(0) as u32 * wdg.calibrated_vdda) / u32::from(MAX_SAMPLE))
+                    as f32,
             );
         }
         ElectricPotential::new::<millivolt>(0.0)
@@ -144,8 +163,10 @@ impl LdPwrExcProtector {
 
     pub fn set_trigger_threshold_v(htr: ElectricPotential) {
         if let Some(ref mut wdg) = LdPwrExcProtector::get() {
-            let code: u32 = ((htr / (ElectricPotential::new::<millivolt>(wdg.calibrated_vdda as f32))).get::<ratio>()
-                * (MAX_SAMPLE as f32)) as u32;
+            let code: u32 = ((((htr / (ElectricPotential::new::<millivolt>(wdg.calibrated_vdda as f32))).get::<ratio>()
+                * (MAX_SAMPLE as f32)) as u32)
+                + wdg.offset)
+                .min(MAX_SAMPLE as u32);
             wdg.pac.htr.write(|w| unsafe { w.bits(code) });
         }
     }