kirdy/src/thermostat/pid_state.rs

220 lines
6.3 KiB
Rust
Raw Normal View History

use miniconf::Tree;
use serde::{Deserialize, Serialize};
use uom::si::{
electric_potential::volt, electrical_resistance::ohm, f32::{
ElectricPotential, ElectricalResistance, ThermodynamicTemperature
}, thermodynamic_temperature::degree_celsius
};
use crate::thermostat::{
ad7172,
steinhart_hart as sh,
};
const R_INNER: f32 = 2.0 * 5100.0;
const VREF_SENS: f32 = 3.3 / 2.0;
#[derive(Deserialize, Serialize, Clone, Copy, Debug, PartialEq, Tree)]
pub struct Parameters {
/// Gain coefficient for proportional term
pub kp: f32,
/// Gain coefficient for integral term
pub ki: f32,
/// Gain coefficient for derivative term
pub kd: f32,
/// Output limit minimum
pub output_min: f32,
/// Output limit maximum
pub output_max: f32,
}
impl Default for Parameters {
fn default() -> Self {
Parameters {
kp: 0.0,
ki: 0.0,
kd: 0.0,
output_min: -1.0,
output_max: 1.0,
}
}
}
#[derive(Clone)]
pub struct Controller {
pub parameters: Parameters,
u1 : f64,
x1 : f64,
x2 : f64,
pub y1 : f64,
}
2024-01-17 17:01:25 +08:00
pub struct PidState {
adc_data: Option<u32>,
adc_calibration: ad7172::ChannelCalibration,
pid_engaged: bool,
set_point: ThermodynamicTemperature,
sh: sh::Parameters,
controller: Controller,
}
2024-01-17 17:01:25 +08:00
impl Default for PidState {
fn default() -> Self {
2024-01-17 17:01:25 +08:00
PidState {
adc_data: None,
adc_calibration: ad7172::ChannelCalibration::default(),
pid_engaged: false,
set_point: ThermodynamicTemperature::new::<degree_celsius>(0.0),
sh: sh::Parameters::default(),
controller: Controller {
parameters: Parameters::default(),
u1 : 0.0,
x1 : 0.0,
x2 : 0.0,
y1 : 0.0,
},
}
}
}
pub enum PidSettings {
Kp,
Ki,
Kd,
Min,
Max,
}
impl PidState {
pub fn update(&mut self, adc_data: u32) {
self.adc_data = if adc_data == ad7172::MAX_VALUE {
// this means there is no thermistor plugged into the ADC.
None
} else {
Some(adc_data)
};
}
// Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation
// Input x(t), target u(t), output y(t)
// y0' = y1 - ki * u0
// + x0 * (kp + ki + kd)
// - x1 * (kp + 2kd)
// + x2 * kd
// + kp * (u0 - u1)
// y0 = clip(y0', ymin, ymax)
pub fn update_pid(&mut self) -> Option<f64> {
let input = self.get_temperature()?.get::<degree_celsius>();
let setpoint = self.set_point.get::<degree_celsius>();
let mut output: f64 = self.controller.y1 - setpoint as f64 * f64::from(self.controller.parameters.ki)
+ input as f64 * f64::from(self.controller.parameters.kp + self.controller.parameters.ki + self.controller.parameters.kd)
- self.controller.x1 * f64::from(self.controller.parameters.kp + 2.0 * self.controller.parameters.kd)
+ self.controller.x2 * f64::from(self.controller.parameters.kd)
+ f64::from(self.controller.parameters.kp) * (setpoint as f64 - self.controller.u1);
if output < self.controller.parameters.output_min.into() {
output = self.controller.parameters.output_min.into();
}
if output > self.controller.parameters.output_max.into() {
output = self.controller.parameters.output_max.into();
}
self.controller.x2 = self.controller.x1;
self.controller.x1 = input as f64;
self.controller.u1 = setpoint as f64;
self.controller.y1 = output;
Some(output)
}
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)
}
pub fn apply_pid_params(&mut self, pid_params: Parameters){
self.controller.parameters = pid_params;
}
pub fn set_pid_params(&mut self, param: PidSettings, val: f32){
match param {
PidSettings::Kp => {
self.controller.parameters.kp = val;
}
PidSettings::Ki => {
self.controller.parameters.ki = val;
}
PidSettings::Kd => {
self.controller.parameters.kd = val;
}
PidSettings::Min => {
self.controller.parameters.output_min = val;
}
PidSettings::Max => {
self.controller.parameters.output_max = val;
}
}
}
pub fn reset_pid_state(&mut self){
self.controller.u1 = 0.0;
self.controller.x1 = 0.0;
self.controller.x2 = 0.0;
self.controller.y1 = 0.0;
}
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: f32){
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) -> Parameters {
self.controller.parameters
}
pub fn get_sh(&mut self) -> sh::Parameters {
self.sh
}
pub fn apply_sh(&mut self, sh: sh::Parameters) {
self.sh = sh;
}
}