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:
parent
8139ebdc1b
commit
88cca12a60
@ -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));
|
||||
|
@ -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,8 +76,9 @@ 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,
|
||||
@ -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;
|
||||
|
||||
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 ld_set_i(&mut self, i: ElectricCurrent){
|
||||
LdCurrentOutCtrlTimer::set_target_i_and_listen_irq(i, self.settings.ld_drive_current);
|
||||
}
|
||||
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
|
||||
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)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Set the calibrated VDDA value obtained from ADC1 calibration
|
||||
|
120
src/laser_diode/ld_current_out_ctrl_timer.rs
Normal file
120
src/laser_diode/ld_current_out_ctrl_timer.rs
Normal 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();
|
||||
}
|
@ -3,3 +3,4 @@ pub mod max5719;
|
||||
pub mod laser_diode;
|
||||
pub mod pd_responsitivity;
|
||||
pub mod ld_pwr_exc_protector;
|
||||
pub mod ld_current_out_ctrl_timer;
|
||||
|
@ -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()));
|
||||
|
Loading…
Reference in New Issue
Block a user