1
0
Fork 0

pid: Use IDSP impl of Pid, Add Eth Pid Ctrl Cmds

This commit is contained in:
linuswck 2024-02-06 15:20:23 +08:00
parent 4cd650367d
commit f22ab430b8
10 changed files with 404 additions and 273 deletions

24
Cargo.lock generated
View File

@ -358,6 +358,17 @@ dependencies = [
"stable_deref_trait", "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]] [[package]]
name = "ieee802_3_miim" name = "ieee802_3_miim"
version = "0.8.0" version = "0.8.0"
@ -385,6 +396,7 @@ dependencies = [
"cortex-m-rt", "cortex-m-rt",
"cortex-m-semihosting 0.5.0", "cortex-m-semihosting 0.5.0",
"fugit", "fugit",
"idsp",
"ieee802_3_miim", "ieee802_3_miim",
"log", "log",
"miniconf", "miniconf",
@ -501,7 +513,7 @@ version = "0.3.1"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f" checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f"
dependencies = [ dependencies = [
"num-complex", "num-complex 0.3.1",
"num-integer", "num-integer",
"num-iter", "num-iter",
"num-rational", "num-rational",
@ -517,6 +529,16 @@ dependencies = [
"num-traits", "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]] [[package]]
name = "num-integer" name = "num-integer"
version = "0.1.45" version = "0.1.45"

View File

@ -33,6 +33,7 @@ usbd-serial = "0.1.1"
fugit = "0.3.6" fugit = "0.3.6"
rtt-target = { version = "0.3.1", features = ["cortex-m"] } rtt-target = { version = "0.3.1", features = ["cortex-m"] }
miniconf = "0.9.0" miniconf = "0.9.0"
idsp = "0.14.1"
serde = { version = "1.0.158", features = ["derive"], default-features = false } serde = { version = "1.0.158", features = ["derive"], default-features = false }
sfkv = "0.1" sfkv = "0.1"
bit_field = "0.10" bit_field = "0.10"

View File

@ -2,12 +2,13 @@
# Kirdy is written to be controlled via a json object based on miniconf rust crate # Kirdy is written to be controlled via a json object based on miniconf rust crate
# Json Field: # Json Field:
# "rev": hw_rev # "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_f32": Optional f32 Data field depending on cmd
# "data_f64": Optional f64 Data field depending on cmd # "data_f64": Optional f64 Data field depending on cmd
import socket import socket
import json import json
import time
# Kirdy IP and Port Number # Kirdy IP and Port Number
HOST = "192.168.1.132" HOST = "192.168.1.132"
@ -19,6 +20,92 @@ ld_cmd = {
"data_f64": 0.0, "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: with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.connect((HOST, PORT)) 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"))

View File

@ -7,15 +7,15 @@ use stm32f4xx_hal::pac::{CorePeripherals, Peripherals};
mod device; mod device;
mod laser_diode; mod laser_diode;
mod thermostat; mod thermostat;
mod pid;
mod net; mod net;
use device::{boot::bootup, log_setup, sys_timer}; use device::{boot::bootup, log_setup, sys_timer};
use uom::fmt::DisplayStyle::Abbreviation; use uom::fmt::DisplayStyle::Abbreviation;
use uom::si::electric_potential::volt; use uom::si::electric_potential::volt;
use uom::si::electric_current::{ampere, milliampere}; use uom::si::electric_current::{ampere, milliampere};
use uom::si::thermodynamic_temperature::degree_celsius;
use uom::si::power::milliwatt; use uom::si::power::milliwatt;
use uom::si::f64::{ElectricPotential, ElectricCurrent, Power}; use uom::si::f64::{ElectricPotential, ElectricCurrent, Power, ThermodynamicTemperature};
use serde::{Serialize, Deserialize}; use serde::{Serialize, Deserialize};
// If RTT is used, print panic info through RTT // If RTT is used, print panic info through RTT
@ -31,14 +31,13 @@ fn panic(info: &PanicInfo) -> ! {
#[cfg(all(not(feature = "RTT"), not(test)))] #[cfg(all(not(feature = "RTT"), not(test)))]
use panic_halt as _; use panic_halt as _;
use miniconf::{Error, JsonCoreSlash, Tree, TreeKey};
static mut ETH_DATA_BUFFER: [u8; 1024] = [0; 1024]; static mut ETH_DATA_BUFFER: [u8; 1024] = [0; 1024];
#[cfg(not(test))] #[cfg(not(test))]
#[entry] #[entry]
fn main() -> ! { fn main() -> ! {
log_setup::init_log(); log_setup::init_log();
info!("Kirdy init"); info!("Kirdy init");
@ -82,22 +81,23 @@ fn main() -> ! {
loop { loop {
wd.feed(); 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; 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() { if net::net::eth_is_socket_active() {
cortex_m::interrupt::free(|cs| cortex_m::interrupt::free(|cs|
{ {
@ -116,7 +116,5 @@ fn main() -> ! {
} }
} }
} }
sys_timer::sleep(500);
} }
} }

View File

@ -2,11 +2,14 @@ use core::fmt::Debug;
use miniconf::{JsonCoreSlash, Tree}; use miniconf::{JsonCoreSlash, Tree};
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
use uom::si::{ use uom::si::{
electric_current::{ampere, milliampere, ElectricCurrent}, electric_current::{ampere, milliampere, ElectricCurrent},
electric_potential::{volt, ElectricPotential} electric_potential::{volt, ElectricPotential},
electrical_resistance::{ElectricalResistance, ohm},
f64::ThermodynamicTemperature, thermodynamic_temperature::degree_celsius
}; };
use crate::laser_diode::laser_diode::LdDrive; use crate::laser_diode::laser_diode::LdDrive;
use crate::thermostat::thermostat::Thermostat; use crate::thermostat::thermostat::Thermostat;
use crate::thermostat::pid_state::PidSettings::*;
use log::info; use log::info;
#[derive(Deserialize, Serialize, Copy, Clone, Default, Debug)] #[derive(Deserialize, Serialize, Copy, Clone, Default, Debug)]
@ -43,9 +46,7 @@ enum ThermostatCmdEnum {
SetTecMaxIPos, SetTecMaxIPos,
SetTecMaxINeg, SetTecMaxINeg,
SetTecIOut, // Constant Current Mode SetTecIOut, // Constant Current Mode
// Control MOde SetTemperatureSetpoint,
SetConstantCurrentMode,
SetPidControlMode,
// PID // PID
SetPidKp, SetPidKp,
SetPidKi, 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]){ match cmd.set_json("/json", &buffer[0..buffer_size]){
Ok(_) => { 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 { match cmd.json.laser_diode_cmd {
Some(LdCmdEnum::PowerUp) => { 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 { match cmd.json.thermostat_cmd {
Some(ThermostatCmdEnum::PowerUp) => { Some(ThermostatCmdEnum::PowerUp) => {
tec.power_up() tec.set_pid_engaged(true);
} }
Some(ThermostatCmdEnum::PowerDown) => { Some(ThermostatCmdEnum::PowerDown) => {
tec.power_down() tec.set_pid_engaged(false);
} }
Some(ThermostatCmdEnum::SetTecMaxV) => { Some(ThermostatCmdEnum::SetTecMaxV) => {
match cmd.json.data_f64 { 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) => { Some(ThermostatCmdEnum::SetTemperatureSetpoint) => {
info!("Not supported Yet") match cmd.json.data_f64 {
} Some(val) => {
Some(ThermostatCmdEnum::SetPidControlMode) => { tec.set_temperature_setpoint(ThermodynamicTemperature::new::<degree_celsius>(val));
info!("Not supported Yet") }
None => {
info!("Wrong Data type is received")
}
}
} }
Some(ThermostatCmdEnum::SetPidKp) => { 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) => { 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) => { 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) => { 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) => { 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) => { Some(ThermostatCmdEnum::SetPidUpdateInterval) => {
info!("Not supported Yet") info!("Not supported Yet")
} }
Some(ThermostatCmdEnum::SetShT0) => { Some(ThermostatCmdEnum::SetShT0) => {
info!("Not supported Yet") match cmd.json.data_f64 {
Some(val) => {
tec.set_sh_t0(ThermodynamicTemperature::new::<degree_celsius>(val));
}
None => {
info!("Wrong Data type is received")
}
}
} }
Some(ThermostatCmdEnum::SetShR0) => { Some(ThermostatCmdEnum::SetShR0) => {
info!("Not supported Yet") match cmd.json.data_f64 {
Some(val) => {
tec.set_sh_r0(ElectricalResistance::new::<ohm>(val));
}
None => {
info!("Wrong Data type is received")
}
}
} }
Some(ThermostatCmdEnum::SetShBeta) => { 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) => { Some(ThermostatCmdEnum::GetTecStatus) => {
info!("Not supported Yet") 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") info!("Unimplemented Command")
} }
} }
} }
Err(err) => { Err(err) => {

View File

@ -1 +0,0 @@
pub mod pid;

View File

@ -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);
}
}

View File

@ -1,92 +1,91 @@
use smoltcp::time::{Duration, Instant};
use uom::si::{ use uom::si::{
f64::{ electric_current::ampere, electric_potential::volt, electrical_resistance::ohm, f64::{
ElectricPotential, ElectricCurrent, ElectricPotential, ElectricalResistance, ThermodynamicTemperature
ElectricalResistance, }, thermodynamic_temperature::degree_celsius
ThermodynamicTemperature,
Time,
},
electric_potential::volt,
electrical_resistance::ohm,
thermodynamic_temperature::degree_celsius,
time::millisecond,
}; };
use crate::thermostat::{ use crate::thermostat::{
ad7172, ad7172,
steinhart_hart as sh, 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 R_INNER: f64 = 2.0 * 5100.0;
const VREF_SENS: f64 = 3.3 / 2.0; const VREF_SENS: f64 = 3.3 / 2.0;
pub struct PidState { pub struct PidState {
pub adc_data: Option<u32>, adc_data: Option<u32>,
pub adc_calibration: ad7172::ChannelCalibration, adc_calibration: ad7172::ChannelCalibration,
pub update_ts: Time, pid_engaged: bool,
pub update_interval: Time, pid: Biquad<f32>,
/// i_set 0A center point pid_out_min: ElectricCurrent,
pub center_point: ElectricPotential, pid_out_max: ElectricCurrent,
pub dac_volt: ElectricPotential, xy: [f32; 4],
pub pid_engaged: bool, set_point: ThermodynamicTemperature,
pub pid: pid::Controller, settings: Pid<f32>,
pub sh: sh::Parameters, sh: sh::Parameters,
}
impl PidState {
fn adc_calibration(mut self, adc_calibration: ad7172::ChannelCalibration) -> Self {
self.adc_calibration = adc_calibration;
self
}
} }
impl Default for PidState { impl Default for PidState {
fn default() -> Self { 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 { PidState {
adc_data: None, adc_data: None,
adc_calibration: ad7172::ChannelCalibration::default(), adc_calibration: ad7172::ChannelCalibration::default(),
update_ts: Time::new::<millisecond>(0.0),
// default: 10 Hz
update_interval: Time::new::<millisecond>(100.0),
center_point: ElectricPotential::new::<volt>(1.5),
dac_volt: ElectricPotential::new::<volt>(0.0),
pid_engaged: false, pid_engaged: false,
pid: pid::Controller::new(pid::Parameters::default()), 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(), sh: sh::Parameters::default(),
} }
} }
} }
impl PidState { pub enum PidSettings {
pub fn new(adc_calibration: ad7172::ChannelCalibration) -> Self { Kp,
PidState::default().adc_calibration(adc_calibration) 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 { self.adc_data = if adc_data == ad7172::MAX_VALUE {
// this means there is no thermistor plugged into the ADC. // this means there is no thermistor plugged into the ADC.
None None
} else { } else {
Some(adc_data) Some(adc_data)
}; };
self.update_interval = Time::new::<millisecond>(now.millis() as f64) - self.update_ts;
self.update_ts = Time::new::<millisecond>(now.millis() as f64);
} }
/// Update PID state on ADC input, calculate new DAC output /// Update PID state on ADC input, calculate new DAC output
pub fn update_pid(&mut self) -> Option<f64> { pub fn update_pid(&mut self) -> Option<f64> {
let temperature = self.get_temperature()? let input = (self.get_temperature()?.get::<degree_celsius>() as f32) - (self.set_point.get::<degree_celsius>() as f32);
.get::<degree_celsius>(); let pid_output = self.pid.update(&mut self.xy, input);
let pid_output = self.pid.update(temperature); debug!("BiQuad Storage [x0, x1, y0, y1]: {:?}", self.xy);
Some(pid_output) Some(pid_output as f64)
} }
pub fn get_update_ts(&self) -> Time { pub fn reset_pid_state(&mut self){
self.update_ts self.xy = [0.0; 4];
}
pub fn get_update_interval(&self) -> Time {
self.update_interval
} }
pub fn get_adc(&self) -> Option<ElectricPotential> { pub fn get_adc(&self) -> Option<ElectricPotential> {
@ -107,4 +106,76 @@ impl PidState {
let temperature = self.sh.get_temperature(r); let temperature = self.sh.get_temperature(r);
Some(temperature) 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::<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!()
}
} }

View File

@ -8,7 +8,7 @@ use uom::si::{
ratio::ratio, ratio::ratio,
thermodynamic_temperature::{degree_celsius, kelvin}, thermodynamic_temperature::{degree_celsius, kelvin},
}; };
use miniconf::{Tree, TreeDeserialize}; use miniconf::Tree;
/// Steinhart-Hart equation parameters /// Steinhart-Hart equation parameters
#[derive(Clone, Debug, PartialEq, Tree)] #[derive(Clone, Debug, PartialEq, Tree)]

View File

@ -1,17 +1,19 @@
use core::f64::NAN;
use core::marker::PhantomData; use core::marker::PhantomData;
use smoltcp::time::Instant; use crate::sys_timer;
use crate::{sys_timer, pid::pid};
use crate::thermostat::ad5680; use crate::thermostat::ad5680;
use crate::thermostat::max1968::{MAX1968, AdcReadTarget, PwmPinsEnum}; use crate::thermostat::max1968::{MAX1968, AdcReadTarget, PwmPinsEnum};
use crate::thermostat::ad7172; use crate::thermostat::ad7172;
use crate::thermostat::pid_state::PidState; use crate::thermostat::pid_state::{PidState, PidSettings};
use crate::thermostat::steinhart_hart; use crate::thermostat::steinhart_hart;
use idsp::iir::Pid;
use log::debug; use log::debug;
use uom::si::{ use uom::si::{
electric_current::ampere, electric_current::ampere,
electric_potential::volt, electric_potential::volt,
electrical_resistance::ohm, electrical_resistance::ohm,
f64::{ElectricCurrent, ElectricPotential, ElectricalResistance, Time, ThermodynamicTemperature}, thermodynamic_temperature::degree_celsius,
f64::{ThermodynamicTemperature, ElectricCurrent, ElectricPotential, ElectricalResistance},
ratio::ratio, ratio::ratio,
}; };
use miniconf::Tree; use miniconf::Tree;
@ -113,7 +115,7 @@ impl Thermostat{
pub fn setup(&mut self){ pub fn setup(&mut self){
self.tec_setup(); self.tec_setup();
let t_adc_ch0_cal = self.t_adc_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) { fn tec_setup(&mut self) {
@ -137,25 +139,28 @@ impl Thermostat{
adc_calibration0 adc_calibration0
} }
pub fn poll_adc(&mut self, instant: Instant) -> Option<u8> { pub fn poll_adc_and_update_pid(&mut self) -> bool {
self.ad7172.data_ready().unwrap().map(|channel| { self.ad7172.data_ready().unwrap().map(|_ch| {
let data = self.ad7172.read_data().unwrap(); let data = self.ad7172.read_data().unwrap();
let state: &mut PidState = &mut self.pid_ctrl_ch0; let state: &mut PidState = &mut self.pid_ctrl_ch0;
state.update(instant, data); state.update(data);
match state.update_pid() { debug!("state.get_pid_engaged(): {:?}", state.get_pid_engaged());
Some(pid_output) if state.pid_engaged => { if state.get_pid_engaged() {
// Forward PID output to i_set DAC match state.update_pid() {
self.set_i(ElectricCurrent::new::<ampere>(pid_output)); Some(pid_output) => {
self.power_up(); 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 => { }
} }
None if state.pid_engaged => { } else {
self.power_down(); self.power_down();
}
_ => {}
} }
debug!("Temperature: {:?} degree", self.get_temperature().get::<degree_celsius>());
channel true
}) });
false
} }
pub fn power_up(&mut self){ pub fn power_up(&mut self){
@ -164,6 +169,8 @@ impl Thermostat{
pub fn power_down(&mut self){ pub fn power_down(&mut self){
self.max1968.power_down(); self.max1968.power_down();
self.pid_ctrl_ch0.reset_pid_state();
self.set_i(ElectricCurrent::new::<ampere>(0.0));
} }
fn set_center_pt(&mut self, value: ElectricPotential){ fn set_center_pt(&mut self, value: ElectricPotential){
@ -259,15 +266,17 @@ impl Thermostat{
} }
} }
pub fn pid_engaged(&mut self) -> bool { pub fn set_pid_engaged(&mut self, val: bool) {
self.pid_ctrl_ch0.pid_engaged 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 { pub fn get_status_report(&mut self) -> StatusReport {
StatusReport { StatusReport {
pid_update_ts: self.pid_ctrl_ch0.get_update_ts(), pid_engaged: self.get_pid_engaged(),
pid_update_interval: self.pid_ctrl_ch0.get_update_interval(),
pid_engaged: self.pid_engaged(),
temperature: self.pid_ctrl_ch0.get_temperature(), temperature: self.pid_ctrl_ch0.get_temperature(),
i_set: self.tec_setting.i_set, i_set: self.tec_setting.i_set,
tec_i: self.get_tec_i(), tec_i: self.get_tec_i(),
@ -276,12 +285,21 @@ impl Thermostat{
} }
} }
pub fn get_pid_settings(&mut self) -> pid::Controller { pub fn get_temperature(&mut self) -> ThermodynamicTemperature {
self.pid_ctrl_ch0.pid.clone() match self.pid_ctrl_ch0.get_temperature() {
Some(val) => {
val
}
None => { ThermodynamicTemperature::new::<degree_celsius>(NAN) }
}
}
pub fn get_pid_settings(&mut self) -> Pid<f32> {
self.pid_ctrl_ch0.get_pid_settings()
} }
pub fn get_steinhart_hart(&mut self) -> steinhart_hart::Parameters { 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 { pub fn get_tec_settings(&mut self) -> TecSettingSummary {
@ -298,12 +316,29 @@ impl Thermostat{
self.max1968.get_calibrated_vdda() 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 { pub struct StatusReport {
pid_update_ts: Time,
pid_update_interval: Time,
pid_engaged: bool, pid_engaged: bool,
temperature: Option<ThermodynamicTemperature>, temperature: Option<ThermodynamicTemperature>,
i_set: ElectricCurrent, i_set: ElectricCurrent,