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

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"

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"

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"))
+

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");
 
@@ -83,20 +82,21 @@ fn main() -> ! {
     loop {
         wd.feed();
 
-        info!("looping");
+        let mut eth_is_pending = false;
-        info!("curr_ld_drive_cuurent: {:?}", mili_amp_fmt.with(laser.get_ld_drive_current()));
         
+        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());
-        
+        }
-        let mut eth_is_pending = false;
         
         if net::net::eth_is_socket_active() {
             cortex_m::interrupt::free(|cs| 
@@ -112,11 +112,9 @@ fn main() -> ! {
                     });
                     let bytes = net::net::eth_recv(&mut ETH_DATA_BUFFER);
                     debug!("Number of bytes recv: {:?}", bytes);
-                    laser = net::cmd_handler::execute_cmd(&mut ETH_DATA_BUFFER, bytes, laser);
+                    (laser, thermostat) = net::cmd_handler::execute_cmd(&mut ETH_DATA_BUFFER, bytes, laser, thermostat);
                 }
             }
         }
-        
-        sys_timer::sleep(500);
     }
 }

src/net/cmd_handler.rs

@@ -1,8 +1,15 @@
 use core::fmt::Debug;
-use miniconf::{Error, JsonCoreSlash, Tree, TreeKey};
+use miniconf::{JsonCoreSlash, Tree};
 use serde::{Deserialize, Serialize};
-use uom::si::electric_current::{milliampere, ElectricCurrent};
+use uom::si::{
+    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)]
@@ -28,10 +35,40 @@ enum LdCmdEnum {
     GetAlramStatus,
 }
 
+#[derive(Deserialize, Serialize, Copy, Clone, Default, Debug)]
+enum ThermostatCmdEnum {
+    #[default]
+    Reserved,
+    PowerUp,
+    PowerDown,
+    // TEC
+    SetTecMaxV,
+    SetTecMaxIPos,
+    SetTecMaxINeg,
+    SetTecIOut, // Constant Current Mode
+    SetTemperatureSetpoint,
+    // PID
+    SetPidKp,
+    SetPidKi,
+    SetPidKd,
+    SetPidOutMin,
+    SetPidOutMax,
+    SetPidUpdateInterval, // Update Interval is set based on the sampling rate of ADC
+    // Steinhart-Hart Equation
+    SetShT0,
+    SetShR0,
+    SetShBeta,
+    // Report Related
+    GetTecStatus,
+    GetPidStatus,
+    GetShParams,
+}
+
 #[derive(Deserialize, Serialize, Copy, Clone, Debug, Default, Tree)]
 pub struct CmdJsonObj{
     rev: u8,
-    laser_diode_cmd: LdCmdEnum,
+    laser_diode_cmd: Option<LdCmdEnum>,
+    thermostat_cmd: Option<ThermostatCmdEnum>,
     data_f32: Option<f32>,
     data_f64: Option<f64>,
 }
@@ -40,28 +77,29 @@ pub struct Cmd {
     json: CmdJsonObj
 }
 
-pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive)->(LdDrive){
+pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive, mut tec: Thermostat)->(LdDrive, Thermostat){
     let mut cmd = Cmd {
         json: CmdJsonObj::default()
     };
     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 {
-                LdCmdEnum::PowerUp => {
+                Some(LdCmdEnum::PowerUp) => {
                     laser.power_up()
                 }
-                LdCmdEnum::PowerDown => {
+                Some(LdCmdEnum::PowerDown) => {
                     laser.power_down()
                 }
-                LdCmdEnum::LdTermsShort => {
+                Some(LdCmdEnum::LdTermsShort) => {
                     laser.ld_short();
                 }
-                LdCmdEnum::LdTermsOpen => {
+                Some(LdCmdEnum::LdTermsOpen) => {
                     laser.ld_open();
                 }
-                LdCmdEnum::SetI => {
+                Some(LdCmdEnum::SetI) => {
                     match cmd.json.data_f64 {
                         Some(val) => {
                             laser.ld_set_i(ElectricCurrent::new::<milliampere>(val));
@@ -71,7 +109,7 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive)->(
                         }
                     }
                 }
-                LdCmdEnum::SetISoftLimit => {
+                Some(LdCmdEnum::SetISoftLimit) => {
                     match cmd.json.data_f64 {
                         Some(val) => {
                             laser.set_ld_drive_current_limit(ElectricCurrent::new::<milliampere>(val))
@@ -81,13 +119,13 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive)->(
                         }
                     }
                 }
-                LdCmdEnum::SetPdResponsitivity => {
+                Some(LdCmdEnum::SetPdResponsitivity) => {
                     info!("Not supported Yet")
                 }
-                LdCmdEnum::SetPdDarkCurrent => {
+                Some(LdCmdEnum::SetPdDarkCurrent) => {
                     info!("Not supported Yet")
                 }
-                LdCmdEnum::SetPdILimit => {
+                Some(LdCmdEnum::SetPdILimit) => {
                     match cmd.json.data_f64 {
                         Some(val) => {
                             laser.set_pd_i_limit(ElectricCurrent::new::<milliampere>(val))
@@ -97,21 +135,178 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive)->(
                         }
                     }
                 }
-                LdCmdEnum::SetLdPwrLimit => {
+                Some(LdCmdEnum::SetLdPwrLimit) => {
                     info!("Not supported Yet")
                 }
-                LdCmdEnum::ClearAlarmStatus => {
+                Some(LdCmdEnum::ClearAlarmStatus) => {
                     laser.pd_mon_clear_alarm()
                 }
-                LdCmdEnum::GetModInTermStatus => {
+                Some(LdCmdEnum::GetModInTermStatus) => {
                     info!("Not supported Yet")
                 }
-                LdCmdEnum::GetLdStatus => {
+                Some(LdCmdEnum::GetLdStatus) => {
                     info!("Not supported Yet")
                 }
-                LdCmdEnum::GetAlramStatus => {
+                Some(LdCmdEnum::GetAlramStatus) => {
                     info!("Not supported Yet")
                 }
+                None => { /* Do Nothing*/ }
+                _ => {
+                    info!("Unimplemented Command")
+                }
+            }
+
+            match cmd.json.thermostat_cmd {
+                Some(ThermostatCmdEnum::PowerUp) => {
+                    tec.set_pid_engaged(true);
+                }
+                Some(ThermostatCmdEnum::PowerDown) => {
+                    tec.set_pid_engaged(false);
+                }
+                Some(ThermostatCmdEnum::SetTecMaxV) => {
+                    match cmd.json.data_f64 {
+                        Some(val) => {
+                            tec.set_max_v(ElectricPotential::new::<volt>(val));
+                        }
+                        None => {
+                            info!("Wrong Data type is received")
+                        }
+                    }
+                }
+                Some(ThermostatCmdEnum::SetTecMaxIPos) => {
+                    match cmd.json.data_f64 {
+                        Some(val) => {
+                            tec.set_max_i_pos(ElectricCurrent::new::<ampere>(val));
+                        }
+                        None => {
+                            info!("Wrong Data type is received")
+                        }
+                    }
+                }
+                Some(ThermostatCmdEnum::SetTecMaxINeg) => {
+                    match cmd.json.data_f64 {
+                        Some(val) => {
+                            tec.set_max_i_pos(ElectricCurrent::new::<milliampere>(val));
+                        }
+                        None => {
+                            info!("Wrong Data type is received")
+                        }
+                    }
+                }
+                Some(ThermostatCmdEnum::SetTecIOut) => {
+                    match cmd.json.data_f64 {
+                        Some(val) => {
+                            tec.set_i(ElectricCurrent::new::<milliampere>(val));
+                        }
+                        None => {
+                            info!("Wrong Data type is received")
+                        }
+                    }
+                }
+                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) => {
+                    match cmd.json.data_f64 {
+                        Some(val) => {
+                            tec.set_pid(Kp, val);
+                        }
+                        None => {
+                            info!("Wrong Data type is received")
+                        }
+                    }
+                }
+                Some(ThermostatCmdEnum::SetPidKi) => {
+                    match cmd.json.data_f64 {
+                        Some(val) => {
+                            tec.set_pid(Ki, val);
+                        }
+                        None => {
+                            info!("Wrong Data type is received")
+                        }
+                    }
+                }
+                Some(ThermostatCmdEnum::SetPidKd) => {
+                    match cmd.json.data_f64 {
+                        Some(val) => {
+                            tec.set_pid(Kd, val);
+                        }
+                        None => {
+                            info!("Wrong Data type is received")
+                        }
+                    }
+                }
+                Some(ThermostatCmdEnum::SetPidOutMin) => {
+                    match cmd.json.data_f64 {
+                        Some(val) => {
+                            tec.set_pid(Min, val);
+                        }
+                        None => {
+                            info!("Wrong Data type is received")
+                        }
+                    }
+                }
+                Some(ThermostatCmdEnum::SetPidOutMax) => {
+                    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) => {
+                    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) => {
+                    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) => {
+                    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")
+                }
+                Some(ThermostatCmdEnum::GetPidStatus) => {
+                    info!("Not supported Yet")
+                }
+                Some(ThermostatCmdEnum::GetShParams) => {
+                    info!("Not supported Yet")
+                }
+                None => { /* Do Nothing*/ }
                 _ => { 
                     info!("Unimplemented Command") 
                 }
@@ -121,5 +316,5 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive)->(
             info!("Invalid Command: {:?}", err);
         }
     }
-    laser
+    (laser, tec)
 }

src/pid/mod.rs

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

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

src/thermostat/pid_state.rs

@@ -1,92 +1,91 @@
-use smoltcp::time::{Duration, Instant};
 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::{
     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>,
+    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 {
     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 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 {
             // 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()?
+        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> {
@@ -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!()
+    }
 }

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)]

src/thermostat/thermostat.rs

@@ -1,17 +1,19 @@
+use core::f64::NAN;
 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::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> {
+    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 state: &mut PidState = &mut self.pid_ctrl_ch0;
-            state.update(instant, data);
+            state.update(data);
+            debug!("state.get_pid_engaged(): {:?}", state.get_pid_engaged());
+            if state.get_pid_engaged() {
                 match state.update_pid() {
-                Some(pid_output) if state.pid_engaged => {
+                    Some(pid_output) => {
-                    // Forward PID output to i_set DAC
                         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 if state.pid_engaged => {
+                    None => { }
+                }
+            } else {
                 self.power_down();
             }
-                _ => {}
+            debug!("Temperature: {:?} degree", self.get_temperature().get::<degree_celsius>());
-            }
+            true
-
+        });
-            channel
+        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,18 +266,17 @@ impl Thermostat{
         }
     }
     
-    pub fn pid_engaged(&mut self) -> bool {
+    pub fn set_pid_engaged(&mut self, val: bool) {
-        if self.pid_ctrl_ch0.pid_engaged {
+        self.pid_ctrl_ch0.set_pid_engaged(val);
-            return true;
     }
-        false
+
+    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_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(),
             i_set: self.tec_setting.i_set,
             tec_i: self.get_tec_i(),
@@ -279,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 {
-        self.pid_ctrl_ch0.sh.clone()
+        self.pid_ctrl_ch0.get_sh()
     }
 
     pub fn get_tec_settings(&mut self) -> TecSettingSummary {
@@ -301,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,