diff --git a/Cargo.lock b/Cargo.lock index 9cc85ff..005ba98 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -100,6 +100,7 @@ checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" dependencies = [ "bare-metal 0.2.5", "bitfield", + "critical-section", "embedded-hal 0.2.7", "volatile-register", ] @@ -358,17 +359,6 @@ 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" @@ -396,7 +386,6 @@ dependencies = [ "cortex-m-rt", "cortex-m-semihosting 0.5.0", "fugit", - "idsp", "ieee802_3_miim", "log", "miniconf", @@ -513,7 +502,7 @@ version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" dependencies = [ - "num-complex 0.3.1", + "num-complex", "num-integer", "num-iter", "num-rational", @@ -529,16 +518,6 @@ 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 1608f6e..62eac4d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,7 +15,7 @@ default-target = "thumbv7em-none-eabihf" [dependencies] panic-halt = "0.2.0" -cortex-m = "0.7.6" +cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = { version = "0.7.1", features = ["device"] } cortex-m-semihosting = "0.5.0" log = "0.4.17" @@ -33,7 +33,6 @@ 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/src/thermostat/pid_state.rs b/src/thermostat/pid_state.rs index a941991..6984c43 100644 --- a/src/thermostat/pid_state.rs +++ b/src/thermostat/pid_state.rs @@ -1,59 +1,76 @@ +use miniconf::Tree; use uom::si::{ - electric_current::ampere, electric_potential::volt, electrical_resistance::ohm, f64::{ - ElectricCurrent, ElectricPotential, ElectricalResistance, ThermodynamicTemperature + electric_potential::volt, electrical_resistance::ohm, f64::{ + ElectricPotential, ElectricalResistance, ThermodynamicTemperature }, thermodynamic_temperature::degree_celsius }; use crate::thermostat::{ ad7172, steinhart_hart as sh, }; -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; +#[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: -1.0, + output_max: 1.0, + } + } +} + +#[derive(Clone)] +pub struct Controller { + pub parameters: Parameters, + u1 : f64, + x1 : f64, + x2 : f64, + pub y1 : f64, +} + + pub struct PidState { 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, + controller: Controller, } 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(), pid_engaged: false, - 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(), + controller: Controller { + parameters: Parameters::default(), + u1 : 0.0, + x1 : 0.0, + x2 : 0.0, + y1 : 0.0, + }, } } } @@ -76,16 +93,33 @@ impl PidState { }; } - /// Update PID state on ADC input, calculate new DAC output + // 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 { - 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 reset_pid_state(&mut self){ - self.xy = [0.0; 4]; + let input = self.get_temperature()?.get::(); + let setpoint = self.set_point.get::(); + let mut output: f64 = self.controller.y1 - setpoint * f64::from(self.controller.parameters.ki) + + input * 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 - 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; + self.controller.u1 = setpoint; + self.controller.y1 = output; + Some(output) } pub fn get_adc(&self) -> Option { @@ -110,33 +144,28 @@ impl PidState { pub fn set_pid_params(&mut self, param: PidSettings, val: f64){ match param { PidSettings::Kp => { - self.settings.gain(Action::Kp, val as f32); + self.controller.parameters.kp = val as f32; } PidSettings::Ki => { - self.settings.gain(Action::Ki, val as f32); + self.controller.parameters.ki = val as f32; } PidSettings::Kd => { - self.settings.gain(Action::Kd, val as f32); + self.controller.parameters.kd = val as f32; } PidSettings::Min => { - self.pid_out_min = ElectricCurrent::new::(val); + self.controller.parameters.output_min = val as f32; } PidSettings::Max => { - self.pid_out_max = ElectricCurrent::new::(val); + self.controller.parameters.output_max = val as f32; } } - 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 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){ @@ -171,8 +200,8 @@ impl PidState { self.pid_engaged } - pub fn get_pid_settings(&mut self) -> Pid { - unimplemented!() + pub fn get_pid_settings(&mut self) -> Parameters { + self.controller.parameters } pub fn get_sh(&mut self) -> sh::Parameters { diff --git a/src/thermostat/thermostat.rs b/src/thermostat/thermostat.rs index 5922991..e856857 100644 --- a/src/thermostat/thermostat.rs +++ b/src/thermostat/thermostat.rs @@ -7,7 +7,6 @@ use crate::thermostat::ad7172; use crate::thermostat::pid_state::{PidState, PidSettings}; use crate::thermostat::steinhart_hart; use serde::{Deserialize, Serialize}; -use idsp::iir::Pid; use log::debug; use uom::si::{ electric_current::ampere, @@ -19,6 +18,8 @@ use uom::si::{ }; use miniconf::Tree; +use super::pid_state; + pub const R_SENSE: ElectricalResistance = ElectricalResistance { dimension: PhantomData, units: PhantomData, @@ -141,6 +142,7 @@ impl Thermostat{ } pub fn poll_adc_and_update_pid(&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; @@ -151,17 +153,14 @@ impl Thermostat{ 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 => { } } - } else { - self.power_down(); } debug!("Temperature: {:?} degree", self.get_temperature().get::()); - true + data_rdy = true; }); - false + data_rdy } pub fn power_up(&mut self){ @@ -296,7 +295,7 @@ impl Thermostat{ } } - pub fn get_pid_settings(&mut self) -> Pid { + pub fn get_pid_settings(&mut self) -> pid_state::Parameters { self.pid_ctrl_ch0.get_pid_settings() }