LdPwrExcProtector: Use IIR filter for OPP logic

- Do not use the built-in analog watchdog
- IIR Filter limits ADC noise to be below 1 LSB
- Fix false positive error for the overpower protection due to ADC noise

src/laser_diode/laser_diode.rs

@@ -167,10 +167,6 @@ impl LdDrive {
         self.settings.ld_drive_current != i
     }
 
-    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) => {

src/laser_diode/ld_pwr_exc_protector.rs

@@ -46,9 +46,11 @@ pub struct LdPwrExcProtector {
     alarm_status: Status,
     calibrated_vdda: u32,
     offset: u32,
-    prev_samples: [u16; 32],
+    trigger_threshold: u16,
-    prev_iir_sample: u16,
+    iir_y1: u32,
-    ptr: u16,
+    iir_y2: u32,
+    iir_x1: u32,
+    iir_x2: u32,
 }
 
 impl LdPwrExcProtector {
@@ -92,8 +94,9 @@ impl LdPwrExcProtector {
                 .res()
                 .twelve_bit()
                 // Set Analog Watchdog to guard Single Regular Channel
+                // Disable Analog Watchdog
                 .awden()
-                .enabled()
+                .disabled()
                 .awdsgl()
                 .single_channel()
                 .jawden()
@@ -101,9 +104,9 @@ impl LdPwrExcProtector {
                 // Disable Analog Watchdog Interrupt
                 .awdie()
                 .disabled()
-                // Set Analog Watchdog to monitor Pd Mon Pin
+                // Enable End of Conversion Interrupt
-                .awdch()
+                .eocie()
-                .variant(PD_MON_ADC_CH_ID)
+                .enabled()
         });
         pac_adc.cr2.write(|w| {
             w
@@ -131,11 +134,6 @@ impl LdPwrExcProtector {
         pac_adc.smpr1.write(|w| unsafe { w.bits(0xFFFF) });
         pac_adc.smpr2.write(|w| unsafe { w.bits(0xFFFF) });
 
-        // Set the higher threshold to be max value initially
-        pac_adc.htr.write(|w| w.ht().variant(MAX_SAMPLE));
-        // Set the lower threshold to be min value initially
-        pac_adc.ltr.write(|w| w.lt().variant(0));
-
         // SWStart should only be set when ADON = 1. Otherwise no conversion is launched.
         pac_adc.cr2.modify(|_, w| w.swstart().set_bit());
 
@@ -148,9 +146,11 @@ impl LdPwrExcProtector {
                 alarm_status: Status::default(),
                 calibrated_vdda: 3300,
                 offset: offset,
-                prev_samples: [0; 32],
+                iir_y1: 0,
-                prev_iir_sample: 0,
+                iir_y2: 0,
-                ptr: 0,
+                iir_x1: 0,
+                iir_x2: 0,
+                trigger_threshold: MAX_SAMPLE,
             });
         }
     }
@@ -159,10 +159,10 @@ impl LdPwrExcProtector {
         unsafe { LD_PWR_EXC_PROTECTOR.as_mut() }
     }
 
-    fn convert_sample_to_volt(sample: u16) -> ElectricPotential {
+    fn convert_sample_to_volt(sample: u32) -> ElectricPotential {
         if let Some(ref mut wdg) = LdPwrExcProtector::get() {
             return ElectricPotential::new::<millivolt>(
-                (((i32::from(sample) - wdg.offset as i32).max(0) as u32 * wdg.calibrated_vdda) / u32::from(MAX_SAMPLE))
+                (((sample as i32 - wdg.offset as i32).max(0) as u32 * wdg.calibrated_vdda) / u32::from(MAX_SAMPLE))
                     as f32,
             );
         }
@@ -176,7 +176,7 @@ impl LdPwrExcProtector {
                 .ceil() as u32)
                 + wdg.offset;
             if code <= MAX_SAMPLE as u32 {
-                wdg.pac.htr.write(|w| unsafe { w.bits(code) });
+                wdg.trigger_threshold = code as u16;
                 return true;
             }
         }
@@ -189,29 +189,37 @@ impl LdPwrExcProtector {
         }
     }
 
-    pub fn poll_pd_mon_v() -> ElectricPotential {
+    pub fn irq_handler() {
         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();
+                // Sampling Frequency: PCLK2(84MHz) / 8 / ( 12 + 480 ) = 21.3kHz
-                wdg.ptr = (wdg.ptr + 1) % 32 as u16;
+                // Second Order Section IIR Filter designed with scipy
+                // Filter Type: Butterworth Low Pass Filter
+                // Coefficient Width: 20bit
+                // Cutoff Frequency(-3dB): 500Hz
+                let sample = wdg.pac.dr.read().data().bits();
+                const A1: u32 = 1879535; // Negative Coefficient
+                const A2: u32 = 851509;
+                const B0: u32 = 5137;
+                const B1: u32 = 10275;
+                const B2: u32 = 5137;
 
-                let mut samples: u32 = 0;
+                let y: u32 = (B0 * sample as u32 + B1 * wdg.iir_x1 + B2 * wdg.iir_x2 + A1 * wdg.iir_y1 - A2 * wdg.iir_y2) >> 20;
-                for idx in 0..32 {
+                wdg.iir_y2 = wdg.iir_y1;
-                    samples += wdg.prev_samples[idx] as u32;
+                wdg.iir_y1 = y;
+                wdg.iir_x2 = wdg.iir_x1;
+                wdg.iir_x1 = sample as u32;
+
+                wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(y);
+                if y > wdg.trigger_threshold as u32 {
+                    LdPwrExcProtector::pwr_excursion_handler();
                 }
-                samples = samples >> 5;
-
-                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;
         }
-        ElectricPotential::zero()
     }
 
     pub fn get_status() -> Status {
         if let Some(ref mut wdg) = LdPwrExcProtector::get() {
-            LdPwrExcProtector::poll_pd_mon_v();
             return wdg.alarm_status.clone();
         }
         Status::default()
@@ -232,9 +240,6 @@ impl LdPwrExcProtector {
                 wdg.alarm_status = Status::default();
                 LdPwrExcProtector::clear_interrupt_bit();
                 LdPwrExcProtector::pwr_on();
-                // Interrupt should be enabled after power on to tackle the following edge case:
-                //      Pd_Mon pin voltage has already exceed threshold before LD Power is on.
-                LdPwrExcProtector::enable_watchdog_interrupt();
             }
         }
     }
@@ -246,18 +251,6 @@ impl LdPwrExcProtector {
         }
     }
 
-    fn enable_watchdog_interrupt() {
-        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
-            wdg.pac.cr1.modify(|_, w| w.awdie().set_bit());
-        }
-    }
-
-    fn disable_watchdog_interrupt() {
-        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
-            wdg.pac.cr1.modify(|_, w| w.awdie().clear_bit());
-        }
-    }
-
     fn clear_interrupt_bit() {
         if let Some(ref mut wdg) = LdPwrExcProtector::get() {
             wdg.pac.sr.modify(|_, w| w.awd().clear_bit());
@@ -275,16 +268,14 @@ impl LdPwrExcProtector {
         if let Some(ref mut wdg) = LdPwrExcProtector::get() {
             wdg.alarm_status.pwr_engaged = false;
             wdg.phy.pwr_en_ch0.set_low();
-            LdPwrExcProtector::disable_watchdog_interrupt();
         }
     }
 
     fn pwr_excursion_handler() {
         if let Some(ref mut wdg) = LdPwrExcProtector::get() {
-            let sample = wdg.pac.dr.read().data().bits();
             LdPwrExcProtector::pwr_off();
             wdg.alarm_status.pwr_excursion = true;
-            wdg.alarm_status.v_tripped = LdPwrExcProtector::convert_sample_to_volt(sample);
+            wdg.alarm_status.v_tripped = LdPwrExcProtector::convert_sample_to_volt(wdg.iir_y1);
         }
     }
 }
@@ -292,9 +283,7 @@ impl LdPwrExcProtector {
 #[interrupt]
 fn ADC() {
     cortex_m::interrupt::free(|_| {
-        LdPwrExcProtector::pwr_excursion_handler();
+        LdPwrExcProtector::irq_handler();
-        // Disable interrupt to avoid getting stuck in infinite loop
-        LdPwrExcProtector::disable_watchdog_interrupt();
         LdPwrExcProtector::clear_interrupt_bit();
     })
 }

src/main.rs

@@ -152,7 +152,6 @@ fn main() -> ! {
             }
             State::MainLoop => {
                 laser.poll_and_update_output_current();
-                laser.poll_pd_mon_v();
 
                 net::net::eth_update_iface_poll_timer();