kirdy/src/thermostat/thermostat.rs

use core::f32::NAN;
use core::marker::PhantomData;
use crate::sys_timer;
use crate::thermostat::ad5680;
use crate::thermostat::max1968::{MAX1968, AdcReadTarget, PwmPinsEnum};
use crate::thermostat::ad7172::{self, FilterType, PostFilter, SingleChODR};
use crate::thermostat::pid_state::{PidState, PidSettings, Parameters as PidParams};
use crate::thermostat::temp_mon::{TempMon, TempStatus, TempMonSettings};
use crate::thermostat::steinhart_hart::Parameters as Sh_Params;
use serde::{Deserialize, Serialize};
use log::debug;
use uom::si::{
    electric_current::ampere,
    electric_potential::volt,
    electrical_resistance::ohm,
    thermodynamic_temperature::degree_celsius,
    f32::{ThermodynamicTemperature, ElectricCurrent, ElectricPotential, ElectricalResistance},
    ratio::ratio,
};
use miniconf::Tree;

use super::pid_state;

pub const R_SENSE: ElectricalResistance = ElectricalResistance {
    dimension: PhantomData,
    units: PhantomData,
    value: 0.05,
};

#[derive(Deserialize, Serialize, Copy, Clone, Debug, Default, Tree)]
pub struct TempAdcFilter{
    pub filter_type: FilterType,
    pub sinc5sinc1odr: Option<SingleChODR>,
    pub sinc3odr: Option<SingleChODR>,
    pub sinc5sinc1postfilter: Option<PostFilter>,
    pub sinc3fineodr: Option<f32>,
    pub rate: Option<f32>,
}


#[derive(Deserialize, Serialize, Clone, Copy, Debug, Tree)]
pub struct TecSettings {
    pub default_pwr_on: bool,
    pub center_pt: ElectricPotential,
    pub max_v_set: ElectricPotential,
    pub max_i_pos_set: ElectricCurrent,
    pub max_i_neg_set: ElectricCurrent,
    pub i_set: ElectricCurrent,
    pub vref: ElectricPotential,
}

impl TecSettings{
    pub const DAC_OUT_V_MAX: ElectricPotential = ElectricPotential {
        dimension: PhantomData,
        units: PhantomData,
        value: 3.0,
    };
    pub const TEC_VSEC_BIAS_V: ElectricPotential = ElectricPotential {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.65,
    };
    
    // Kirdy Design Specs:
    // MaxV = 5.0V
    // MAX Current = +- 1.0A
    const MAX_I_SET : ElectricCurrent = ElectricCurrent {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.0,
    };
    const MAX_V_DUTY_TO_CURRENT_RATE: ElectricPotential = ElectricPotential {
        dimension: PhantomData,
        units: PhantomData,
        value: 4.0 * 3.3,
    };
    pub const MAX_V_MAX: ElectricPotential = ElectricPotential {
        dimension: PhantomData,
        units: PhantomData,
        value: 5.0,
    };
    const MAX_V_DUTY_MAX: f64 = TecSettings::MAX_V_MAX.value as f64 / TecSettings::MAX_V_DUTY_TO_CURRENT_RATE.value as f64;
    const MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE: ElectricCurrent = ElectricCurrent {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.0 / (10.0 * R_SENSE.value / 3.3),
    };
    pub const MAX_I_POS_CURRENT: ElectricCurrent = ElectricCurrent {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.0,
    };
    pub const MAX_I_NEG_CURRENT: ElectricCurrent = ElectricCurrent {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.0,
    };
    // .get::<ratio>() is not implemented for const
    const MAX_I_POS_DUTY_MAX: f64 = TecSettings::MAX_I_POS_CURRENT.value as f64 / TecSettings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE.value as f64;
    const MAX_I_NEG_DUTY_MAX: f64 = TecSettings::MAX_I_NEG_CURRENT.value as f64 / TecSettings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE.value as f64;
}

impl Default for TecSettings {
    fn default() -> Self {
        Self {
            default_pwr_on: false,
            center_pt: ElectricPotential::new::<volt>(1.5),
            max_v_set: ElectricPotential::new::<volt>(5.0),
            max_i_pos_set: ElectricCurrent::new::<ampere>(1.0),
            max_i_neg_set: ElectricCurrent::new::<ampere>(1.0),
            i_set: ElectricCurrent::new::<ampere>(0.0),
            vref: ElectricPotential::new::<volt>(1.5),
        }
    }
}

pub struct Thermostat {
    max1968: MAX1968,
    ad7172: ad7172::AdcPhy,
    pub tec_settings: TecSettings,
    pid_ctrl_ch0: PidState,
    temp_mon: TempMon,
}

#[derive(Deserialize, Serialize, Copy, Clone, Debug, Tree)]
pub struct ThermostatSettingsSummary {
    default_pwr_on: bool,
    pid_engaged: bool,
    temperature_setpoint: f32,
    tec_settings: TecSettingSummary,
    pid_params: PidParams,
    temp_adc_settings: TempAdcFilter,
    temp_mon_settings: TempMonSettings,
    thermistor_params: ThermistorParams,
}

impl Thermostat{
    pub fn new (max1968: MAX1968, ad7172: ad7172::AdcPhy) -> Self {
        Thermostat{
            max1968: max1968,
            ad7172: ad7172,
            tec_settings: TecSettings::default(),
            pid_ctrl_ch0: PidState::default(),
            temp_mon: TempMon::default(),
        }
    }
    pub fn setup(&mut self){
        self.tec_setup();
        let t_adc_ch0_cal = self.t_adc_setup();
        self.pid_ctrl_ch0.set_adc_calibration(t_adc_ch0_cal) ;
    }

    /// start_tec_readings_conversion() should not be called before the current
    /// DMA request is serviced or the conversion process will be restarted
    /// Thus, no new readings is available when you call get_tec_readings() fn
    pub fn start_tec_readings_conversion(&mut self){
        self.max1968.dma_adc_start_conversion();
    }

    fn tec_setup(&mut self) {
        self.power_down();

        self.tec_settings = TecSettings::default();

        self.set_i(self.tec_settings.i_set);

        self.set_max_v(self.tec_settings.max_v_set);
        self.set_max_i_pos(self.tec_settings.max_i_pos_set);
        self.set_max_i_neg(self.tec_settings.max_i_neg_set);
    }

    fn t_adc_setup(&mut self)->ad7172::ChannelCalibration{
        self.ad7172.set_sync_enable(false).unwrap();
        self.ad7172.setup_channel(0, ad7172::Input::Ain0, ad7172::Input::Ain1).unwrap();
        let adc_calibration0 = self.ad7172.get_calibration(0)
            .expect("adc_calibration0");
        self.ad7172.start_continuous_conversion().unwrap();
        adc_calibration0
    }

    pub fn poll_adc(&mut self) -> bool {
        let mut data_rdy = false;
        self.ad7172.data_ready().unwrap().map(|_ch|  {
            let data = self.ad7172.read_data().unwrap();
            let state: &mut PidState = &mut self.pid_ctrl_ch0;
            state.update(data);
            let pid_engaged = state.get_pid_engaged();
            let temp = self.get_temperature();
            self.temp_mon.update_status(pid_engaged, self.max1968.is_powered_on(), temp);
            debug!("state.get_pid_engaged(): {:?}", pid_engaged);
            debug!("Temperature: {:?} degree", temp.get::<degree_celsius>());
            data_rdy = true;
        });
        data_rdy
    }

    pub fn update_pid(&mut self) {
        let state: &mut PidState = &mut self.pid_ctrl_ch0;
        let pid_engaged = state.get_pid_engaged();
        if pid_engaged {
            match state.update_pid() {
                Some(pid_output) => {
                    self.set_i(ElectricCurrent::new::<ampere>(pid_output as f32));
                    debug!("Temperature Set Point: {:?} degree", self.pid_ctrl_ch0.get_pid_setpoint().get::<degree_celsius>());
                }
                None => { }
            }
        }
    }

    pub fn get_temp_mon_status(&mut self) -> TempStatus {
        self.temp_mon.get_status()
    }

    pub fn power_up(&mut self){
        self.max1968.power_up();
    }

    pub fn power_down(&mut self){
        self.max1968.power_down();
        self.pid_ctrl_ch0.reset_pid_state();
        self.set_i(ElectricCurrent::new::<ampere>(0.0));
    }

    fn set_center_pt(&mut self, value: ElectricPotential){
        self.tec_settings.center_pt = value;
    }

    pub fn set_default_pwr_on(&mut self, pwr_on: bool) {
            self.tec_settings.default_pwr_on = pwr_on;
    }

    pub fn set_i(&mut self, i_tec: ElectricCurrent) -> ElectricCurrent {
        let voltage = i_tec * 10.0 * R_SENSE + self.tec_settings.center_pt;
        let voltage = self.max1968.set_dac(voltage, TecSettings::DAC_OUT_V_MAX);
        self.tec_settings.i_set = (voltage - self.tec_settings.center_pt) / (10.0 * R_SENSE);
        self.tec_settings.i_set
    }

    pub fn set_max_v(&mut self, max_v: ElectricPotential) -> ElectricPotential {
        let duty = (max_v / TecSettings::MAX_V_DUTY_TO_CURRENT_RATE).get::<ratio>();
        let duty = self.max1968.set_pwm(PwmPinsEnum::MaxV, duty as f64, TecSettings::MAX_V_DUTY_MAX);
        self.tec_settings.max_v_set = duty as f32 * TecSettings::MAX_V_DUTY_TO_CURRENT_RATE;
        self.tec_settings.max_v_set
    }

    pub fn set_max_i_pos(&mut self, max_i_pos: ElectricCurrent) -> ElectricCurrent {
        let duty = (max_i_pos / TecSettings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE).get::<ratio>();
        let duty = self.max1968.set_pwm(PwmPinsEnum::MaxPosI, duty as f64, TecSettings::MAX_I_POS_DUTY_MAX);
        self.tec_settings.max_i_pos_set = duty as f32 * TecSettings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE;
        self.tec_settings.max_i_pos_set
    }

    pub fn set_max_i_neg(&mut self, max_i_neg: ElectricCurrent) -> ElectricCurrent {
        let duty = (max_i_neg / TecSettings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE).get::<ratio>();
        let duty = self.max1968.set_pwm(PwmPinsEnum::MaxNegI, duty as f64, TecSettings::MAX_I_NEG_DUTY_MAX);
        self.tec_settings.max_i_neg_set = duty as f32 * TecSettings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE;
        self.tec_settings.max_i_neg_set
    }

    #[allow(unused)]
    fn get_dac_vfb(&mut self) -> ElectricPotential {
        self.max1968.adc_read(AdcReadTarget::DacVfb, 16)
    }

    #[allow(unused)]
    fn get_vref(&mut self) -> ElectricPotential {
        self.max1968.adc_read(AdcReadTarget::VREF, 16)
    }

    #[allow(unused)]
    pub fn get_tec_i(&mut self) -> ElectricCurrent {
        let vref = self.max1968.adc_read(AdcReadTarget::VREF, 16);
        (self.max1968.adc_read(AdcReadTarget::ITec, 16) - vref) / ElectricalResistance::new::<ohm>(0.4)
    }

    #[allow(unused)]
    pub fn get_tec_v(&mut self) -> ElectricPotential {
        (self.max1968.adc_read(AdcReadTarget::VTec, 16) - TecSettings::TEC_VSEC_BIAS_V) * 4.0
    }

    pub fn get_tec_readings(&mut self) -> (ElectricPotential, ElectricCurrent) {
        let vref = self.tec_settings.vref;
        let (vtec, itec) = self.max1968.get_tec_readings();
        ((vtec - TecSettings::TEC_VSEC_BIAS_V) * 4.0, (itec - vref) / ElectricalResistance::new::<ohm>(0.4))
    }

    /// Calibrates the DAC output to match vref of the MAX driver to reduce zero-current offset of the MAX driver output.
    ///
    /// The thermostat DAC applies a control voltage signal to the CTLI pin of MAX driver chip to control its output current.
    /// The CTLI input signal is centered around VREF of the MAX chip. Applying VREF to CTLI sets the output current to 0.
    /// 
    /// This calibration routine measures the VREF voltage and the DAC output with the STM32 ADC, and uses a breadth-first     
    /// search to find the DAC setting that will produce a DAC output voltage closest to VREF. This DAC output voltage will 
    /// be stored and used in subsequent i_set routines to bias the current control signal to the measured VREF, reducing 
    /// the offset error of the current control signal.
    ///
    /// The input offset of the STM32 ADC is eliminated by using the same ADC for the measurements, and by only using the
    /// difference in VREF and DAC output for the calibration.
    /// 
    /// This routine should be called only once after boot, repeated reading of the vref signal and changing of the stored 
    /// VREF measurement can introduce significant noise at the current output, degrading the stabilily performance of the
    /// thermostat. 
    pub fn calibrate_dac_value(&mut self) {
        const STEPS: i32 = 18;
        let target_voltage = self.max1968.adc_read(AdcReadTarget::VREF, 64);
        let mut start_value = 1;
        let mut best_error = ElectricPotential::new::<volt>(100.0);
        for step in (0..STEPS).rev() {
            debug!("TEC VREF Calibrating: Step {}/{}", step, STEPS);
            let mut prev_value = start_value;
            for value in (start_value..=ad5680::MAX_VALUE).step_by(1 << step) {
                self.max1968.phy.dac.set(value).unwrap();
                sys_timer::sleep(5);

                let dac_feedback = self.max1968.adc_read(AdcReadTarget::DacVfb, 64);
                let error = target_voltage - dac_feedback;
                if error < ElectricPotential::new::<volt>(0.0) {
                    break;
                } else if error < best_error {
                    best_error = error;
                    start_value = prev_value;

                    let vref = (value as f32 / ad5680::MAX_VALUE as f32) * TecSettings::DAC_OUT_V_MAX;
                    self.set_center_pt(vref);
                }
                prev_value = value;
            }
        }
        self.tec_settings.vref = target_voltage;
    }
    
    pub fn set_pid_engaged(&mut self, val: bool) {
        self.pid_ctrl_ch0.set_pid_engaged(val);
    }

    fn get_pid_engaged(&mut self) -> bool {
       self.pid_ctrl_ch0.get_pid_engaged()
    }

    pub fn get_status_report(&mut self) -> StatusReport {
        let (tec_v, tec_i) = self.get_tec_readings();
        let temperature: Option<f32>;

        match self.pid_ctrl_ch0.get_temperature() {
            Some(val) => {
                temperature = Some(val.get::<degree_celsius>())
            }
            None => {
                temperature = None;
            }
        }
        
        StatusReport {
            pwr_on: self.max1968.is_powered_on(),
            pid_engaged: self.get_pid_engaged(),
            temp_mon_status: self.temp_mon.get_status(),
            temperature: temperature,
            i_set: self.tec_settings.i_set,
            tec_i: tec_i,
            tec_v: tec_v,
        }
    }

    pub fn get_temperature(&mut self) -> ThermodynamicTemperature {
        match self.pid_ctrl_ch0.get_temperature() {
            Some(val) => {
                val
            }
            None => { ThermodynamicTemperature::new::<degree_celsius>(NAN) }
        }
    }

    fn get_pid_settings(&mut self) -> pid_state::Parameters {
        self.pid_ctrl_ch0.get_pid_settings()
    }

    fn get_steinhart_hart(&mut self) -> ThermistorParams {
        let sh = self.pid_ctrl_ch0.get_sh();
        ThermistorParams {
            t0: sh.t0.get::<degree_celsius>(),
            r0: sh.r0,
            b: sh.b,
        }
        
    }

    fn apply_steinhart_hart(&mut self, sh: ThermistorParams) {
        self.pid_ctrl_ch0.apply_sh(
            Sh_Params {
                t0: ThermodynamicTemperature::new::<degree_celsius>(sh.t0),
                r0: sh.r0,
                b: sh.b,
            }
        )
    }


    fn get_tec_settings(&mut self) -> TecSettingSummary {
        TecSettingSummary {
            i_set: TecSettingsSummaryField { value: self.tec_settings.i_set, max: TecSettings::MAX_I_SET },
            max_v: TecSettingsSummaryField { value: self.tec_settings.max_v_set, max: TecSettings::MAX_V_MAX },
            max_i_pos: TecSettingsSummaryField { value: self.tec_settings.max_i_pos_set, max: TecSettings::MAX_I_POS_CURRENT },
            max_i_neg: TecSettingsSummaryField { value: self.tec_settings.max_i_neg_set, max: TecSettings::MAX_I_NEG_CURRENT },
        }
    }

    pub fn get_calibrated_vdda(&mut self) -> u32 {
        self.max1968.get_calibrated_vdda()
    }

    pub fn set_pid(&mut self, param: PidSettings, val: f32){
        self.pid_ctrl_ch0.set_pid_params(param, val);
    }

    pub fn set_sh_beta(&mut self, beta: f32) {
        self.pid_ctrl_ch0.set_sh_beta(beta);
    }

    pub fn set_sh_r0(&mut self, r0: ElectricalResistance) {
        self.pid_ctrl_ch0.set_sh_r0(r0);
    }

    pub fn set_sh_t0(&mut self, t0: ThermodynamicTemperature) {
        self.pid_ctrl_ch0.set_sh_t0(t0);
    }

    pub fn set_temperature_setpoint(&mut self, t: ThermodynamicTemperature) {
        let t = t.min(self.temp_mon.get_upper_limit()).max(self.temp_mon.get_lower_limit());
        self.pid_ctrl_ch0.set_pid_setpoint(t);
        self.temp_mon.set_setpoint(t);
    }

    pub fn apply_temp_mon_settings(&mut self, settings: TempMonSettings){
        self.temp_mon.set_upper_limit(ThermodynamicTemperature::new::<degree_celsius>(settings.upper_limit));
        self.temp_mon.set_lower_limit(ThermodynamicTemperature::new::<degree_celsius>(settings.lower_limit));
    }

    pub fn set_temp_mon_upper_limit(&mut self, t: ThermodynamicTemperature) {
        self.temp_mon.set_upper_limit(t);
    }

    pub fn set_temp_mon_lower_limit(&mut self, t: ThermodynamicTemperature) {
        self.temp_mon.set_lower_limit(t);
    }

    pub fn set_temp_adc_sinc5_sinc1_filter(&mut self, index: u8, odr: ad7172::SingleChODR) {
        self.ad7172.set_sinc5_sinc1_filter(index, odr).unwrap();
    }

    pub fn set_temp_adc_sinc3_filter(&mut self, index: u8, odr: ad7172::SingleChODR) {
        self.ad7172.set_sinc3_filter(index, odr).unwrap();
    }

    pub fn set_temp_adc_sinc5_sinc1_with_postfilter(&mut self, index: u8, odr: ad7172::PostFilter) {
        self.ad7172.set_sinc5_sinc1_with_50hz_60hz_rejection(index, odr).unwrap();
    }

    pub fn set_temp_adc_sinc3_fine_filter(&mut self, index: u8, rate: f32) {
        self.ad7172.set_sinc3_fine_filter(index, rate).unwrap();
    }

    pub fn clear_temp_mon_alarm(&mut self) {
        self.temp_mon.clear_alarm();
    }

    fn get_temp_mon_settings(&mut self) -> TempMonSettings {
        self.temp_mon.get_settings()
    }

    pub fn get_settings_summary(&mut self) -> ThermostatSettingsSummary {
        let temp_adc_filter_type: FilterType;
        let update_rate: f32;
        match self.ad7172.get_filter_type_and_rate(0) {
            Ok((filter_type, rate)) => {
                temp_adc_filter_type = filter_type;
                update_rate = rate;
            }
            Err(_) => {
                panic!("Cannot read ADC filter type and rate");
            }
        }

        ThermostatSettingsSummary {
            default_pwr_on: self.tec_settings.default_pwr_on,
            pid_engaged: self.get_pid_engaged(),
            temperature_setpoint: self.pid_ctrl_ch0.get_pid_setpoint().get::<degree_celsius>(),
            tec_settings: self.get_tec_settings(),
            pid_params: self.get_pid_settings(),
            temp_adc_settings: TempAdcFilter{
                filter_type : temp_adc_filter_type,
                sinc5sinc1odr : None,
                sinc3odr : None,
                sinc5sinc1postfilter : None,
                sinc3fineodr : None,
                rate : Some(update_rate),
            },
            temp_mon_settings: self.get_temp_mon_settings(),
            thermistor_params: self.get_steinhart_hart(),
        }
    }

    pub fn load_settings_from_summary(&mut self, settings: ThermostatSettingsSummary) {
        self.power_down();
        self.set_max_i_neg(settings.tec_settings.max_i_neg.value);
        self.set_max_i_pos(settings.tec_settings.max_i_pos.value);
        self.set_max_v(settings.tec_settings.max_v.value);
        
        self.apply_steinhart_hart(settings.thermistor_params);
        self.apply_temp_mon_settings(settings.temp_mon_settings);
        
        match settings.temp_adc_settings.rate {
            Some(rate) => {
                match settings.temp_adc_settings.filter_type {
                    FilterType::Sinc3 => {
                        self.set_temp_adc_sinc3_filter(0, SingleChODR::closest(rate).unwrap())
                    }
                    FilterType::Sinc5Sinc1 => {
                        self.set_temp_adc_sinc5_sinc1_filter(0, SingleChODR::closest(rate).unwrap())
                    }
                    FilterType::Sinc3WithFineODR => {
                        self.set_temp_adc_sinc3_fine_filter(0, rate)
                    }
                    FilterType::Sinc5Sinc1With50hz60HzRejection => {
                        self.set_temp_adc_sinc5_sinc1_with_postfilter(0, PostFilter::closest(rate).unwrap())
                    }
                }
            }
            None => {
                debug!(" Temperature ADC Settings is not found");
            }
        }

        self.set_pid_engaged(settings.pid_engaged);
        self.pid_ctrl_ch0.apply_pid_params(settings.pid_params);
        self.set_temperature_setpoint(ThermodynamicTemperature::new::<degree_celsius>(settings.temperature_setpoint));
        if !settings.pid_engaged {
            self.set_i(settings.tec_settings.i_set.value);
        }
        self.set_default_pwr_on(settings.default_pwr_on);
        if settings.default_pwr_on {
            self.power_up();
        } else {
            self.power_down();
        }
    }
}

#[derive(Deserialize, Serialize, Copy, Clone, Debug, Tree)]
pub struct StatusReport {
    pwr_on: bool,
    pid_engaged: bool,
    temp_mon_status: TempStatus,
    temperature: Option<f32>,
    i_set: ElectricCurrent,
    tec_i: ElectricCurrent,
    tec_v: ElectricPotential,
}

#[derive(Deserialize, Serialize, Copy, Clone, Debug, Tree)]
pub struct TecSettingsSummaryField<T> {
    value: T,
    max: T,
}

#[derive(Deserialize, Serialize, Copy, Clone, Debug, Tree)]
pub struct TecSettingSummary {
    i_set: TecSettingsSummaryField<ElectricCurrent>,
    max_v: TecSettingsSummaryField<ElectricPotential>,
    max_i_pos: TecSettingsSummaryField<ElectricCurrent>,
    max_i_neg: TecSettingsSummaryField<ElectricCurrent>,
}

#[derive(Deserialize, Serialize, Clone, Copy, Debug, Tree)]
pub struct ThermistorParams {
    t0: f32,
    r0: ElectricalResistance,
    b: f32
}