1
0
forked from M-Labs/kirdy

ld: Use Timer IRQ to ramp up/down output current

- Fixes possible watchdog timeout
- Improves ethernet response time & thermostat Pid update interval consistency
This commit is contained in:
linuswck 2024-02-20 12:39:49 +08:00
parent 8139ebdc1b
commit 88cca12a60
5 changed files with 150 additions and 37 deletions

View File

@ -8,6 +8,7 @@ use crate::net::net::ServerHandle;
use stm32_eth; use stm32_eth;
use fugit::ExtU32; use fugit::ExtU32;
use log::{info, debug}; use log::{info, debug};
use stm32f4xx_hal::timer::TimerExt;
use stm32f4xx_hal::{ use stm32f4xx_hal::{
pac::{CorePeripherals, Peripherals}, pac::{CorePeripherals, Peripherals},
rcc::RccExt, rcc::RccExt,
@ -71,7 +72,7 @@ pub fn bootup(
debug!("Setting up Laser Driver"); debug!("Setting up Laser Driver");
let current_source = LdCtrl::new(current_source_phy); 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.setup();
laser.ld_open(); laser.ld_open();
laser.set_ld_drive_current_limit(ElectricCurrent::new::<ampere>(0.2)); laser.set_ld_drive_current_limit(ElectricCurrent::new::<ampere>(0.2));

View File

@ -1,9 +1,11 @@
use miniconf::Tree; 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 uom::si::electric_current::ampere;
use crate::laser_diode::ld_ctrl::{LdCtrl, Impedance}; use crate::laser_diode::ld_ctrl::{LdCtrl, Impedance};
use crate::laser_diode::ld_pwr_exc_protector::{LdPwrExcProtector, self}; use crate::laser_diode::ld_pwr_exc_protector::{LdPwrExcProtector, self};
use crate::laser_diode::pd_responsitivity; use crate::laser_diode::pd_responsitivity;
use crate::laser_diode::ld_current_out_ctrl_timer::LdCurrentOutCtrlTimer;
use core::marker::PhantomData; use core::marker::PhantomData;
use crate::device::sys_timer::sleep; use crate::device::sys_timer::sleep;
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
@ -12,9 +14,7 @@ use uom::si::{
electric_current::milliampere, electric_current::milliampere,
f64::{ElectricPotential, ElectricCurrent, Power}, f64::{ElectricPotential, ElectricCurrent, Power},
}; };
use num_traits::Float; use uom::{si::{ISQ, SI, Quantity}, typenum::*};
use uom::{si::{ISQ, SI, Quantity, ratio::ratio}, typenum::*};
// Volt / Ampere // Volt / Ampere
pub type TransimpedanceUnit = Quantity<ISQ<P2, P1, N3, N2, Z0, Z0, Z0>, SI<f64>, f64>; pub type TransimpedanceUnit = Quantity<ISQ<P2, P1, N3, N2, Z0, Z0, Z0>, SI<f64>, f64>;
@ -40,14 +40,6 @@ impl Settings{
units: PhantomData, units: PhantomData,
value: 10.0 / 0.75, 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)] #[derive(Clone, Debug, Tree)]
@ -84,9 +76,10 @@ pub struct LdDrive{
} }
impl 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<TIM2>, phy: ld_pwr_exc_protector::LdPwrExcProtectorPhy)-> Self {
LdPwrExcProtector::setup(pins_adc, phy); LdPwrExcProtector::setup(pins_adc, phy);
LdCurrentOutCtrlTimer::setup(tim2);
LdDrive { LdDrive {
ctrl: current_source, ctrl: current_source,
settings: Settings::default() settings: Settings::default()
@ -95,7 +88,8 @@ impl LdDrive{
pub fn setup(&mut self) { pub fn setup(&mut self) {
LdPwrExcProtector::pwr_off(); LdPwrExcProtector::pwr_off();
self.ld_set_i(ElectricCurrent::new::<milliampere>(0.0)); self.ctrl.set_i(ElectricCurrent::new::<milliampere>(0.0), Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX);
LdCurrentOutCtrlTimer::reset();
self.ld_short(); self.ld_short();
} }
@ -117,6 +111,7 @@ impl LdDrive{
pub fn power_up(&mut self){ pub fn power_up(&mut self){
let _ = self.ctrl.set_i(ElectricCurrent::new::<milliampere>(0.0), Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX); let _ = self.ctrl.set_i(ElectricCurrent::new::<milliampere>(0.0), Settings::LD_DRIVE_TRANSIMPEDANCE, Settings::DAC_OUT_V_MAX);
LdCurrentOutCtrlTimer::reset();
LdPwrExcProtector::pwr_on_and_arm_protection(); LdPwrExcProtector::pwr_on_and_arm_protection();
// Wait for LD Power Supply to start up before driving current to laser diode // Wait for LD Power Supply to start up before driving current to laser diode
sleep(30); 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) 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){
pub fn ld_set_i(&mut self, i: ElectricCurrent) -> ElectricCurrent { LdCurrentOutCtrlTimer::set_target_i_and_listen_irq(i, self.settings.ld_drive_current);
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::<ampere>(0.0));
let num_of_step = ((final_i_set - prev_i_set)/Settings::LD_CURRENT_STEP_SIZE).get::<ratio>().floor() as i32;
let current_step = if num_of_step.is_positive() { pub fn poll_and_update_output_current(&mut self) -> ElectricCurrent {
Settings::LD_CURRENT_STEP_SIZE match LdCurrentOutCtrlTimer::get_irq_status() {
} else { Some(i_set) => {
-Settings::LD_CURRENT_STEP_SIZE 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();
for _ in 0..num_of_step.abs() { i_set
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); None => {
sleep(Settings::LD_CURRENT_TIME_STEP_MS); ElectricCurrent::new::<ampere>(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 // Set the calibrated VDDA value obtained from ADC1 calibration

View File

@ -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<TIM2>,
timeout: bool,
}
static mut LD_CURRENT_OUT_CTRL_TIMER: Option<LdCurrentOutCtrlTimer> = 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>) {
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::<ampere>(0.0),
now_i: ElectricCurrent::new::<ampere>(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::<ampere>(0.0);
ld_current_out_ctrl_timer.now_i = ElectricCurrent::new::<ampere>(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<ElectricCurrent> {
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();
}

View File

@ -2,4 +2,5 @@ pub mod ld_ctrl;
pub mod max5719; pub mod max5719;
pub mod laser_diode; pub mod laser_diode;
pub mod pd_responsitivity; pub mod pd_responsitivity;
pub mod ld_pwr_exc_protector; pub mod ld_pwr_exc_protector;
pub mod ld_current_out_ctrl_timer;

View File

@ -94,6 +94,8 @@ fn main() -> ! {
if !should_reset { if !should_reset {
let mut eth_is_pending = false; let mut eth_is_pending = false;
laser.poll_and_update_output_current();
if thermostat.poll_adc_and_update_pid() { if thermostat.poll_adc_and_update_pid() {
info!("curr_dac_vfb: {:?}", volt_fmt.with(thermostat.get_dac_vfb())); info!("curr_dac_vfb: {:?}", volt_fmt.with(thermostat.get_dac_vfb()));
info!("curr_vref: {:?}", volt_fmt.with(thermostat.get_vref())); info!("curr_vref: {:?}", volt_fmt.with(thermostat.get_vref()));