diff --git a/README.md b/README.md index 4fe4ae1..80917fa 100644 --- a/README.md +++ b/README.md @@ -110,8 +110,8 @@ formatted as line-delimited JSON. | `pid` | Show PID configuration | | `pid <0/1> target ` | Set the PID controller target temperature | | `pid <0/1> kp ` | Set proportional gain | -| `pid <0/1> ki ` | Set integral gain (unit: 10 Hz) | -| `pid <0/1> kd ` | Set differential gain (unit: 0.1 seconds) | +| `pid <0/1> ki ` | Set integral gain | +| `pid <0/1> kd ` | Set differential gain | | `pid <0/1> output_min ` | Set mininum output | | `pid <0/1> output_max ` | Set maximum output | | `pid <0/1> integral_min ` | Set integral lower bound | diff --git a/src/channel_state.rs b/src/channel_state.rs index c4d6cbc..faa7ae8 100644 --- a/src/channel_state.rs +++ b/src/channel_state.rs @@ -1,13 +1,15 @@ -use smoltcp::time::Instant; +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, }; use crate::{ ad7172, @@ -23,6 +25,7 @@ pub struct ChannelState { pub adc_data: Option, pub adc_calibration: ad7172::ChannelCalibration, pub adc_time: Instant, + pub adc_interval: Duration, /// VREF for the TEC (1.5V) pub vref: ElectricPotential, /// i_set 0A center point @@ -39,6 +42,8 @@ impl ChannelState { adc_data: None, adc_calibration, adc_time: Instant::from_secs(0), + // default: 10 Hz + adc_interval: Duration::from_millis(100), // updated later with Channels.read_vref() vref: ElectricPotential::new::(1.5), center: CenterPoint::Vref, @@ -56,6 +61,7 @@ impl ChannelState { } else { Some(adc_data) }; + self.adc_interval = now - self.adc_time; self.adc_time = now; } @@ -63,10 +69,18 @@ impl ChannelState { pub fn update_pid(&mut self) -> Option { let temperature = self.get_temperature()? .get::(); - let pid_output = self.pid.update(temperature); + let pid_output = self.pid.update(temperature, self.get_adc_interval()); Some(pid_output) } + pub fn get_adc_time(&self) -> Time { + Time::new::(self.adc_time.total_millis() as f64) + } + + pub fn get_adc_interval(&self) -> Time { + Time::new::(self.adc_interval.total_millis() as f64) + } + pub fn get_adc(&self) -> Option { Some(self.adc_calibration.convert_data(self.adc_data?)) } diff --git a/src/channels.rs b/src/channels.rs index c1d4584..4ff5984 100644 --- a/src/channels.rs +++ b/src/channels.rs @@ -3,7 +3,7 @@ use serde::{Serialize, Serializer}; use smoltcp::time::Instant; use stm32f4xx_hal::hal; use uom::si::{ - f64::{ElectricCurrent, ElectricPotential, ElectricalResistance}, + f64::{ElectricCurrent, ElectricPotential, ElectricalResistance, Time}, electric_potential::{millivolt, volt}, electric_current::ampere, electrical_resistance::ohm, @@ -425,7 +425,8 @@ impl Channels { ); Report { channel, - time: state.adc_time.total_millis(), + time: state.get_adc_time(), + interval: state.get_adc_interval(), adc: state.get_adc(), sens: state.get_sens(), temperature: state.get_temperature() @@ -510,7 +511,8 @@ type JsonBuffer = Vec; #[derive(Serialize)] pub struct Report { channel: usize, - time: i64, + time: Time, + interval: Time, adc: Option, sens: Option, temperature: Option, diff --git a/src/pid.rs b/src/pid.rs index 461e60a..5243f88 100644 --- a/src/pid.rs +++ b/src/pid.rs @@ -1,4 +1,8 @@ use serde::{Serialize, Deserialize}; +use uom::si::{ + f64::Time, + time::second, +}; #[derive(Clone, Debug, PartialEq, Serialize, Deserialize)] pub struct Parameters { @@ -45,7 +49,9 @@ impl Controller { } } - pub fn update(&mut self, input: f64) -> f64 { + pub fn update(&mut self, input: f64, time_delta: Time) -> f64 { + let time_delta = time_delta.get::(); + // error let error = input - self.target; @@ -53,7 +59,7 @@ impl Controller { let p = f64::from(self.parameters.kp) * error; // integral - self.integral += f64::from(self.parameters.ki) * error; + self.integral += f64::from(self.parameters.ki) * error * time_delta; if self.integral < self.parameters.integral_min.into() { self.integral = self.parameters.integral_min.into(); } @@ -64,8 +70,10 @@ impl Controller { // derivative let d = match self.last_input { - None => 0.0, - Some(last_input) => f64::from(self.parameters.kd) * (input - last_input), + None => + 0.0, + Some(last_input) => + f64::from(self.parameters.kd) * (input - last_input) / time_delta, }; self.last_input = Some(input);