2024-01-17 12:49:57 +08:00
|
|
|
use uom::si::{
|
2024-02-06 15:20:23 +08:00
|
|
|
electric_current::ampere, electric_potential::volt, electrical_resistance::ohm, f64::{
|
|
|
|
ElectricCurrent, ElectricPotential, ElectricalResistance, ThermodynamicTemperature
|
|
|
|
}, thermodynamic_temperature::degree_celsius
|
2024-01-17 12:49:57 +08:00
|
|
|
};
|
|
|
|
use crate::thermostat::{
|
|
|
|
ad7172,
|
|
|
|
steinhart_hart as sh,
|
|
|
|
};
|
2024-02-06 15:20:23 +08:00
|
|
|
use idsp::iir::{Pid, Action, Biquad};
|
|
|
|
use crate::debug;
|
2024-01-17 12:49:57 +08:00
|
|
|
|
|
|
|
const R_INNER: f64 = 2.0 * 5100.0;
|
|
|
|
const VREF_SENS: f64 = 3.3 / 2.0;
|
|
|
|
|
2024-01-17 17:01:25 +08:00
|
|
|
pub struct PidState {
|
2024-02-06 15:20:23 +08:00
|
|
|
adc_data: Option<u32>,
|
|
|
|
adc_calibration: ad7172::ChannelCalibration,
|
|
|
|
pid_engaged: bool,
|
|
|
|
pid: Biquad<f32>,
|
|
|
|
pid_out_min: ElectricCurrent,
|
|
|
|
pid_out_max: ElectricCurrent,
|
|
|
|
xy: [f32; 4],
|
|
|
|
set_point: ThermodynamicTemperature,
|
|
|
|
settings: Pid<f32>,
|
|
|
|
sh: sh::Parameters,
|
2024-01-17 16:00:02 +08:00
|
|
|
}
|
|
|
|
|
2024-01-17 17:01:25 +08:00
|
|
|
impl Default for PidState {
|
2024-01-17 16:00:02 +08:00
|
|
|
fn default() -> Self {
|
2024-02-06 15:20:23 +08:00
|
|
|
const OUT_MIN: f32 = -1.0;
|
|
|
|
const OUT_MAX: f32 = 1.0;
|
|
|
|
|
|
|
|
let mut pid_settings = Pid::<f32>::default();
|
|
|
|
pid_settings
|
|
|
|
// Pid Update Period depends on the AD7172 Output Data Rate Set
|
|
|
|
.period(0.1)
|
|
|
|
.limit(Action::Kp, 10.0)
|
|
|
|
.limit(Action::Ki, 10.0)
|
|
|
|
.limit(Action::Kd, 10.0);
|
|
|
|
|
|
|
|
let mut pid: Biquad<f32> = pid_settings.build().unwrap().into();
|
|
|
|
pid.set_min(OUT_MIN);
|
|
|
|
pid.set_max(OUT_MAX);
|
|
|
|
|
2024-01-17 17:01:25 +08:00
|
|
|
PidState {
|
2024-01-17 12:49:57 +08:00
|
|
|
adc_data: None,
|
2024-01-17 16:00:02 +08:00
|
|
|
adc_calibration: ad7172::ChannelCalibration::default(),
|
2024-01-17 12:49:57 +08:00
|
|
|
pid_engaged: false,
|
2024-02-06 15:20:23 +08:00
|
|
|
pid: pid,
|
|
|
|
pid_out_min: ElectricCurrent::new::<ampere>(OUT_MIN as f64),
|
|
|
|
pid_out_max: ElectricCurrent::new::<ampere>(OUT_MAX as f64),
|
|
|
|
xy: [0.0; 4],
|
|
|
|
set_point: ThermodynamicTemperature::new::<degree_celsius>(0.0),
|
|
|
|
settings: pid_settings,
|
2024-01-17 12:49:57 +08:00
|
|
|
sh: sh::Parameters::default(),
|
|
|
|
}
|
|
|
|
}
|
2024-01-17 16:00:02 +08:00
|
|
|
}
|
|
|
|
|
2024-02-06 15:20:23 +08:00
|
|
|
pub enum PidSettings {
|
|
|
|
Kp,
|
|
|
|
Ki,
|
|
|
|
Kd,
|
|
|
|
Min,
|
|
|
|
Max,
|
|
|
|
}
|
2024-01-17 12:49:57 +08:00
|
|
|
|
2024-02-06 15:20:23 +08:00
|
|
|
impl PidState {
|
|
|
|
pub fn update(&mut self, adc_data: u32) {
|
2024-01-17 12:49:57 +08:00
|
|
|
self.adc_data = if adc_data == ad7172::MAX_VALUE {
|
|
|
|
// this means there is no thermistor plugged into the ADC.
|
|
|
|
None
|
|
|
|
} else {
|
|
|
|
Some(adc_data)
|
|
|
|
};
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Update PID state on ADC input, calculate new DAC output
|
|
|
|
pub fn update_pid(&mut self) -> Option<f64> {
|
2024-02-06 15:20:23 +08:00
|
|
|
let input = (self.get_temperature()?.get::<degree_celsius>() as f32) - (self.set_point.get::<degree_celsius>() as f32);
|
|
|
|
let pid_output = self.pid.update(&mut self.xy, input);
|
|
|
|
debug!("BiQuad Storage [x0, x1, y0, y1]: {:?}", self.xy);
|
|
|
|
Some(pid_output as f64)
|
2024-01-17 12:49:57 +08:00
|
|
|
}
|
|
|
|
|
2024-02-06 15:20:23 +08:00
|
|
|
pub fn reset_pid_state(&mut self){
|
|
|
|
self.xy = [0.0; 4];
|
2024-01-17 12:49:57 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
pub fn get_adc(&self) -> Option<ElectricPotential> {
|
|
|
|
Some(self.adc_calibration.convert_data(self.adc_data?))
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Get `SENS[01]` input resistance
|
|
|
|
pub fn get_sens(&self) -> Option<ElectricalResistance> {
|
|
|
|
let r_inner = ElectricalResistance::new::<ohm>(R_INNER);
|
|
|
|
let vref = ElectricPotential::new::<volt>(VREF_SENS);
|
|
|
|
let adc_input = self.get_adc()?;
|
|
|
|
let r = r_inner * adc_input / (vref - adc_input);
|
|
|
|
Some(r)
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn get_temperature(&self) -> Option<ThermodynamicTemperature> {
|
|
|
|
let r = self.get_sens()?;
|
|
|
|
let temperature = self.sh.get_temperature(r);
|
|
|
|
Some(temperature)
|
|
|
|
}
|
2024-02-06 15:20:23 +08:00
|
|
|
|
|
|
|
pub fn set_pid_params(&mut self, param: PidSettings, val: f64){
|
|
|
|
match param {
|
|
|
|
PidSettings::Kp => {
|
|
|
|
self.settings.gain(Action::Kp, val as f32);
|
|
|
|
}
|
|
|
|
PidSettings::Ki => {
|
|
|
|
self.settings.gain(Action::Ki, val as f32);
|
|
|
|
}
|
|
|
|
PidSettings::Kd => {
|
|
|
|
self.settings.gain(Action::Kd, val as f32);
|
|
|
|
}
|
|
|
|
PidSettings::Min => {
|
|
|
|
self.pid_out_min = ElectricCurrent::new::<ampere>(val);
|
|
|
|
}
|
|
|
|
PidSettings::Max => {
|
|
|
|
self.pid_out_max = ElectricCurrent::new::<ampere>(val);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
self.update_pid_settings();
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn set_pid_period(&mut self, period: f32){
|
|
|
|
self.settings.period(period);
|
|
|
|
self.pid = self.settings.build().unwrap().into();
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn update_pid_settings(&mut self){
|
|
|
|
self.pid = self.settings.build().unwrap().into();
|
|
|
|
self.pid.set_max(self.pid_out_max.get::<ampere>() as f32);
|
|
|
|
self.pid.set_min(self.pid_out_min.get::<ampere>() as f32);
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn set_pid_setpoint(&mut self, temperature: ThermodynamicTemperature){
|
|
|
|
self.set_point = temperature;
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn get_pid_setpoint(&mut self) -> ThermodynamicTemperature {
|
|
|
|
self.set_point
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn set_sh_t0(&mut self, t0: ThermodynamicTemperature){
|
|
|
|
self.sh.t0 = t0
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn set_sh_r0(&mut self, r0: ElectricalResistance){
|
|
|
|
self.sh.r0 = r0
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn set_sh_beta(&mut self, beta: f64){
|
|
|
|
self.sh.b = beta
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn set_adc_calibration(&mut self, adc_cal: ad7172::ChannelCalibration){
|
|
|
|
self.adc_calibration = adc_cal;
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn set_pid_engaged(&mut self, pid_engaged: bool){
|
|
|
|
self.pid_engaged = pid_engaged;
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn get_pid_engaged(&mut self) -> bool {
|
|
|
|
self.pid_engaged
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn get_pid_settings(&mut self) -> Pid<f32> {
|
|
|
|
unimplemented!()
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn get_sh(&mut self) -> sh::Parameters {
|
|
|
|
unimplemented!()
|
|
|
|
}
|
2024-01-17 12:49:57 +08:00
|
|
|
}
|