From bd83a8fc92154d1f5561ddaed41cc2e532383368 Mon Sep 17 00:00:00 2001 From: linuswck Date: Tue, 28 Jan 2025 15:53:23 +0800 Subject: [PATCH] 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 | 4 -- src/laser_diode/ld_pwr_exc_protector.rs | 93 +++++++++++-------------- src/main.rs | 1 - 3 files changed, 41 insertions(+), 57 deletions(-) diff --git a/src/laser_diode/laser_diode.rs b/src/laser_diode/laser_diode.rs index c56467e..568a190 100644 --- a/src/laser_diode/laser_diode.rs +++ b/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) => { diff --git a/src/laser_diode/ld_pwr_exc_protector.rs b/src/laser_diode/ld_pwr_exc_protector.rs index 41c5e85..8f5de39 100644 --- a/src/laser_diode/ld_pwr_exc_protector.rs +++ b/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], - prev_iir_sample: u16, - ptr: u16, + trigger_threshold: u16, + iir_y1: u32, + 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 - .awdch() - .variant(PD_MON_ADC_CH_ID) + // Enable End of Conversion Interrupt + .eocie() + .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], - prev_iir_sample: 0, - ptr: 0, + iir_y1: 0, + iir_y2: 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::( - (((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(); - wdg.ptr = (wdg.ptr + 1) % 32 as u16; + // Sampling Frequency: PCLK2(84MHz) / 8 / ( 12 + 480 ) = 21.3kHz + // 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; - for idx in 0..32 { - samples += wdg.prev_samples[idx] as u32; + let y: u32 = (B0 * sample as u32 + B1 * wdg.iir_x1 + B2 * wdg.iir_x2 + A1 * wdg.iir_y1 - A2 * wdg.iir_y2) >> 20; + wdg.iir_y2 = wdg.iir_y1; + 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(); - // Disable interrupt to avoid getting stuck in infinite loop - LdPwrExcProtector::disable_watchdog_interrupt(); + LdPwrExcProtector::irq_handler(); LdPwrExcProtector::clear_interrupt_bit(); }) } diff --git a/src/main.rs b/src/main.rs index fcd4749..cea8b02 100644 --- a/src/main.rs +++ b/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();