pid: Use Thermostat's PID algo instead of IDSP

- Has lower steady state error upon testing with PID autotune

Cargo.lock

@@ -100,6 +100,7 @@ checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9"
 dependencies = [
  "bare-metal 0.2.5",
  "bitfield",
+ "critical-section",
  "embedded-hal 0.2.7",
  "volatile-register",
 ]
@@ -358,17 +359,6 @@ dependencies = [
  "stable_deref_trait",
 ]
 
-[[package]]
-name = "idsp"
-version = "0.14.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8f255ee573949fb629362d10aa3abd0a97a7c4950a3b8890b435b8c7516cf38f"
-dependencies = [
- "num-complex 0.4.4",
- "num-traits",
- "serde",
-]
-
 [[package]]
 name = "ieee802_3_miim"
 version = "0.8.0"
@@ -396,7 +386,6 @@ dependencies = [
  "cortex-m-rt",
  "cortex-m-semihosting 0.5.0",
  "fugit",
- "idsp",
  "ieee802_3_miim",
  "log",
  "miniconf",
@@ -513,7 +502,7 @@ version = "0.3.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8b7a8e9be5e039e2ff869df49155f1c06bd01ade2117ec783e56ab0932b67a8f"
 dependencies = [
- "num-complex 0.3.1",
+ "num-complex",
  "num-integer",
  "num-iter",
  "num-rational",
@@ -529,16 +518,6 @@ dependencies = [
  "num-traits",
 ]
 
-[[package]]
-name = "num-complex"
-version = "0.4.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1ba157ca0885411de85d6ca030ba7e2a83a28636056c7c699b07c8b6f7383214"
-dependencies = [
- "num-traits",
- "serde",
-]
-
 [[package]]
 name = "num-integer"
 version = "0.1.45"

Cargo.toml

@@ -15,7 +15,7 @@ default-target = "thumbv7em-none-eabihf"
 
 [dependencies]
 panic-halt = "0.2.0"
-cortex-m = "0.7.6"
+cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] }
 cortex-m-rt = { version = "0.7.1", features = ["device"] }
 cortex-m-semihosting = "0.5.0"
 log = "0.4.17"
@@ -33,7 +33,6 @@ usbd-serial = "0.1.1"
 fugit = "0.3.6"
 rtt-target = { version = "0.3.1", features = ["cortex-m"] }
 miniconf = "0.9.0"
-idsp =  "0.14.1"
 serde = { version = "1.0.158", features = ["derive"], default-features = false }
 sfkv = "0.1"
 bit_field = "0.10"

src/thermostat/pid_state.rs

@@ -1,59 +1,76 @@
+use miniconf::Tree;
 use uom::si::{
-    electric_current::ampere, electric_potential::volt, electrical_resistance::ohm, f64::{
+    electric_potential::volt, electrical_resistance::ohm, f64::{
-    ElectricCurrent, ElectricPotential, ElectricalResistance, ThermodynamicTemperature
+    ElectricPotential, ElectricalResistance, ThermodynamicTemperature
     }, thermodynamic_temperature::degree_celsius
 };
 use crate::thermostat::{
     ad7172,
     steinhart_hart as sh,
 };
-use idsp::iir::{Pid, Action, Biquad};
-use crate::debug;
-
 const R_INNER: f64 = 2.0 * 5100.0;
 const VREF_SENS: f64 = 3.3 / 2.0;
 
+#[derive(Clone, Copy, Debug, PartialEq, Tree)]
+pub struct Parameters {
+    /// Gain coefficient for proportional term
+    pub kp: f32,
+    /// Gain coefficient for integral term
+    pub ki: f32,
+    /// Gain coefficient for derivative term
+    pub kd: f32,
+    /// Output limit minimum
+    pub output_min: f32,
+    /// Output limit maximum
+    pub output_max: f32,
+}
+
+impl Default for Parameters {
+    fn default() -> Self {
+        Parameters {
+            kp: 0.0,
+            ki: 0.0,
+            kd: 0.0,
+            output_min: -1.0,
+            output_max: 1.0,
+        }
+    }
+}
+
+#[derive(Clone)]
+pub struct Controller {
+    pub parameters: Parameters,
+    u1 : f64,
+    x1 : f64,
+    x2 : f64,
+    pub y1 : f64,
+}
+
+
 pub struct PidState {
     adc_data: Option<u32>,
     adc_calibration: ad7172::ChannelCalibration,
     pid_engaged: bool,
-    pid: Biquad<f32>,
-    pid_out_min: ElectricCurrent,
-    pid_out_max: ElectricCurrent,
-    xy: [f32; 4],
     set_point: ThermodynamicTemperature,
-    settings: Pid<f32>,
     sh: sh::Parameters,
+    controller: Controller,
 }
 
 impl Default for PidState {
     fn default() -> Self {
-        const OUT_MIN: f32 = -1.0;
-        const OUT_MAX: f32 = 1.0;
-
-        let mut pid_settings = Pid::<f32>::default();
-        pid_settings
-            // Pid Update Period depends on the AD7172 Output Data Rate Set
-            .period(0.1)
-            .limit(Action::Kp, 10.0)
-            .limit(Action::Ki, 10.0)
-            .limit(Action::Kd, 10.0);
-
-        let mut pid: Biquad<f32> = pid_settings.build().unwrap().into();
-        pid.set_min(OUT_MIN);
-        pid.set_max(OUT_MAX);
-
         PidState {
             adc_data: None,
             adc_calibration: ad7172::ChannelCalibration::default(),
             pid_engaged: false,
-            pid: pid,
-            pid_out_min: ElectricCurrent::new::<ampere>(OUT_MIN as f64),
-            pid_out_max: ElectricCurrent::new::<ampere>(OUT_MAX as f64),
-            xy: [0.0; 4],
             set_point: ThermodynamicTemperature::new::<degree_celsius>(0.0),
-            settings: pid_settings,
             sh: sh::Parameters::default(),
+            controller: Controller {
+                parameters: Parameters::default(),
+                u1 : 0.0,
+                x1 : 0.0,
+                x2 : 0.0,
+                y1 : 0.0,
+            },
         }
     }
 }
@@ -76,16 +93,33 @@ impl PidState {
         };
     }
 
-    /// Update PID state on ADC input, calculate new DAC output
+    // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation
+    // Input x(t), target u(t), output y(t)
+    // y0' =   y1 - ki * u0   
+    //       + x0 * (kp + ki + kd)
+    //       - x1 * (kp + 2kd)
+    //       + x2 * kd
+    //       + kp * (u0 - u1)
+    // y0  = clip(y0', ymin, ymax)
     pub fn update_pid(&mut self) -> Option<f64> {
-        let input = (self.get_temperature()?.get::<degree_celsius>() as f32) - (self.set_point.get::<degree_celsius>() as f32);
+        let input = self.get_temperature()?.get::<degree_celsius>();
-        let pid_output = self.pid.update(&mut self.xy, input);
+        let setpoint = self.set_point.get::<degree_celsius>();
-        debug!("BiQuad Storage [x0, x1, y0, y1]: {:?}", self.xy);
+        let mut output: f64 = self.controller.y1 - setpoint * f64::from(self.controller.parameters.ki)
-        Some(pid_output as f64)
+                            + input * f64::from(self.controller.parameters.kp + self.controller.parameters.ki + self.controller.parameters.kd)
+                            - self.controller.x1 * f64::from(self.controller.parameters.kp + 2.0 * self.controller.parameters.kd)
+                            + self.controller.x2 * f64::from(self.controller.parameters.kd)
+                            + f64::from(self.controller.parameters.kp) * (setpoint - self.controller.u1);
+        if output < self.controller.parameters.output_min.into() {
+            output = self.controller.parameters.output_min.into();
         }
-
+        if output > self.controller.parameters.output_max.into() {
-    pub fn reset_pid_state(&mut self){
+            output = self.controller.parameters.output_max.into();
-        self.xy = [0.0; 4];
+        }
+        self.controller.x2 = self.controller.x1;
+        self.controller.x1 = input;
+        self.controller.u1 = setpoint;
+        self.controller.y1 = output;        
+        Some(output)
     }
 
     pub fn get_adc(&self) -> Option<ElectricPotential> {
@@ -110,33 +144,28 @@ impl PidState {
     pub fn set_pid_params(&mut self, param: PidSettings, val: f64){
         match param {
             PidSettings::Kp => { 
-                self.settings.gain(Action::Kp, val as f32);
+                self.controller.parameters.kp = val as f32;
             }
             PidSettings::Ki => { 
-                self.settings.gain(Action::Ki, val as f32);
+                self.controller.parameters.ki = val as f32;
             }
             PidSettings::Kd => { 
-                self.settings.gain(Action::Kd, val as f32);
+                self.controller.parameters.kd = val as f32;
             }
             PidSettings::Min => { 
-                self.pid_out_min = ElectricCurrent::new::<ampere>(val);
+                self.controller.parameters.output_min = val as f32;
             }
             PidSettings::Max => { 
-                self.pid_out_max = ElectricCurrent::new::<ampere>(val);
+                self.controller.parameters.output_max = val as f32;
             }
         }
-        self.update_pid_settings();
     }
 
-    pub fn set_pid_period(&mut self, period: f32){
+    pub fn reset_pid_state(&mut self){
-        self.settings.period(period);
+        self.controller.u1 = 0.0;
-        self.pid = self.settings.build().unwrap().into();
+        self.controller.x1 = 0.0;
-    }
+        self.controller.x2 = 0.0;
-
+        self.controller.y1 = 0.0;
-    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){
@@ -171,8 +200,8 @@ impl PidState {
         self.pid_engaged
     }
 
-    pub fn get_pid_settings(&mut self) -> Pid<f32> {
+    pub fn get_pid_settings(&mut self) -> Parameters {
-        unimplemented!()
+        self.controller.parameters
     }
 
     pub fn get_sh(&mut self) -> sh::Parameters {

src/thermostat/thermostat.rs

@@ -7,7 +7,6 @@ use crate::thermostat::ad7172;
 use crate::thermostat::pid_state::{PidState, PidSettings};
 use crate::thermostat::steinhart_hart;
 use serde::{Deserialize, Serialize};
-use idsp::iir::Pid;
 use log::debug;
 use uom::si::{
     electric_current::ampere,
@@ -19,6 +18,8 @@ use uom::si::{
 };
 use miniconf::Tree;
 
+use super::pid_state;
+
 pub const R_SENSE: ElectricalResistance = ElectricalResistance {
     dimension: PhantomData,
     units: PhantomData,
@@ -141,6 +142,7 @@ impl Thermostat{
     }
 
     pub fn poll_adc_and_update_pid(&mut self) -> bool {
+        let mut data_rdy = false;
         self.ad7172.data_ready().unwrap().map(|_ch|  {
             let data = self.ad7172.read_data().unwrap();
             let state: &mut PidState = &mut self.pid_ctrl_ch0;
@@ -151,17 +153,14 @@ impl Thermostat{
                     Some(pid_output) => {
                         self.set_i(ElectricCurrent::new::<ampere>(pid_output));
                         debug!("Temperature Set Point: {:?} degree", self.pid_ctrl_ch0.get_pid_setpoint().get::<degree_celsius>());
-                        self.power_up();
                     }
                     None => { }
                 }
-            } else {
-                self.power_down();
             }
             debug!("Temperature: {:?} degree", self.get_temperature().get::<degree_celsius>());
-            true
+            data_rdy = true;
         });
-        false
+        data_rdy
     }
 
     pub fn power_up(&mut self){
@@ -296,7 +295,7 @@ impl Thermostat{
         }
     }
 
-    pub fn get_pid_settings(&mut self) -> Pid<f32> {
+    pub fn get_pid_settings(&mut self) -> pid_state::Parameters {
         self.pid_ctrl_ch0.get_pid_settings()
     }