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
}
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) => {

93
src/laser_diode/ld_pwr_exc_protector.rs
View File

@ -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::<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();
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();
})
}

1
src/main.rs
View File

@ -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();