M-Labs/kirdy
3
0
Fork 1
Code Issues 11 Pull Requests Projects Releases Wiki Activity

LdPwrExcProtector: Use IIR filter for OPP logic

Browse Source 
- 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
This commit is contained in:
linuswck 2025-01-28 15:53:23 +08:00
parent c1855f6a08
commit bd83a8fc92
3 changed files with 41 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/laser_diode/laser_diode.rs
View File

@ -167,10 +167,6 @@ impl LdDrive {
self.settings.ld_drive_current != i 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 { pub fn poll_and_update_output_current(&mut self) -> ElectricCurrent {
match LdCurrentOutCtrlTimer::get_irq_status() { match LdCurrentOutCtrlTimer::get_irq_status() {
Some(i_set) => { Some(i_set) => {

93
src/laser_diode/ld_pwr_exc_protector.rs
View File

@ -46,9 +46,11 @@ pub struct LdPwrExcProtector {
alarm_status: Status, alarm_status: Status,
calibrated_vdda: u32, calibrated_vdda: u32,
offset: 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 { impl LdPwrExcProtector {
@ -92,8 +94,9 @@ impl LdPwrExcProtector {
.res() .res()
.twelve_bit() .twelve_bit()
// Set Analog Watchdog to guard Single Regular Channel // Set Analog Watchdog to guard Single Regular Channel
// Disable Analog Watchdog
.awden() .awden()
.enabled() .disabled()
.awdsgl() .awdsgl()
.single_channel() .single_channel()
.jawden() .jawden()
@ -101,9 +104,9 @@ impl LdPwrExcProtector {
// Disable Analog Watchdog Interrupt // Disable Analog Watchdog Interrupt
.awdie() .awdie()
.disabled() .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| { pac_adc.cr2.write(|w| {
w w
@ -131,11 +134,6 @@ impl LdPwrExcProtector {
pac_adc.smpr1.write(|w| unsafe { w.bits(0xFFFF) }); pac_adc.smpr1.write(|w| unsafe { w.bits(0xFFFF) });
pac_adc.smpr2.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. // SWStart should only be set when ADON = 1. Otherwise no conversion is launched.
pac_adc.cr2.modify(|_, w| w.swstart().set_bit()); pac_adc.cr2.modify(|_, w| w.swstart().set_bit());
@ -148,9 +146,11 @@ impl LdPwrExcProtector {
alarm_status: Status::default(), alarm_status: Status::default(),
calibrated_vdda: 3300, calibrated_vdda: 3300,
offset: offset, 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() } 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() { if let Some(ref mut wdg) = LdPwrExcProtector::get() {
return ElectricPotential::new::<millivolt>( 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, as f32,
); );
} }
@ -176,7 +176,7 @@ impl LdPwrExcProtector {
.ceil() as u32) .ceil() as u32)
+ wdg.offset; + wdg.offset;
if code <= MAX_SAMPLE as u32 { if code <= MAX_SAMPLE as u32 {
wdg.pac.htr.write(|w| unsafe { w.bits(code) }); wdg.trigger_threshold = code as u16;
return true; 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 let Some(ref mut wdg) = LdPwrExcProtector::get() {
if wdg.pac.sr.read().eoc().bit() { 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;
samples = samples >> 5; wdg.iir_x1 = sample as u32;
wdg.prev_iir_sample = (7 * wdg.prev_iir_sample + samples as u16) >> 3; wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(y);
wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(wdg.prev_iir_sample); if y > wdg.trigger_threshold as u32 {
LdPwrExcProtector::pwr_excursion_handler();
}
} }
return wdg.alarm_status.v;
} }
ElectricPotential::zero()
} }
pub fn get_status() -> Status { pub fn get_status() -> Status {
if let Some(ref mut wdg) = LdPwrExcProtector::get() { if let Some(ref mut wdg) = LdPwrExcProtector::get() {
LdPwrExcProtector::poll_pd_mon_v();
return wdg.alarm_status.clone(); return wdg.alarm_status.clone();
} }
Status::default() Status::default()
@ -232,9 +240,6 @@ impl LdPwrExcProtector {
wdg.alarm_status = Status::default(); wdg.alarm_status = Status::default();
LdPwrExcProtector::clear_interrupt_bit(); LdPwrExcProtector::clear_interrupt_bit();
LdPwrExcProtector::pwr_on(); 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() { fn clear_interrupt_bit() {
if let Some(ref mut wdg) = LdPwrExcProtector::get() { if let Some(ref mut wdg) = LdPwrExcProtector::get() {
wdg.pac.sr.modify(|_, w| w.awd().clear_bit()); wdg.pac.sr.modify(|_, w| w.awd().clear_bit());
@ -275,16 +268,14 @@ impl LdPwrExcProtector {
if let Some(ref mut wdg) = LdPwrExcProtector::get() { if let Some(ref mut wdg) = LdPwrExcProtector::get() {
wdg.alarm_status.pwr_engaged = false; wdg.alarm_status.pwr_engaged = false;
wdg.phy.pwr_en_ch0.set_low(); wdg.phy.pwr_en_ch0.set_low();
LdPwrExcProtector::disable_watchdog_interrupt();
} }
} }
fn pwr_excursion_handler() { fn pwr_excursion_handler() {
if let Some(ref mut wdg) = LdPwrExcProtector::get() { if let Some(ref mut wdg) = LdPwrExcProtector::get() {
let sample = wdg.pac.dr.read().data().bits();
LdPwrExcProtector::pwr_off(); LdPwrExcProtector::pwr_off();
wdg.alarm_status.pwr_excursion = true; 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] #[interrupt]
fn ADC() { fn ADC() {
cortex_m::interrupt::free(|_| { 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(); LdPwrExcProtector::clear_interrupt_bit();
}) })
} }

1
src/main.rs
View File

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