forked from M-Labs/kirdy
pid: Use IDSP impl of Pid, Add Eth Pid Ctrl Cmds
This commit is contained in:
parent
4cd650367d
commit
f22ab430b8
24
Cargo.lock
generated
24
Cargo.lock
generated
@ -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"
|
||||
|
||||
@ -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"
|
||||
|
||||
@ -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"))
|
||||
|
||||
|
||||
36
src/main.rs
36
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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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::<degree_celsius>(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::<degree_celsius>(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::<ohm>(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) => {
|
||||
|
||||
@ -1 +0,0 @@
|
||||
pub mod pid;
|
||||
144
src/pid/pid.rs
144
src/pid/pid.rs
@ -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);
|
||||
}
|
||||
}
|
||||
@ -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<u32>,
|
||||
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<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,
|
||||
}
|
||||
|
||||
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(),
|
||||
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: 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(),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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::<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
|
||||
pub fn update_pid(&mut self) -> Option<f64> {
|
||||
let temperature = self.get_temperature()?
|
||||
.get::<degree_celsius>();
|
||||
let pid_output = self.pid.update(temperature);
|
||||
Some(pid_output)
|
||||
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)
|
||||
}
|
||||
|
||||
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<ElectricPotential> {
|
||||
@ -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::<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!()
|
||||
}
|
||||
}
|
||||
|
||||
@ -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)]
|
||||
|
||||
@ -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<u8> {
|
||||
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::<ampere>(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::<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 => {
|
||||
self.power_down();
|
||||
}
|
||||
_ => {}
|
||||
} else {
|
||||
self.power_down();
|
||||
}
|
||||
|
||||
channel
|
||||
})
|
||||
debug!("Temperature: {:?} degree", self.get_temperature().get::<degree_celsius>());
|
||||
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::<ampere>(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::<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 {
|
||||
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<ThermodynamicTemperature>,
|
||||
i_set: ElectricCurrent,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user