From 88cca12a60be6f5ae0465abba75042c12e8ea14e Mon Sep 17 00:00:00 2001 From: linuswck Date: Tue, 20 Feb 2024 12:39:49 +0800 Subject: [PATCH] ld: Use Timer IRQ to ramp up/down output current - Fixes possible watchdog timeout - Improves ethernet response time & thermostat Pid update interval consistency --- src/device/boot.rs | 3 +- src/laser_diode/laser_diode.rs | 59 ++++----- src/laser_diode/ld_current_out_ctrl_timer.rs | 120 +++++++++++++++++++ src/laser_diode/mod.rs | 3 +- src/main.rs | 2 + 5 files changed, 150 insertions(+), 37 deletions(-) create mode 100644 src/laser_diode/ld_current_out_ctrl_timer.rs diff --git a/src/device/boot.rs b/src/device/boot.rs index 450184c..e64ee4a 100644 --- a/src/device/boot.rs +++ b/src/device/boot.rs @@ -8,6 +8,7 @@ use crate::net::net::ServerHandle; use stm32_eth; use fugit::ExtU32; use log::{info, debug}; +use stm32f4xx_hal::timer::TimerExt; use stm32f4xx_hal::{ pac::{CorePeripherals, Peripherals}, rcc::RccExt, @@ -71,7 +72,7 @@ pub fn bootup( debug!("Setting up Laser Driver"); let current_source = LdCtrl::new(current_source_phy); - let mut laser = LdDrive::new(current_source, perif.ADC2, pd_mon_phy); + let mut laser = LdDrive::new(current_source, perif.ADC2, perif.TIM2.counter(&clocks), pd_mon_phy); laser.setup(); laser.ld_open(); laser.set_ld_drive_current_limit(ElectricCurrent::new::(0.2)); diff --git a/src/laser_diode/laser_diode.rs b/src/laser_diode/laser_diode.rs index 1758ce4..7f60ade 100644 --- a/src/laser_diode/laser_diode.rs +++ b/src/laser_diode/laser_diode.rs @@ -1,9 +1,11 @@ use miniconf::Tree; -use stm32f4xx_hal::pac::ADC2; +use stm32f4xx_hal::timer::CounterUs; +use stm32f4xx_hal::pac::{ADC2, TIM2}; use uom::si::electric_current::ampere; use crate::laser_diode::ld_ctrl::{LdCtrl, Impedance}; use crate::laser_diode::ld_pwr_exc_protector::{LdPwrExcProtector, self}; use crate::laser_diode::pd_responsitivity; +use crate::laser_diode::ld_current_out_ctrl_timer::LdCurrentOutCtrlTimer; use core::marker::PhantomData; use crate::device::sys_timer::sleep; use serde::{Deserialize, Serialize}; @@ -12,9 +14,7 @@ use uom::si::{ electric_current::milliampere, f64::{ElectricPotential, ElectricCurrent, Power}, }; -use num_traits::Float; - -use uom::{si::{ISQ, SI, Quantity, ratio::ratio}, typenum::*}; +use uom::{si::{ISQ, SI, Quantity}, typenum::*}; // Volt / Ampere pub type TransimpedanceUnit = Quantity, SI, f64>; @@ -40,14 +40,6 @@ impl Settings{ units: PhantomData, value: 10.0 / 0.75, }; - - const LD_CURRENT_STEP_SIZE: ElectricCurrent = ElectricCurrent { - dimension: PhantomData, - units: PhantomData, - value: 0.0001, - }; - - const LD_CURRENT_TIME_STEP_MS: u32 = 1; } #[derive(Clone, Debug, Tree)] @@ -84,9 +76,10 @@ pub struct LdDrive{ } impl LdDrive{ - pub fn new(current_source: LdCtrl, pins_adc: ADC2, phy: ld_pwr_exc_protector::LdPwrExcProtectorPhy)-> Self { + pub fn new(current_source: LdCtrl, pins_adc: ADC2, tim2: CounterUs, phy: ld_pwr_exc_protector::LdPwrExcProtectorPhy)-> Self { LdPwrExcProtector::setup(pins_adc, phy); - + LdCurrentOutCtrlTimer::setup(tim2); + LdDrive { ctrl: current_source, settings: Settings::default() @@ -95,7 +88,8 @@ impl LdDrive{ pub fn setup(&mut self) { LdPwrExcProtector::pwr_off(); - self.ld_set_i(ElectricCurrent::new::(0.0)); + self.ctrl.set_i(ElectricCurrent::new::(0.0), Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX); + LdCurrentOutCtrlTimer::reset(); self.ld_short(); } @@ -117,6 +111,7 @@ impl LdDrive{ pub fn power_up(&mut self){ let _ = self.ctrl.set_i(ElectricCurrent::new::(0.0), Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX); + LdCurrentOutCtrlTimer::reset(); LdPwrExcProtector::pwr_on_and_arm_protection(); // Wait for LD Power Supply to start up before driving current to laser diode sleep(30); @@ -135,28 +130,22 @@ impl LdDrive{ self.settings.pd_responsitivity.get_ld_pwr_from_ld_i(LdPwrExcProtector::get_status().v * Settings::PD_MON_TRANSCONDUCTANCE) } - // Ramping up or down laser diode current according to preset current step size and time step. - pub fn ld_set_i(&mut self, i: ElectricCurrent) -> ElectricCurrent { - let mut prev_i_set = self.settings.ld_drive_current; - let final_i_set = i.min(self.settings.ld_drive_current_limit).max(ElectricCurrent::new::(0.0)); - - let num_of_step = ((final_i_set - prev_i_set)/Settings::LD_CURRENT_STEP_SIZE).get::().floor() as i32; + pub fn ld_set_i(&mut self, i: ElectricCurrent){ + LdCurrentOutCtrlTimer::set_target_i_and_listen_irq(i, self.settings.ld_drive_current); + } - let current_step = if num_of_step.is_positive() { - Settings::LD_CURRENT_STEP_SIZE - } else { - -Settings::LD_CURRENT_STEP_SIZE - }; - - for _ in 0..num_of_step.abs() { - prev_i_set = prev_i_set + current_step; - let _ = self.ctrl.set_i(prev_i_set, Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX); - sleep(Settings::LD_CURRENT_TIME_STEP_MS); + pub fn poll_and_update_output_current(&mut self) -> ElectricCurrent { + match LdCurrentOutCtrlTimer::get_irq_status() { + Some(i_set) => { + let i_set = self.ctrl.set_i(i_set, Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX); + self.settings.ld_drive_current = i_set; + LdCurrentOutCtrlTimer::clear_alarm_and_resume_listening(); + i_set + } + None => { + ElectricCurrent::new::(0.0) + } } - let prev_i_set = self.ctrl.set_i(final_i_set, Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX); - - self.settings.ld_drive_current = prev_i_set; - prev_i_set } // Set the calibrated VDDA value obtained from ADC1 calibration diff --git a/src/laser_diode/ld_current_out_ctrl_timer.rs b/src/laser_diode/ld_current_out_ctrl_timer.rs new file mode 100644 index 0000000..b28c4d9 --- /dev/null +++ b/src/laser_diode/ld_current_out_ctrl_timer.rs @@ -0,0 +1,120 @@ +use stm32f4xx_hal::timer::{CounterUs, Event}; +use stm32f4xx_hal::pac::{interrupt, Interrupt, TIM2}; +use uom::si::{f64::ElectricCurrent, electric_current::ampere}; +use fugit::{TimerDurationU32, KilohertzU32}; +use core::marker::PhantomData; + +pub struct LdCurrentOutCtrlTimer { + target_i: ElectricCurrent, + now_i: ElectricCurrent, + timer: CounterUs, + timeout: bool, +} +static mut LD_CURRENT_OUT_CTRL_TIMER: Option = None; + +/// This timer notifies the main loop to set the correct output current so that ld output current can ramp up/down slowly. +/// The current output slope is guaranteed to be larger but not necessarily equal to than the preset value. +impl LdCurrentOutCtrlTimer { + const TIME_STEP_MS: TimerDurationU32<1000000> = TimerDurationU32::from_rate(KilohertzU32::from_raw(1)); + const STEP_SIZE: ElectricCurrent = ElectricCurrent { + dimension: PhantomData, + units: PhantomData, + value: 0.0001, + }; + + pub fn setup(mut tim2: CounterUs) { + tim2.start(LdCurrentOutCtrlTimer::TIME_STEP_MS).unwrap(); + unsafe { + cortex_m::peripheral::NVIC::unmask(Interrupt::TIM2); + } + unsafe { + LD_CURRENT_OUT_CTRL_TIMER = Some( + LdCurrentOutCtrlTimer { + target_i: ElectricCurrent::new::(0.0), + now_i: ElectricCurrent::new::(0.0), + timer: tim2, + timeout: false + } + ); + } + } + + fn get() -> Option<&'static mut Self> { + unsafe { LD_CURRENT_OUT_CTRL_TIMER.as_mut() } + } + + pub fn reset() { + if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() { + ld_current_out_ctrl_timer.target_i = ElectricCurrent::new::(0.0); + ld_current_out_ctrl_timer.now_i = ElectricCurrent::new::(0.0); + ld_current_out_ctrl_timer.timeout = false; + ld_current_out_ctrl_timer.timer.unlisten(Event::Update); + } + } + + pub fn set_target_i_and_listen_irq(target: ElectricCurrent, now: ElectricCurrent) { + if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() { + cortex_m::interrupt::free(|_| { + ld_current_out_ctrl_timer.target_i = target; + ld_current_out_ctrl_timer.now_i = now; + ld_current_out_ctrl_timer.timer.listen(Event::Update); + } + ) + } + } + + pub fn set_alarm() { + if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() { + ld_current_out_ctrl_timer.timeout = true; + } + } + pub fn clear_alarm_and_resume_listening() { + if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() { + ld_current_out_ctrl_timer.timeout = false; + ld_current_out_ctrl_timer.timer.listen(Event::Update); + } + } + pub fn get_irq_status() -> Option { + if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() { + if ld_current_out_ctrl_timer.timeout { + return Some(ld_current_out_ctrl_timer.now_i); + } + } + None + } + + pub fn clear_irq_flag() { + if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() { + ld_current_out_ctrl_timer.timer.clear_interrupt(Event::Update); + } + } + + pub fn update_next_i_set() -> bool { + if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() { + let update = ld_current_out_ctrl_timer.now_i != ld_current_out_ctrl_timer.target_i; + if ld_current_out_ctrl_timer.target_i > ld_current_out_ctrl_timer.now_i { + ld_current_out_ctrl_timer.now_i = (ld_current_out_ctrl_timer.now_i + LdCurrentOutCtrlTimer::STEP_SIZE).min(ld_current_out_ctrl_timer.target_i); + } + else { + ld_current_out_ctrl_timer.now_i = (ld_current_out_ctrl_timer.now_i - LdCurrentOutCtrlTimer::STEP_SIZE).max(ld_current_out_ctrl_timer.target_i); + } + return update + } + false + } + + pub fn stop_listening() { + if let Some(ref mut ld_current_out_ctrl_timer ) = LdCurrentOutCtrlTimer::get() { + ld_current_out_ctrl_timer.timer.unlisten(Event::Update); + } + } +} + +#[interrupt] +fn TIM2(){ + if LdCurrentOutCtrlTimer::update_next_i_set() { + LdCurrentOutCtrlTimer::set_alarm(); + } + LdCurrentOutCtrlTimer::stop_listening(); + LdCurrentOutCtrlTimer::clear_irq_flag(); +} diff --git a/src/laser_diode/mod.rs b/src/laser_diode/mod.rs index c765ba1..0cca298 100644 --- a/src/laser_diode/mod.rs +++ b/src/laser_diode/mod.rs @@ -2,4 +2,5 @@ pub mod ld_ctrl; pub mod max5719; pub mod laser_diode; pub mod pd_responsitivity; -pub mod ld_pwr_exc_protector; \ No newline at end of file +pub mod ld_pwr_exc_protector; +pub mod ld_current_out_ctrl_timer; diff --git a/src/main.rs b/src/main.rs index 30f9440..9e5b155 100644 --- a/src/main.rs +++ b/src/main.rs @@ -94,6 +94,8 @@ fn main() -> ! { if !should_reset { let mut eth_is_pending = false; + laser.poll_and_update_output_current(); + if thermostat.poll_adc_and_update_pid() { info!("curr_dac_vfb: {:?}", volt_fmt.with(thermostat.get_dac_vfb())); info!("curr_vref: {:?}", volt_fmt.with(thermostat.get_vref()));