From f22ab430b8d80eb0552cc025d9de0a2242b0496f Mon Sep 17 00:00:00 2001 From: linuswck Date: Tue, 6 Feb 2024 15:20:23 +0800 Subject: [PATCH] pid: Use IDSP impl of Pid, Add Eth Pid Ctrl Cmds --- Cargo.lock | 24 ++++- Cargo.toml | 1 + eth_cmd_test.py | 91 +++++++++++++++- src/main.rs | 36 +++---- src/net/cmd_handler.rs | 106 +++++++++++++++---- src/pid/mod.rs | 1 - src/pid/pid.rs | 144 ------------------------- src/thermostat/pid_state.rs | 175 ++++++++++++++++++++++--------- src/thermostat/steinhart_hart.rs | 2 +- src/thermostat/thermostat.rs | 97 +++++++++++------ 10 files changed, 404 insertions(+), 273 deletions(-) delete mode 100644 src/pid/mod.rs delete mode 100644 src/pid/pid.rs diff --git a/Cargo.lock b/Cargo.lock index 4ea9460..9cc85ff 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -358,6 +358,17 @@ dependencies = [ "stable_deref_trait", ] +[[package]] +name = "idsp" +version = "0.14.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f255ee573949fb629362d10aa3abd0a97a7c4950a3b8890b435b8c7516cf38f" +dependencies = [ + "num-complex 0.4.4", + "num-traits", + "serde", +] + [[package]] name = "ieee802_3_miim" version = "0.8.0" @@ -385,6 +396,7 @@ dependencies = [ "cortex-m-rt", "cortex-m-semihosting 0.5.0", "fugit", + "idsp", "ieee802_3_miim", "log", "miniconf", @@ -501,7 +513,7 @@ version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" dependencies = [ - "num-complex", + "num-complex 0.3.1", "num-integer", "num-iter", "num-rational", @@ -517,6 +529,16 @@ dependencies = [ "num-traits", ] +[[package]] +name = "num-complex" +version = "0.4.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ba157ca0885411de85d6ca030ba7e2a83a28636056c7c699b07c8b6f7383214" +dependencies = [ + "num-traits", + "serde", +] + [[package]] name = "num-integer" version = "0.1.45" diff --git a/Cargo.toml b/Cargo.toml index b5f4d5b..1608f6e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -33,6 +33,7 @@ usbd-serial = "0.1.1" fugit = "0.3.6" rtt-target = { version = "0.3.1", features = ["cortex-m"] } miniconf = "0.9.0" +idsp = "0.14.1" serde = { version = "1.0.158", features = ["derive"], default-features = false } sfkv = "0.1" bit_field = "0.10" diff --git a/eth_cmd_test.py b/eth_cmd_test.py index 2bef560..e403f51 100644 --- a/eth_cmd_test.py +++ b/eth_cmd_test.py @@ -2,12 +2,13 @@ # Kirdy is written to be controlled via a json object based on miniconf rust crate # Json Field: # "rev": hw_rev -# "laser_diode_cmd": Check cmd_handler.rs for the cmd Enum to control the laser diode +# "laser_diode_cmd / thermostat_cmd": Check cmd_handler.rs for the list of cmds # "data_f32": Optional f32 Data field depending on cmd # "data_f64": Optional f64 Data field depending on cmd import socket import json +import time # Kirdy IP and Port Number HOST = "192.168.1.132" @@ -19,6 +20,92 @@ ld_cmd = { "data_f64": 0.0, } +tec_power_down = { + "rev": 3, + "thermostat_cmd": "PowerDown", +} + +tec_set_sh_t0_cmd = { + "rev": 3, + "thermostat_cmd": "SetShT0", + "data_f64": 25.0, +} + +tec_set_sh_r0_cmd = { + "rev": 3, + "thermostat_cmd": "SetShR0", + "data_f64": 10.0 * 1000, +} + +tec_set_sh_beta_cmd = { + "rev": 3, + "thermostat_cmd": "SetShBeta", + "data_f64": 3900.0, +} + +tec_set_temperature_setpoint_cmd = { + "rev": 3, + "thermostat_cmd": "SetTemperatureSetpoint", + "data_f64": 45.0, +} + +tec_set_pid_kp_cmd = { + "rev": 3, + "thermostat_cmd": "SetPidKp", + "data_f64": 1.0, +} + +tec_set_pid_ki_cmd = { + "rev": 3, + "thermostat_cmd": "SetPidKi", + "data_f64": 0.01, +} + +tec_set_pid_kd_cmd = { + "rev": 3, + "thermostat_cmd": "SetPidKd", + "data_f64": 0.0, +} + +tec_set_pid_out_min_cmd = { + "rev": 3, + "thermostat_cmd": "SetPidOutMin", + "data_f64": -1.0, +} + +tec_set_pid_out_max_cmd = { + "rev": 3, + "thermostat_cmd": "SetPidOutMax", + "data_f64": 1.0, +} + +tec_power_up = { + "rev": 3, + "thermostat_cmd": "PowerUp", +} + +# Current version of cmd_handler cannot service multiple cmds in the same eth buffer +delay = 0.25 with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: s.connect((HOST, PORT)) - s.send(bytes(json.dumps(ld_cmd), "UTF-8")) + s.send(bytes(json.dumps(tec_power_down), "UTF-8")) + time.sleep(delay) + s.send(bytes(json.dumps(tec_set_sh_t0_cmd), "UTF-8")) + time.sleep(delay) + s.send(bytes(json.dumps(tec_set_sh_r0_cmd), "UTF-8")) + time.sleep(delay) + s.send(bytes(json.dumps(tec_set_sh_beta_cmd), "UTF-8")) + time.sleep(delay) + s.send(bytes(json.dumps(tec_set_temperature_setpoint_cmd), "UTF-8")) + time.sleep(delay) + s.send(bytes(json.dumps(tec_set_pid_kp_cmd), "UTF-8")) + time.sleep(delay) + s.send(bytes(json.dumps(tec_set_pid_ki_cmd), "UTF-8")) + time.sleep(delay) + s.send(bytes(json.dumps(tec_set_pid_kd_cmd), "UTF-8")) + time.sleep(delay) + s.send(bytes(json.dumps(tec_power_up), "UTF-8")) + print("press enter to force thermostat to stop") + input() + s.send(bytes(json.dumps(tec_power_down), "UTF-8")) + diff --git a/src/main.rs b/src/main.rs index 227c834..3e9d80f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -7,15 +7,15 @@ use stm32f4xx_hal::pac::{CorePeripherals, Peripherals}; mod device; mod laser_diode; mod thermostat; -mod pid; mod net; use device::{boot::bootup, log_setup, sys_timer}; use uom::fmt::DisplayStyle::Abbreviation; use uom::si::electric_potential::volt; use uom::si::electric_current::{ampere, milliampere}; +use uom::si::thermodynamic_temperature::degree_celsius; use uom::si::power::milliwatt; -use uom::si::f64::{ElectricPotential, ElectricCurrent, Power}; +use uom::si::f64::{ElectricPotential, ElectricCurrent, Power, ThermodynamicTemperature}; use serde::{Serialize, Deserialize}; // If RTT is used, print panic info through RTT @@ -31,14 +31,13 @@ fn panic(info: &PanicInfo) -> ! { #[cfg(all(not(feature = "RTT"), not(test)))] use panic_halt as _; -use miniconf::{Error, JsonCoreSlash, Tree, TreeKey}; - static mut ETH_DATA_BUFFER: [u8; 1024] = [0; 1024]; #[cfg(not(test))] #[entry] fn main() -> ! { + log_setup::init_log(); info!("Kirdy init"); @@ -82,22 +81,23 @@ fn main() -> ! { loop { wd.feed(); - - info!("looping"); - info!("curr_ld_drive_cuurent: {:?}", mili_amp_fmt.with(laser.get_ld_drive_current())); - - info!("curr_dac_vfb: {:?}", volt_fmt.with(thermostat.get_dac_vfb())); - info!("curr_vref: {:?}", volt_fmt.with(thermostat.get_vref())); - info!("curr_tec_i: {:?}", amp_fmt.with(thermostat.get_tec_i())); - info!("curr_tec_v: {:?}", volt_fmt.with(thermostat.get_tec_v())); - info!("pd_mon_v: {:?}", volt_fmt.with(laser.pd_mon_status().v)); - info!("power_excursion: {:?}", laser.pd_mon_status().pwr_excursion); - - info!("Termination Status: {:?}", laser.get_term_status()); - let mut eth_is_pending = false; + 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())); + info!("curr_tec_i: {:?}", amp_fmt.with(thermostat.get_tec_i())); + info!("curr_tec_v: {:?}", volt_fmt.with(thermostat.get_tec_v())); + + info!("curr_ld_drive_cuurent: {:?}", mili_amp_fmt.with(laser.get_ld_drive_current())); + + info!("pd_mon_v: {:?}", volt_fmt.with(laser.pd_mon_status().v)); + info!("power_excursion: {:?}", laser.pd_mon_status().pwr_excursion); + + info!("Termination Status: {:?}", laser.get_term_status()); + } + if net::net::eth_is_socket_active() { cortex_m::interrupt::free(|cs| { @@ -116,7 +116,5 @@ fn main() -> ! { } } } - - sys_timer::sleep(500); } } diff --git a/src/net/cmd_handler.rs b/src/net/cmd_handler.rs index b97b171..cde48d1 100644 --- a/src/net/cmd_handler.rs +++ b/src/net/cmd_handler.rs @@ -2,11 +2,14 @@ use core::fmt::Debug; use miniconf::{JsonCoreSlash, Tree}; use serde::{Deserialize, Serialize}; use uom::si::{ - electric_current::{ampere, milliampere, ElectricCurrent}, - electric_potential::{volt, ElectricPotential} + electric_current::{ampere, milliampere, ElectricCurrent}, + electric_potential::{volt, ElectricPotential}, + electrical_resistance::{ElectricalResistance, ohm}, + f64::ThermodynamicTemperature, thermodynamic_temperature::degree_celsius }; use crate::laser_diode::laser_diode::LdDrive; use crate::thermostat::thermostat::Thermostat; +use crate::thermostat::pid_state::PidSettings::*; use log::info; #[derive(Deserialize, Serialize, Copy, Clone, Default, Debug)] @@ -43,9 +46,7 @@ enum ThermostatCmdEnum { SetTecMaxIPos, SetTecMaxINeg, SetTecIOut, // Constant Current Mode - // Control MOde - SetConstantCurrentMode, - SetPidControlMode, + SetTemperatureSetpoint, // PID SetPidKp, SetPidKi, @@ -82,7 +83,8 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive, mu }; match cmd.set_json("/json", &buffer[0..buffer_size]){ Ok(_) => { - info!("############ Command Received {:?}", cmd.json.laser_diode_cmd); + info!("############ Laser Diode Command Received {:?}", cmd.json.laser_diode_cmd); + info!("############ Thermostat Command Received {:?}", cmd.json.thermostat_cmd); match cmd.json.laser_diode_cmd { Some(LdCmdEnum::PowerUp) => { @@ -156,10 +158,10 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive, mu match cmd.json.thermostat_cmd { Some(ThermostatCmdEnum::PowerUp) => { - tec.power_up() + tec.set_pid_engaged(true); } Some(ThermostatCmdEnum::PowerDown) => { - tec.power_down() + tec.set_pid_engaged(false); } Some(ThermostatCmdEnum::SetTecMaxV) => { match cmd.json.data_f64 { @@ -201,38 +203,99 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive, mu } } } - Some(ThermostatCmdEnum::SetConstantCurrentMode) => { - info!("Not supported Yet") - } - Some(ThermostatCmdEnum::SetPidControlMode) => { - info!("Not supported Yet") + Some(ThermostatCmdEnum::SetTemperatureSetpoint) => { + match cmd.json.data_f64 { + Some(val) => { + tec.set_temperature_setpoint(ThermodynamicTemperature::new::(val)); + } + None => { + info!("Wrong Data type is received") + } + } } + Some(ThermostatCmdEnum::SetPidKp) => { - info!("Not supported Yet") + match cmd.json.data_f64 { + Some(val) => { + tec.set_pid(Kp, val); + } + None => { + info!("Wrong Data type is received") + } + } } Some(ThermostatCmdEnum::SetPidKi) => { - info!("Not supported Yet") + match cmd.json.data_f64 { + Some(val) => { + tec.set_pid(Ki, val); + } + None => { + info!("Wrong Data type is received") + } + } } Some(ThermostatCmdEnum::SetPidKd) => { - info!("Not supported Yet") + match cmd.json.data_f64 { + Some(val) => { + tec.set_pid(Kd, val); + } + None => { + info!("Wrong Data type is received") + } + } } Some(ThermostatCmdEnum::SetPidOutMin) => { - info!("Not supported Yet") + match cmd.json.data_f64 { + Some(val) => { + tec.set_pid(Min, val); + } + None => { + info!("Wrong Data type is received") + } + } } Some(ThermostatCmdEnum::SetPidOutMax) => { - info!("Not supported Yet") + match cmd.json.data_f64 { + Some(val) => { + tec.set_pid(Max, val); + } + None => { + info!("Wrong Data type is received") + } + } } Some(ThermostatCmdEnum::SetPidUpdateInterval) => { info!("Not supported Yet") } Some(ThermostatCmdEnum::SetShT0) => { - info!("Not supported Yet") + match cmd.json.data_f64 { + Some(val) => { + tec.set_sh_t0(ThermodynamicTemperature::new::(val)); + } + None => { + info!("Wrong Data type is received") + } + } } Some(ThermostatCmdEnum::SetShR0) => { - info!("Not supported Yet") + match cmd.json.data_f64 { + Some(val) => { + tec.set_sh_r0(ElectricalResistance::new::(val)); + } + None => { + info!("Wrong Data type is received") + } + } } Some(ThermostatCmdEnum::SetShBeta) => { - info!("Not supported Yet") + match cmd.json.data_f64 { + Some(val) => { + tec.set_sh_beta(val); + } + None => { + info!("Wrong Data type is received") + } + } } Some(ThermostatCmdEnum::GetTecStatus) => { info!("Not supported Yet") @@ -247,7 +310,6 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive, mu _ => { info!("Unimplemented Command") } - } } Err(err) => { diff --git a/src/pid/mod.rs b/src/pid/mod.rs deleted file mode 100644 index e319c78..0000000 --- a/src/pid/mod.rs +++ /dev/null @@ -1 +0,0 @@ -pub mod pid; \ No newline at end of file diff --git a/src/pid/pid.rs b/src/pid/pid.rs deleted file mode 100644 index b2129c5..0000000 --- a/src/pid/pid.rs +++ /dev/null @@ -1,144 +0,0 @@ -#[macro_use] -use miniconf::Tree; -use miniconf::{Serialize, Deserialize}; - -#[derive(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: -2.0, - output_max: 2.0, - } - } -} - -#[derive(Clone, Debug, PartialEq, Tree)] -pub struct Controller { - #[tree] - pub parameters: Parameters, - pub target : f64, - u1 : f64, - x1 : f64, - x2 : f64, - pub y1 : f64, -} - -impl Controller { - pub const fn new(parameters: Parameters) -> Controller { - Controller { - parameters: parameters, - target : 0.0, - u1 : 0.0, - x1 : 0.0, - x2 : 0.0, - y1 : 0.0, - } - } - - // 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(&mut self, input: f64) -> f64 { - - let mut output: f64 = self.y1 - self.target * f64::from(self.parameters.ki) - + input * f64::from(self.parameters.kp + self.parameters.ki + self.parameters.kd) - - self.x1 * f64::from(self.parameters.kp + 2.0 * self.parameters.kd) - + self.x2 * f64::from(self.parameters.kd) - + f64::from(self.parameters.kp) * (self.target - self.u1); - if output < self.parameters.output_min.into() { - output = self.parameters.output_min.into(); - } - if output > self.parameters.output_max.into() { - output = self.parameters.output_max.into(); - } - self.x2 = self.x1; - self.x1 = input; - self.u1 = self.target; - self.y1 = output; - output - } - - pub fn summary(&self) -> Summary { - Summary { - parameters: self.parameters.clone(), - target: self.target, - } - } -} - -#[derive(Clone, Debug, Tree)] -pub struct Summary { - #[tree] - parameters: Parameters, - target: f64, -} - -#[cfg(test)] -mod test { - use super::*; - - const PARAMETERS: Parameters = Parameters { - kp: 0.03, - ki: 0.002, - kd: 0.15, - output_min: -10.0, - output_max: 10.0, - }; - - #[test] - fn test_controller() { - // Initial and ambient temperature - const DEFAULT: f64 = 20.0; - // Target temperature - const TARGET: f64 = 40.0; - // Control tolerance - const ERROR: f64 = 0.01; - // System response delay - const DELAY: usize = 10; - // Heat lost - const LOSS: f64 = 0.05; - // Limit simulation cycle, reaching this limit before settling fails test - const CYCLE_LIMIT: u32 = 1000; - - let mut pid = Controller::new(PARAMETERS.clone()); - pid.target = TARGET; - - let mut values = [DEFAULT; DELAY]; - let mut t = 0; - let mut total_t = 0; - let mut output: f64 = 0.0; - let target = (TARGET - ERROR)..=(TARGET + ERROR); - while !values.iter().all(|value| target.contains(value)) && total_t < CYCLE_LIMIT { - let next_t = (t + 1) % DELAY; - // Feed the oldest temperature - output = pid.update(values[next_t]); - // Overwrite oldest with previous temperature - output - values[next_t] = values[t] - output - (values[t] - DEFAULT) * LOSS; - t = next_t; - total_t += 1; - println!("{}", values[t].to_string()); - } - assert_ne!(CYCLE_LIMIT, total_t); - } -} diff --git a/src/thermostat/pid_state.rs b/src/thermostat/pid_state.rs index 9651340..a941991 100644 --- a/src/thermostat/pid_state.rs +++ b/src/thermostat/pid_state.rs @@ -1,92 +1,91 @@ -use smoltcp::time::{Duration, Instant}; use uom::si::{ - f64::{ - ElectricPotential, - ElectricalResistance, - ThermodynamicTemperature, - Time, - }, - electric_potential::volt, - electrical_resistance::ohm, - thermodynamic_temperature::degree_celsius, - time::millisecond, + electric_current::ampere, electric_potential::volt, electrical_resistance::ohm, f64::{ + ElectricCurrent, ElectricPotential, ElectricalResistance, ThermodynamicTemperature + }, thermodynamic_temperature::degree_celsius }; use crate::thermostat::{ ad7172, steinhart_hart as sh, }; -use crate::pid::pid; +use idsp::iir::{Pid, Action, Biquad}; +use crate::debug; const R_INNER: f64 = 2.0 * 5100.0; const VREF_SENS: f64 = 3.3 / 2.0; pub struct PidState { - pub adc_data: Option, - pub adc_calibration: ad7172::ChannelCalibration, - pub update_ts: Time, - pub update_interval: Time, - /// i_set 0A center point - pub center_point: ElectricPotential, - pub dac_volt: ElectricPotential, - pub pid_engaged: bool, - pub pid: pid::Controller, - pub sh: sh::Parameters, -} - -impl PidState { - fn adc_calibration(mut self, adc_calibration: ad7172::ChannelCalibration) -> Self { - self.adc_calibration = adc_calibration; - self - } + adc_data: Option, + adc_calibration: ad7172::ChannelCalibration, + pid_engaged: bool, + pid: Biquad, + pid_out_min: ElectricCurrent, + pid_out_max: ElectricCurrent, + xy: [f32; 4], + set_point: ThermodynamicTemperature, + settings: Pid, + sh: sh::Parameters, } impl Default for PidState { fn default() -> Self { + const OUT_MIN: f32 = -1.0; + const OUT_MAX: f32 = 1.0; + + let mut pid_settings = Pid::::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 = pid_settings.build().unwrap().into(); + pid.set_min(OUT_MIN); + pid.set_max(OUT_MAX); + PidState { adc_data: None, adc_calibration: ad7172::ChannelCalibration::default(), - update_ts: Time::new::(0.0), - // default: 10 Hz - update_interval: Time::new::(100.0), - center_point: ElectricPotential::new::(1.5), - dac_volt: ElectricPotential::new::(0.0), pid_engaged: false, - pid: pid::Controller::new(pid::Parameters::default()), + pid: pid, + pid_out_min: ElectricCurrent::new::(OUT_MIN as f64), + pid_out_max: ElectricCurrent::new::(OUT_MAX as f64), + xy: [0.0; 4], + set_point: ThermodynamicTemperature::new::(0.0), + settings: pid_settings, sh: sh::Parameters::default(), } } } -impl PidState { - pub fn new(adc_calibration: ad7172::ChannelCalibration) -> Self { - PidState::default().adc_calibration(adc_calibration) - } +pub enum PidSettings { + Kp, + Ki, + Kd, + Min, + Max, +} - pub fn update(&mut self, now: Instant, adc_data: u32) { +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) }; - self.update_interval = Time::new::(now.millis() as f64) - self.update_ts; - self.update_ts = Time::new::(now.millis() as f64); } /// Update PID state on ADC input, calculate new DAC output pub fn update_pid(&mut self) -> Option { - let temperature = self.get_temperature()? - .get::(); - let pid_output = self.pid.update(temperature); - Some(pid_output) + let input = (self.get_temperature()?.get::() as f32) - (self.set_point.get::() 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) } - pub fn get_update_ts(&self) -> Time { - self.update_ts - } - - pub fn get_update_interval(&self) -> Time { - self.update_interval + pub fn reset_pid_state(&mut self){ + self.xy = [0.0; 4]; } pub fn get_adc(&self) -> Option { @@ -107,4 +106,76 @@ impl PidState { let temperature = self.sh.get_temperature(r); Some(temperature) } + + 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::(val); + } + PidSettings::Max => { + self.pid_out_max = ElectricCurrent::new::(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::() as f32); + self.pid.set_min(self.pid_out_min.get::() 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 { + unimplemented!() + } + + pub fn get_sh(&mut self) -> sh::Parameters { + unimplemented!() + } } diff --git a/src/thermostat/steinhart_hart.rs b/src/thermostat/steinhart_hart.rs index 311074a..153cfc1 100644 --- a/src/thermostat/steinhart_hart.rs +++ b/src/thermostat/steinhart_hart.rs @@ -8,7 +8,7 @@ use uom::si::{ ratio::ratio, thermodynamic_temperature::{degree_celsius, kelvin}, }; -use miniconf::{Tree, TreeDeserialize}; +use miniconf::Tree; /// Steinhart-Hart equation parameters #[derive(Clone, Debug, PartialEq, Tree)] diff --git a/src/thermostat/thermostat.rs b/src/thermostat/thermostat.rs index 7c17b4c..322694e 100644 --- a/src/thermostat/thermostat.rs +++ b/src/thermostat/thermostat.rs @@ -1,17 +1,19 @@ +use core::f64::NAN; use core::marker::PhantomData; -use smoltcp::time::Instant; -use crate::{sys_timer, pid::pid}; +use crate::sys_timer; use crate::thermostat::ad5680; use crate::thermostat::max1968::{MAX1968, AdcReadTarget, PwmPinsEnum}; use crate::thermostat::ad7172; -use crate::thermostat::pid_state::PidState; +use crate::thermostat::pid_state::{PidState, PidSettings}; use crate::thermostat::steinhart_hart; +use idsp::iir::Pid; use log::debug; use uom::si::{ electric_current::ampere, electric_potential::volt, electrical_resistance::ohm, - f64::{ElectricCurrent, ElectricPotential, ElectricalResistance, Time, ThermodynamicTemperature}, + thermodynamic_temperature::degree_celsius, + f64::{ThermodynamicTemperature, ElectricCurrent, ElectricPotential, ElectricalResistance}, ratio::ratio, }; use miniconf::Tree; @@ -113,7 +115,7 @@ impl Thermostat{ pub fn setup(&mut self){ self.tec_setup(); let t_adc_ch0_cal = self.t_adc_setup(); - self.pid_ctrl_ch0.adc_calibration = t_adc_ch0_cal; + self.pid_ctrl_ch0.set_adc_calibration(t_adc_ch0_cal) ; } fn tec_setup(&mut self) { @@ -137,25 +139,28 @@ impl Thermostat{ adc_calibration0 } - pub fn poll_adc(&mut self, instant: Instant) -> Option { - self.ad7172.data_ready().unwrap().map(|channel| { + pub fn poll_adc_and_update_pid(&mut self) -> bool { + 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(instant, data); - match state.update_pid() { - Some(pid_output) if state.pid_engaged => { - // Forward PID output to i_set DAC - self.set_i(ElectricCurrent::new::(pid_output)); - self.power_up(); + state.update(data); + debug!("state.get_pid_engaged(): {:?}", state.get_pid_engaged()); + if state.get_pid_engaged() { + match state.update_pid() { + Some(pid_output) => { + self.set_i(ElectricCurrent::new::(pid_output)); + debug!("Temperature Set Point: {:?} degree", self.pid_ctrl_ch0.get_pid_setpoint().get::()); + self.power_up(); + } + None => { } } - None if state.pid_engaged => { - self.power_down(); - } - _ => {} + } else { + self.power_down(); } - - channel - }) + debug!("Temperature: {:?} degree", self.get_temperature().get::()); + true + }); + false } pub fn power_up(&mut self){ @@ -164,6 +169,8 @@ impl Thermostat{ pub fn power_down(&mut self){ self.max1968.power_down(); + self.pid_ctrl_ch0.reset_pid_state(); + self.set_i(ElectricCurrent::new::(0.0)); } fn set_center_pt(&mut self, value: ElectricPotential){ @@ -259,15 +266,17 @@ impl Thermostat{ } } - pub fn pid_engaged(&mut self) -> bool { - self.pid_ctrl_ch0.pid_engaged + pub fn set_pid_engaged(&mut self, val: bool) { + self.pid_ctrl_ch0.set_pid_engaged(val); + } + + pub fn get_pid_engaged(&mut self) -> bool { + self.pid_ctrl_ch0.get_pid_engaged() } pub fn get_status_report(&mut self) -> StatusReport { StatusReport { - pid_update_ts: self.pid_ctrl_ch0.get_update_ts(), - pid_update_interval: self.pid_ctrl_ch0.get_update_interval(), - pid_engaged: self.pid_engaged(), + pid_engaged: self.get_pid_engaged(), temperature: self.pid_ctrl_ch0.get_temperature(), i_set: self.tec_setting.i_set, tec_i: self.get_tec_i(), @@ -276,12 +285,21 @@ impl Thermostat{ } } - pub fn get_pid_settings(&mut self) -> pid::Controller { - self.pid_ctrl_ch0.pid.clone() + pub fn get_temperature(&mut self) -> ThermodynamicTemperature { + match self.pid_ctrl_ch0.get_temperature() { + Some(val) => { + val + } + None => { ThermodynamicTemperature::new::(NAN) } + } + } + + pub fn get_pid_settings(&mut self) -> Pid { + self.pid_ctrl_ch0.get_pid_settings() } pub fn get_steinhart_hart(&mut self) -> steinhart_hart::Parameters { - self.pid_ctrl_ch0.sh.clone() + self.pid_ctrl_ch0.get_sh() } pub fn get_tec_settings(&mut self) -> TecSettingSummary { @@ -298,12 +316,29 @@ impl Thermostat{ self.max1968.get_calibrated_vdda() } + pub fn set_pid(&mut self, param: PidSettings, val: f64){ + self.pid_ctrl_ch0.set_pid_params(param, val); + } + + pub fn set_sh_beta(&mut self, beta: f64) { + 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) { + self.pid_ctrl_ch0.set_pid_setpoint(t); + } } -#[derive(Tree)] +#[derive(Tree, Debug)] pub struct StatusReport { - pid_update_ts: Time, - pid_update_interval: Time, pid_engaged: bool, temperature: Option, i_set: ElectricCurrent,