linuswck
/
kirdy-firmware
forked from M-Labs/kirdy
1
0
Fork 0
Code Pull Requests Activity

pid: Use Thermostat's PID algo instead of IDSP 

- Has lower steady state error upon testing with PID autotune
This commit is contained in:
linuswck 2024-02-15 11:00:53 +08:00
parent e9a396f001
commit ffa5f4e8ff
4 changed files with 94 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
Cargo.lock generated
View File

@ -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"

3
Cargo.toml
View File

@ -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"

139
src/thermostat/pid_state.rs
View File

@ -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<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,
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::<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);
PidState {
adc_data: None,
adc_calibration: ad7172::ChannelCalibration::default(),
pid_engaged: false,
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,
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<f64> {
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)
let input = self.get_temperature()?.get::<degree_celsius>();
let setpoint = self.set_point.get::<degree_celsius>();
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();
}
pub fn reset_pid_state(&mut self){
self.xy = [0.0; 4];
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<ElectricPotential> {
@ -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::<ampere>(val);
self.controller.parameters.output_min = val as f32;
}
PidSettings::Max => {
self.pid_out_max = ElectricCurrent::new::<ampere>(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::<ampere>() as f32);
self.pid.set_min(self.pid_out_min.get::<ampere>() 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<f32> {
unimplemented!()
pub fn get_pid_settings(&mut self) -> Parameters {
self.controller.parameters
}
pub fn get_sh(&mut self) -> sh::Parameters {

13
src/thermostat/thermostat.rs
View File

@ -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::<ampere>(pid_output));
debug!("Temperature Set Point: {:?} degree", self.pid_ctrl_ch0.get_pid_setpoint().get::<degree_celsius>());
self.power_up();
}
None => { }
}
} else {
self.power_down();
}
debug!("Temperature: {:?} degree", self.get_temperature().get::<degree_celsius>());
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<f32> {
pub fn get_pid_settings(&mut self) -> pid_state::Parameters {
self.pid_ctrl_ch0.get_pid_settings()
}