diff --git a/src/laser_diode/ld_pwr_exc_protector.rs b/src/laser_diode/ld_pwr_exc_protector.rs index 4a9259d..41c5e85 100644 --- a/src/laser_diode/ld_pwr_exc_protector.rs +++ b/src/laser_diode/ld_pwr_exc_protector.rs @@ -47,6 +47,7 @@ pub struct LdPwrExcProtector { calibrated_vdda: u32, offset: u32, prev_samples: [u16; 32], + prev_iir_sample: u16, ptr: u16, } @@ -148,6 +149,7 @@ impl LdPwrExcProtector { calibrated_vdda: 3300, offset: offset, prev_samples: [0; 32], + prev_iir_sample: 0, ptr: 0, }); } @@ -198,7 +200,9 @@ impl LdPwrExcProtector { samples += wdg.prev_samples[idx] as u32; } samples = samples >> 5; - wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(samples as u16); + + 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; }