kirdy/src/laser_diode/ld_pwr_exc_protector.rs

use stm32f4xx_hal::{gpio::{gpioa::PA3, gpiod::PD9, Analog, Output, PushPull},
                    interrupt, pac,
                    pac::{ADC3, NVIC},
                    rcc::Enable};
use uom::si::{electric_potential::millivolt, f32::ElectricPotential, ratio::ratio};

// 12 bit Resolution
const MAX_SAMPLE: u16 = 4095;
pub type LdPwrEnPinType = PD9<Output<PushPull>>;
pub type PdMonAdcPinType = PA3<Analog>;
const PD_MON_ADC_CH_ID: u8 = 0x03;

static mut LD_PWR_EXC_PROTECTOR: Option<LdPwrExcProtector> = None;

pub struct LdPwrExcProtectorPhy {
    // To make sure Pd Mon Pin is configured to Analog mode
    pub _pd_mon_ch0: PdMonAdcPinType,
    pub pwr_en_ch0: LdPwrEnPinType,
}

#[derive(Clone)]
pub struct Status {
    pub pwr_excursion: bool,
    pub v_tripped: ElectricPotential,
    pub pwr_engaged: bool,
    pub v: ElectricPotential,
}

impl Default for Status {
    fn default() -> Self {
        Status {
            pwr_excursion: false,
            v_tripped: ElectricPotential::new::<millivolt>(0.0),
            pwr_engaged: false,
            v: ElectricPotential::new::<millivolt>(0.0),
        }
    }
}

pub struct LdPwrExcProtector {
    pac: ADC3,
    phy: LdPwrExcProtectorPhy,
    alarm_status: Status,
    calibrated_vdda: u32,
}

impl LdPwrExcProtector {
    /// ADC Analog Watchdog is configured to guard a single regular Adc channel on Pd Mon Pin.
    /// ADC is configured to start continuous conversion without using DMA immediately.
    /// Interrupt is disabled by default.
    pub fn setup(pac_adc: ADC3, mut phy: LdPwrExcProtectorPhy) {
        unsafe {
            // All ADCs share the same reset interface.
            // NOTE(unsafe) this reference will only be used for atomic writes with no side effects.
            let rcc = &(*pac::RCC::ptr());
            // Enable the ADC3 Clock
            pac::ADC3::enable(rcc);
            // Enable ADC Interrupt
            NVIC::unmask(interrupt::ADC);
        }

        pac_adc.cr1.reset();
        pac_adc.cr2.reset();
        pac_adc.sqr1.reset();
        pac_adc.sqr2.reset();
        pac_adc.sqr3.reset();

        pac_adc.cr1.write(|w| {
            w
                // 12 Bit Resolution
                .res()
                .twelve_bit()
                // Set Analog Watchdog to guard Single Regular Channel
                .awden()
                .enabled()
                .awdsgl()
                .single_channel()
                .jawden()
                .disabled()
                // Disable Analog Watchdog Interrupt
                .awdie()
                .disabled()
                // Set Analog Watchdog to monitor Pd Mon Pin
                .awdch()
                .variant(PD_MON_ADC_CH_ID)
        });
        pac_adc.cr2.write(|w| {
            w
                // Continous Conversion Mode
                .cont()
                .set_bit()
                // Power up ADC
                .adon()
                .set_bit()
                // Set data alignment to the right
                .align()
                .right()
                // End of conversion selection: Each Sequence
                .eocs()
                .each_sequence()
                .exten()
                .disabled()
                .extsel()
                .tim1cc1()
        });
        // Set the Conversion Sequence to include Pd Mon Pin
        pac_adc.sqr3.write(|w| w.sq1().variant(PD_MON_ADC_CH_ID));
        // Set all sampling channels to have fastest sampling interval
        pac_adc.smpr1.reset();
        pac_adc.smpr2.reset();

        // Set the higher threshold to be max value initially
        pac_adc.htr.write(|w| w.ht().variant(MAX_SAMPLE));
        // Set the lower threshold to be min value initially
        pac_adc.ltr.write(|w| w.lt().variant(0));

        // SWStart should only be set when ADON = 1. Otherwise no conversion is launched.
        pac_adc.cr2.modify(|_, w| w.swstart().set_bit());

        phy.pwr_en_ch0.set_low();

        unsafe {
            LD_PWR_EXC_PROTECTOR = Some(LdPwrExcProtector {
                pac: pac_adc,
                phy: phy,
                alarm_status: Status::default(),
                calibrated_vdda: 3300,
            });
        }
    }

    fn get() -> Option<&'static mut Self> {
        unsafe { LD_PWR_EXC_PROTECTOR.as_mut() }
    }

    fn convert_sample_to_volt(sample: u16) -> ElectricPotential {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            return ElectricPotential::new::<millivolt>(
                ((u32::from(sample) * wdg.calibrated_vdda) / u32::from(MAX_SAMPLE)) as f32,
            );
        }
        ElectricPotential::new::<millivolt>(0.0)
    }

    pub fn set_trigger_threshold_v(htr: ElectricPotential) {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            let code: u32 = ((htr / (ElectricPotential::new::<millivolt>(wdg.calibrated_vdda as f32))).get::<ratio>()
                * (MAX_SAMPLE as f32)) as u32;
            wdg.pac.htr.write(|w| unsafe { w.bits(code) });
        }
    }

    pub fn set_calibrated_vdda(val: u32) {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            wdg.calibrated_vdda = val;
        }
    }

    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());
            return wdg.alarm_status.clone();
        }
        Status::default()
    }

    pub fn pwr_on_and_arm_protection() {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            wdg.alarm_status = Status::default();
            LdPwrExcProtector::pwr_on();
            // Interrupt should be enabled after power on to tackle the following edge case:
            //      Pd_Mon pin voltage has already exceed threshold before LD Power is on.
            LdPwrExcProtector::enable_watchdog_interrupt();
        }
    }

    pub fn clear_alarm_status() {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            wdg.alarm_status.pwr_excursion = false;
            wdg.alarm_status.v_tripped = ElectricPotential::new::<millivolt>(0.0);
        }
    }

    fn enable_watchdog_interrupt() {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            wdg.pac.cr1.modify(|_, w| w.awdie().set_bit());
        }
    }

    fn disable_watchdog_interrupt() {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            wdg.pac.cr1.modify(|_, w| w.awdie().clear_bit());
        }
    }

    fn clear_interrupt_bit() {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            wdg.pac.sr.modify(|_, w| w.awd().clear_bit());
        }
    }

    fn pwr_on() {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            wdg.alarm_status.pwr_engaged = true;
            wdg.phy.pwr_en_ch0.set_high()
        }
    }

    pub fn pwr_off() {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            wdg.alarm_status.pwr_engaged = false;
            wdg.phy.pwr_en_ch0.set_low()
        }
    }

    fn pwr_excursion_handler() {
        if let Some(ref mut wdg) = LdPwrExcProtector::get() {
            let sample = wdg.pac.dr.read().data().bits();
            LdPwrExcProtector::pwr_off();
            wdg.alarm_status.pwr_excursion = true;
            wdg.alarm_status.v_tripped = LdPwrExcProtector::convert_sample_to_volt(sample);
        }
    }
}

#[interrupt]
fn ADC() {
    cortex_m::interrupt::free(|_| {
        LdPwrExcProtector::pwr_excursion_handler();
        // Disable interrupt to avoid getting stuck in infinite loop
        LdPwrExcProtector::disable_watchdog_interrupt();
        LdPwrExcProtector::clear_interrupt_bit();
    })
}