diff --git a/src/laser_diode/laser_diode.rs b/src/laser_diode/laser_diode.rs index b91fd46..ace4c9a 100644 --- a/src/laser_diode/laser_diode.rs +++ b/src/laser_diode/laser_diode.rs @@ -157,6 +157,10 @@ impl LdDrive { LdCurrentOutCtrlTimer::set_target_i_and_listen_irq(self.settings.ld_drive_current, self.ctrl.get_i_set()); } + 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 362e53c..7dfbddf 100644 --- a/src/laser_diode/ld_pwr_exc_protector.rs +++ b/src/laser_diode/ld_pwr_exc_protector.rs @@ -1,3 +1,4 @@ +use num_traits::Zero; use stm32f4xx_hal::{adc::{config::{self, AdcConfig}, Adc}, gpio::{gpioa::PA3, gpiod::PD9, Analog, Output, PushPull}, @@ -45,6 +46,8 @@ pub struct LdPwrExcProtector { alarm_status: Status, calibrated_vdda: u32, offset: u32, + prev_samples: [u16; 32], + ptr: u16, } impl LdPwrExcProtector { @@ -143,6 +146,8 @@ impl LdPwrExcProtector { alarm_status: Status::default(), calibrated_vdda: 3300, offset: offset, + prev_samples: [0; 32], + ptr: 0, }); } } @@ -177,9 +182,27 @@ impl LdPwrExcProtector { } } + pub fn poll_pd_mon_v() -> ElectricPotential { + 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; + + let mut samples: u32 = 0; + for idx in 0..32 { + samples += wdg.prev_samples[idx] as u32; + } + samples = samples >> 5; + wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(samples as u16); + } + return wdg.alarm_status.v; + } + ElectricPotential::zero() + } + pub fn get_status() -> Status { if let Some(ref mut wdg) = LdPwrExcProtector::get() { - wdg.alarm_status.v = LdPwrExcProtector::convert_sample_to_volt(wdg.pac.dr.read().data().bits()); + LdPwrExcProtector::poll_pd_mon_v(); return wdg.alarm_status.clone(); } Status::default() diff --git a/src/main.rs b/src/main.rs index dee2807..ec71424 100644 --- a/src/main.rs +++ b/src/main.rs @@ -153,6 +153,7 @@ fn main() -> ! { } State::MainLoop => { laser.poll_and_update_output_current(); + laser.poll_pd_mon_v(); net::net::eth_update_iface_poll_timer();