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 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::<ampere>(0.2));

View File

@ -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<ISQ<P2, P1, N3, N2, Z0, Z0, Z0>, SI<f64>, 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<TIM2>, 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::<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();
}
@ -117,6 +111,7 @@ impl LdDrive{
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);
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::<ampere>(0.0));
let num_of_step = ((final_i_set - prev_i_set)/Settings::LD_CURRENT_STEP_SIZE).get::<ratio>().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::<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

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 laser_diode;
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 {
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()));