update eth_cmd_test code

- Port from thermostat repo

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"

eth_cmd_test.py

@@ -1,7 +1,6 @@
 # Python Test Scripts for Controlling Kirdy
 #   Kirdy is written to be controlled via a json object based on miniconf rust crate
 #   Json Field:
-#       "rev": hw_rev
 #       "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
@@ -9,103 +8,112 @@
 import socket
 import json
 import time
+import signal
 
 # Kirdy IP and Port Number
 HOST = "192.168.1.132"
 PORT = 1337
 
 ld_cmd = {
-    "rev": 3,
     "laser_diode_cmd": "SetI",
     "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,
+    "data_f64": 25.0,
 }
 
 tec_set_pid_kp_cmd = {
-    "rev": 3,
     "thermostat_cmd": "SetPidKp",
-    "data_f64": 1.0,
+    "data_f64": 0.10889684439011593
 }
 
 tec_set_pid_ki_cmd = {
-    "rev": 3,
     "thermostat_cmd": "SetPidKi",
-    "data_f64": 0.01,
+    "data_f64": 0.0038377132059211646
 }
 
 tec_set_pid_kd_cmd = {
-    "rev": 3,
     "thermostat_cmd": "SetPidKd",
-    "data_f64": 0.0,
+    "data_f64": 0.22294449514406328
 }
 
 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
+tec_pid_engage = {
+    "thermostat_cmd": "SetPidEngage",
+}
+
+tec_get_tec_status = {
+    "thermostat_cmd": "GetTecStatus",
+}
+
+def send_cmd(input, socket):
+    socket.send(bytes(json.dumps(input), "UTF-8"))
+    # Give some time for Kirdy to process the cmd
+    time.sleep(0.5)
+
+def read_cmd(input, socket):
+    socket.send(bytes(json.dumps(input), "UTF-8"))
+    data = socket.recv(1024).decode('utf8')
+    return json.loads(data)
+
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+def signal_handler(sig, frame):
+    send_cmd(tec_power_down, s)
+    s.close()
+    exit()
+
+signal.signal(signal.SIGINT, signal_handler)
+
 with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
     s.connect((HOST, PORT))
-    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"))
+    send_cmd(ld_cmd, s)
+    send_cmd(tec_power_down, s)
+    send_cmd(tec_set_sh_t0_cmd, s)
+    send_cmd(tec_set_sh_r0_cmd, s)
+    send_cmd(tec_set_sh_beta_cmd, s)
+    send_cmd(tec_set_temperature_setpoint_cmd, s)
+    send_cmd(tec_set_pid_kp_cmd, s)
+    send_cmd(tec_set_pid_ki_cmd, s)
+    send_cmd(tec_set_pid_kd_cmd, s)
+    send_cmd(tec_pid_engage, s)
+    send_cmd(tec_power_up, s)
 
+    while True:
+        tec_status = read_cmd(tec_get_tec_status, s)
+        print(f"Ts: {tec_status['ts']} | Temperature: {tec_status['temperature'] - 273.15}")
+    s.close()

pid_autotune.py

@@ -0,0 +1,306 @@
+import math
+import logging
+from collections import deque, namedtuple
+from enum import Enum
+import socket
+import json
+import time
+import signal
+
+# Based on hirshmann pid-autotune libiary
+# See https://github.com/hirschmann/pid-autotune
+# Which is in turn based on a fork of Arduino PID AutoTune Library
+# See https://github.com/t0mpr1c3/Arduino-PID-AutoTune-Library
+
+
+class PIDAutotuneState(Enum):
+    STATE_OFF = 'off'
+    STATE_RELAY_STEP_UP = 'relay step up'
+    STATE_RELAY_STEP_DOWN = 'relay step down'
+    STATE_SUCCEEDED = 'succeeded'
+    STATE_FAILED = 'failed'
+
+
+class PIDAutotune:
+    PIDParams = namedtuple('PIDParams', ['Kp', 'Ki', 'Kd'])
+
+    PEAK_AMPLITUDE_TOLERANCE = 0.05
+
+    _tuning_rules = {
+        "ziegler-nichols": [0.6, 1.2, 0.075],
+        "tyreus-luyben": [0.4545,  0.2066, 0.07214],
+        "ciancone-marlin": [0.303, 0.1364, 0.0481],
+        "pessen-integral": [0.7, 1.75, 0.105],
+        "some-overshoot": [0.333, 0.667,  0.111],
+        "no-overshoot": [0.2, 0.4,  0.0667]
+    }
+
+    def __init__(self, setpoint, out_step=10, lookback=60,
+                 noiseband=0.5, sampletime=1.2):
+        if setpoint is None:
+            raise ValueError('setpoint must be specified')
+
+        self._inputs = deque(maxlen=round(lookback / sampletime))
+        self._setpoint = setpoint
+        self._outputstep = out_step
+        self._noiseband = noiseband
+        self._out_min = -out_step
+        self._out_max = out_step
+        self._state = PIDAutotuneState.STATE_OFF
+        self._peak_timestamps = deque(maxlen=5)
+        self._peaks = deque(maxlen=5)
+        self._output = 0
+        self._last_run_timestamp = 0
+        self._peak_type = 0
+        self._peak_count = 0
+        self._initial_output = 0
+        self._induced_amplitude = 0
+        self._Ku = 0
+        self._Pu = 0
+
+    def state(self):
+        """Get the current state."""
+        return self._state
+
+    def output(self):
+        """Get the last output value."""
+        return self._output
+
+    def tuning_rules(self):
+        """Get a list of all available tuning rules."""
+        return self._tuning_rules.keys()
+
+    def get_pid_parameters(self, tuning_rule='ziegler-nichols'):
+        """Get PID parameters.
+
+        Args:
+            tuning_rule (str): Sets the rule which should be used to calculate
+                the parameters.
+        """
+        divisors = self._tuning_rules[tuning_rule]
+        kp = self._Ku * divisors[0]
+        ki = divisors[1] * self._Ku / self._Pu
+        kd = divisors[2] * self._Ku * self._Pu
+        return PIDAutotune.PIDParams(kp, ki, kd)
+
+    def run(self, input_val, time_input):
+        """To autotune a system, this method must be called periodically.
+
+        Args:
+            input_val (float): The temperature input value.
+            time_input (float): Current time in seconds.
+
+        Returns:
+            `true` if tuning is finished, otherwise `false`.
+        """
+        now = time_input * 1000
+
+        if (self._state == PIDAutotuneState.STATE_OFF
+                or self._state == PIDAutotuneState.STATE_SUCCEEDED
+                or self._state == PIDAutotuneState.STATE_FAILED):
+            self._state = PIDAutotuneState.STATE_RELAY_STEP_UP
+
+        self._last_run_timestamp = now
+
+        # check input and change relay state if necessary
+        if (self._state == PIDAutotuneState.STATE_RELAY_STEP_UP
+                and input_val > self._setpoint + self._noiseband):
+            self._state = PIDAutotuneState.STATE_RELAY_STEP_DOWN
+            logging.debug('switched state: {0}'.format(self._state))
+            logging.debug('input: {0}'.format(input_val))
+        elif (self._state == PIDAutotuneState.STATE_RELAY_STEP_DOWN
+                and input_val < self._setpoint - self._noiseband):
+            self._state = PIDAutotuneState.STATE_RELAY_STEP_UP
+            logging.debug('switched state: {0}'.format(self._state))
+            logging.debug('input: {0}'.format(input_val))
+
+        # set output
+        if (self._state == PIDAutotuneState.STATE_RELAY_STEP_UP):
+            self._output = self._initial_output - self._outputstep
+        elif self._state == PIDAutotuneState.STATE_RELAY_STEP_DOWN:
+            self._output = self._initial_output + self._outputstep
+
+        # respect output limits
+        self._output = min(self._output, self._out_max)
+        self._output = max(self._output, self._out_min)
+
+        # identify peaks
+        is_max = True
+        is_min = True
+
+        for val in self._inputs:
+            is_max = is_max and (input_val >= val)
+            is_min = is_min and (input_val <= val)
+
+        self._inputs.append(input_val)
+
+        # we don't trust the maxes or mins until the input array is full
+        if len(self._inputs) < self._inputs.maxlen:
+            return False
+
+        # increment peak count and record peak time for maxima and minima
+        inflection = False
+
+        # peak types:
+        # -1: minimum
+        # +1: maximum
+        if is_max:
+            if self._peak_type == -1:
+                inflection = True
+            self._peak_type = 1
+        elif is_min:
+            if self._peak_type == 1:
+                inflection = True
+            self._peak_type = -1
+
+        # update peak times and values
+        if inflection:
+            self._peak_count += 1
+            self._peaks.append(input_val)
+            self._peak_timestamps.append(now)
+            logging.debug('found peak: {0}'.format(input_val))
+            logging.debug('peak count: {0}'.format(self._peak_count))
+
+        # check for convergence of induced oscillation
+        # convergence of amplitude assessed on last 4 peaks (1.5 cycles)
+        self._induced_amplitude = 0
+
+        if inflection and (self._peak_count > 4):
+            abs_max = self._peaks[-2]
+            abs_min = self._peaks[-2]
+            for i in range(0, len(self._peaks) - 2):
+                self._induced_amplitude += abs(self._peaks[i]
+                                               - self._peaks[i+1])
+                abs_max = max(self._peaks[i], abs_max)
+                abs_min = min(self._peaks[i], abs_min)
+
+            self._induced_amplitude /= 6.0
+
+            # check convergence criterion for amplitude of induced oscillation
+            amplitude_dev = ((0.5 * (abs_max - abs_min)
+                              - self._induced_amplitude)
+                             / self._induced_amplitude)
+
+            logging.debug('amplitude: {0}'.format(self._induced_amplitude))
+            logging.debug('amplitude deviation: {0}'.format(amplitude_dev))
+
+            if amplitude_dev < PIDAutotune.PEAK_AMPLITUDE_TOLERANCE:
+                self._state = PIDAutotuneState.STATE_SUCCEEDED
+
+        # if the autotune has not already converged
+        # terminate after 10 cycles
+        if self._peak_count >= 20:
+            self._output = 0
+            self._state = PIDAutotuneState.STATE_FAILED
+            return True
+
+        if self._state == PIDAutotuneState.STATE_SUCCEEDED:
+            self._output = 0
+            logging.debug('peak finding successful')
+
+            # calculate ultimate gain
+            self._Ku = 4.0 * self._outputstep / \
+                (self._induced_amplitude * math.pi)
+            print('Ku: {0}'.format(self._Ku))
+
+            # calculate ultimate period in seconds
+            period1 = self._peak_timestamps[3] - self._peak_timestamps[1]
+            period2 = self._peak_timestamps[4] - self._peak_timestamps[2]
+            self._Pu = 0.5 * (period1 + period2) / 1000.0
+            print('Pu: {0}'.format(self._Pu))
+
+            for rule in self._tuning_rules:
+                params = self.get_pid_parameters(rule)
+                print('rule: {0}'.format(rule))
+                print('Kp: {0}'.format(params.Kp))
+                print('Ki: {0}'.format(params.Ki))
+                print('Kd: {0}'.format(params.Kd))
+
+            return True
+        return False
+
+tec_power_up = {
+    "thermostat_cmd": "PowerUp",
+}
+
+tec_power_down = {
+    "thermostat_cmd": "PowerDown",
+}
+
+tec_get_tec_status = {
+    "thermostat_cmd": "GetTecStatus",
+}
+
+tec_pid_dis_engage = {
+    "thermostat_cmd": "SetPidDisEngage",
+}
+
+tec_set_i_out = {
+    "thermostat_cmd": "SetTecIOut",
+    "data_f64": 0.0,
+}
+
+# Kirdy IP and Port Number
+HOST = "192.168.1.132"
+PORT = 1337
+SAMPLING_RATE = 16.67
+
+def send_cmd(input, socket):
+    socket.send(bytes(json.dumps(input), "UTF-8"))
+    time.sleep(0.5)
+
+def read_cmd(input, socket):
+    socket.send(bytes(json.dumps(input), "UTF-8"))
+    data = socket.recv(1024).decode('utf8')
+    return json.loads(data)
+
+def main():
+    # Target temperature of the autotune routine, celsius
+    target_temperature = 20
+    # Value by which output will be increased/decreased from zero, amps
+    output_step = 1
+    # Reference period for local minima/maxima, seconds
+    lookback = 1
+    # Determines by how much the input value must
+    # overshoot/undershoot the setpoint, celsius
+    noiseband = 1.5
+
+    tuner = PIDAutotune(target_temperature, output_step,
+                        lookback, noiseband, 1/SAMPLING_RATE)
+
+    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+    def signal_handler(sig, frame):
+        send_cmd(tec_power_down, s)
+        s.close()
+        exit()
+
+    signal.signal(signal.SIGINT, signal_handler)
+
+    s.connect((HOST, PORT))
+
+    send_cmd(tec_pid_dis_engage, s)
+    send_cmd(tec_power_down, s)
+    send_cmd(tec_power_up, s)
+
+    while True:
+        tec_status = read_cmd(tec_get_tec_status, s)
+        
+        temperature = tec_status["temperature"] - 273.15
+        ts = tec_status['ts']
+        
+        if (tuner.run(temperature, ts / 1000.0)):
+            break
+
+        tuner_out = tuner.output()
+
+        tec_set_i_out["data_f64"] = float(tuner_out * 1000.0)
+
+        send_cmd(tec_set_i_out, s)
+    
+    tec_set_i_out["data_f64"] = 0.0
+    send_cmd(tec_power_down, s)
+    s.close()
+
+if __name__ == "__main__":
+    main()

src/device/hw_rev.rs

@@ -29,7 +29,7 @@ impl HWRev {
     ///     See Issue #32 on Kirdy Hw Repo
     pub fn startup_delay_before_gpio_init(&mut self){
         if self.major == 0 && self.minor == 3 {
-            sleep(1500);
+            sleep(5000);
         }
     }
 }

src/laser_diode/laser_diode.rs

@@ -1,7 +1,7 @@
 use miniconf::Tree;
 use stm32f4xx_hal::pac::ADC2;
 use uom::si::electric_current::ampere;
-use crate::laser_diode::ld_ctrl::LdCtrl;
+use crate::laser_diode::ld_ctrl::{LdCtrl, Impedance};
 use crate::laser_diode::ld_pwr_exc_protector::{LdPwrExcProtector, self};
 use crate::laser_diode::pd_responsitivity;
 use core::marker::PhantomData;
@@ -175,7 +175,7 @@ impl LdDrive{
         LdPwrExcProtector::set_trigger_threshold_v(i / Settings::PD_MON_TRANSCONDUCTANCE);
     }
 
-    pub fn get_term_status(&mut self)->bool{
-        self.ctrl.get_term_status()
+    pub fn get_term_status(&mut self) -> Impedance {
+        self.ctrl.get_lf_mod_in_impedance()
     }
 }

src/laser_diode/ld_ctrl.rs

@@ -13,6 +13,12 @@ use uom::si::{
 use crate::laser_diode::max5719::{self, Dac};
 use crate::laser_diode::laser_diode::TransimpedanceUnit;
 
+#[derive(Debug)]
+pub enum Impedance {
+    Is50Ohm,
+    Not50Ohm,
+}
+
 pub trait ChannelPins {
     type CurrentSourceShort: OutputPin;
     type TerminationStatus: InputPin;
@@ -61,8 +67,13 @@ impl LdCtrl {
         self.phy.current_source_short_pin.set_high();
     }
 
-    pub fn get_term_status(&mut self)-> bool{
-        self.phy.termination_status_pin.is_high()
+    pub fn get_lf_mod_in_impedance(&mut self)-> Impedance {
+        if self.phy.termination_status_pin.is_high() {
+            Impedance::Is50Ohm
+        }
+        else {
+            Impedance::Not50Ohm
+        }
     }
 
     pub fn set_dac(&mut self, voltage: ElectricPotential, dac_out_v_max: ElectricPotential) -> ElectricPotential {

src/main.rs

@@ -36,8 +36,6 @@ static mut ETH_DATA_BUFFER: [u8; 1024] = [0; 1024];
 #[cfg(not(test))]
 #[entry]
 fn main() -> ! {
-
-
     log_setup::init_log();
     info!("Kirdy init");
 
@@ -76,8 +74,8 @@ fn main() -> ! {
     // https://github.com/iliekturtles/uom/blob/master/examples/si.rs
     let volt_fmt = ElectricPotential::format_args(volt, Abbreviation);
     let amp_fmt = ElectricCurrent::format_args(ampere, Abbreviation);
-    let mili_amp_fmt = ElectricCurrent::format_args(milliampere, Abbreviation);
-    let mili_watt_fmt = Power::format_args(milliwatt, Abbreviation);
+    let milli_amp_fmt = ElectricCurrent::format_args(milliampere, Abbreviation);
+    let milli_watt_fmt = Power::format_args(milliwatt, Abbreviation);
 
     loop {
         wd.feed();
@@ -90,7 +88,7 @@ fn main() -> ! {
             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!("curr_ld_drive_cuurent: {:?}", milli_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);

src/net/cmd_handler.rs

@@ -7,7 +7,7 @@ use uom::si::{
     electrical_resistance::{ElectricalResistance, ohm}, 
     f64::ThermodynamicTemperature, thermodynamic_temperature::degree_celsius
 };
-use crate::laser_diode::laser_diode::LdDrive;
+use crate::{laser_diode::laser_diode::LdDrive, net::net, thermostat::thermostat::StatusReport};
 use crate::thermostat::thermostat::Thermostat;
 use crate::thermostat::pid_state::PidSettings::*;
 use log::info;
@@ -48,6 +48,8 @@ enum ThermostatCmdEnum {
     SetTecIOut, // Constant Current Mode
     SetTemperatureSetpoint,
     // PID
+    SetPidEngage,
+    SetPidDisEngage,
     SetPidKp,
     SetPidKi,
     SetPidKd,
@@ -66,7 +68,6 @@ enum ThermostatCmdEnum {
 
 #[derive(Deserialize, Serialize, Copy, Clone, Debug, Default, Tree)]
 pub struct CmdJsonObj{
-    rev: u8,
     laser_diode_cmd: Option<LdCmdEnum>,
     thermostat_cmd: Option<ThermostatCmdEnum>,
     data_f32: Option<f32>,
@@ -77,6 +78,11 @@ pub struct Cmd {
     json: CmdJsonObj
 }
 
+#[derive(Deserialize, Serialize, Copy, Clone, Debug, Default, Tree)]
+pub struct StatusReportStruct {
+    json: StatusReport
+}
+
 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()
@@ -158,10 +164,10 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive, mu
 
             match cmd.json.thermostat_cmd {
                 Some(ThermostatCmdEnum::PowerUp) => {
-                    tec.set_pid_engaged(true);
+                    tec.power_up()
                 }
                 Some(ThermostatCmdEnum::PowerDown) => {
-                    tec.set_pid_engaged(false);
+                    tec.power_down()
                 }
                 Some(ThermostatCmdEnum::SetTecMaxV) => {
                     match cmd.json.data_f64 {
@@ -213,7 +219,12 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive, mu
                         }
                     }
                 }
-
+                Some(ThermostatCmdEnum::SetPidEngage) => {
+                    tec.set_pid_engaged(true);
+                }
+                Some(ThermostatCmdEnum::SetPidDisEngage) => {
+                    tec.set_pid_engaged(false);
+                }
                 Some(ThermostatCmdEnum::SetPidKp) => {
                     match cmd.json.data_f64 {
                         Some(val) => {
@@ -298,7 +309,11 @@ pub fn execute_cmd(buffer: &mut [u8], buffer_size: usize, mut laser: LdDrive, mu
                     }
                 }
                 Some(ThermostatCmdEnum::GetTecStatus) => {
-                    info!("Not supported Yet")
+                    let status_report = StatusReportStruct {
+                        json: tec.get_status_report()
+                    };
+                    let num_bytes = status_report.get_json("/json", buffer).unwrap();
+                    net::eth_send(buffer, num_bytes);
                 }
                 Some(ThermostatCmdEnum::GetPidStatus) => {
                     info!("Not supported Yet")

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

src/thermostat/thermostat.rs

@@ -6,7 +6,7 @@ use crate::thermostat::max1968::{MAX1968, AdcReadTarget, PwmPinsEnum};
 use crate::thermostat::ad7172;
 use crate::thermostat::pid_state::{PidState, PidSettings};
 use crate::thermostat::steinhart_hart;
-use idsp::iir::Pid;
+use serde::{Deserialize, Serialize};
 use log::debug;
 use uom::si::{
     electric_current::ampere,
@@ -18,6 +18,8 @@ use uom::si::{
 };
 use miniconf::Tree;
 
+use super::pid_state;
+
 pub const R_SENSE: ElectricalResistance = ElectricalResistance {
     dimension: PhantomData,
     units: PhantomData,
@@ -140,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;
@@ -150,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){
@@ -276,6 +276,7 @@ impl Thermostat{
 
     pub fn get_status_report(&mut self) -> StatusReport {
         StatusReport {
+            ts: sys_timer::now(),
             pid_engaged: self.get_pid_engaged(),
             temperature: self.pid_ctrl_ch0.get_temperature(),
             i_set: self.tec_setting.i_set,
@@ -294,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()
     }
 
@@ -337,8 +338,9 @@ impl Thermostat{
     }
 }
 
-#[derive(Tree, Debug)]
+#[derive(Deserialize, Serialize, Copy, Clone, Debug, Default, Tree)]
 pub struct StatusReport {
+    ts: u32,
     pid_engaged: bool,
     temperature: Option<ThermodynamicTemperature>,
     i_set: ElectricCurrent,