steinhart_hart: fix calculation, rename parameters

src/ad7172/mod.rs

@@ -1,4 +1,5 @@
 use core::fmt;
+use lexical_core::Float;
 use stm32f4xx_hal::{
     time::{MegaHertz, U32Ext},
     spi,
@@ -146,18 +147,9 @@ impl PostFilter {
     ];
 
     pub fn closest(rate: f32) -> Option<Self> {
-        /// (x - y).abs()
-        fn d(x: f32, y: f32) -> f32 {
-            if x >= y {
-                x - y
-            } else {
-                y - x
-            }
-        }
-
         let mut best: Option<(f32, Self)> = None;
         for value in Self::VALID_VALUES {
-            let error = d(rate, value.output_rate().unwrap());
+            let error = (rate - value.output_rate().unwrap()).abs();
             let better = best
                 .map(|(best_error, _)| error < best_error)
                 .unwrap_or(true);

src/command_parser.rs

@@ -123,12 +123,12 @@ pub enum Command {
     Pid {
         channel: usize,
         parameter: PidParameter,
-        value: f32,
+        value: f64,
     },
     SteinhartHart {
         channel: usize,
         parameter: ShParameter,
-        value: f32,
+        value: f64,
     },
     PostFilter {
         channel: usize,
@@ -158,12 +158,12 @@ fn unsigned(input: &[u8]) -> IResult<&[u8], Result<u16, Error>> {
         })
 }
 
-fn float(input: &[u8]) -> IResult<&[u8], Result<f32, Error>> {
+fn float(input: &[u8]) -> IResult<&[u8], Result<f64, Error>> {
     let (input, sign) = opt(is_a("-"))(input)?;
     let negative = sign.is_some();
     let (input, digits) = take_while1(|c| is_digit(c) || c == '.' as u8)(input)?;
     let result = lexical::parse(digits)
-        .map(|result: f32| if negative { -result } else { result })
+        .map(|result: f64| if negative { -result } else { result })
         .map_err(|e| e.into());
     Ok((input, result))
 }
@@ -357,7 +357,8 @@ fn postfilter(input: &[u8]) -> IResult<&[u8], Result<Command, Error>> {
                 let (input, rate) = float(input)?;
                 let result = rate
                     .map(|rate| Command::PostFilter {
-                        channel, rate,
+                        channel,
+                        rate: rate as f32,
                     });
                 Ok((input, result))
             }

src/main.rs

@@ -38,6 +38,8 @@ use session::{CHANNELS, Session, SessionOutput};
 mod command_parser;
 use command_parser::{Command, ShowCommand, PwmSetup, PwmMode};
 mod timer;
+mod pid;
+mod steinhart_hart;
 
 
 #[derive(Clone, Copy, Debug)]

src/pid.rs

@@ -0,0 +1,122 @@
+#[derive(Clone, Copy)]
+pub struct Parameters {
+    pub kp: f64,
+    pub ki: f64,
+    pub kd: f64,
+    pub output_min: f64,
+    pub output_max: f64,
+    pub integral_min: f64,
+    pub integral_max: f64
+}
+
+#[derive(Clone)]
+pub struct Controller {
+    parameters: Parameters,
+    target: f64,
+    integral: f64,
+    last_input: Option<f64>
+}
+
+impl Controller {
+    pub const fn new(parameters: Parameters) -> Controller {
+        Controller {
+            parameters: parameters,
+            target: 0.0,
+            last_input: None,
+            integral: 0.0
+        }
+    }
+
+    pub fn update(&mut self, input: f64) -> f64 {
+        let error = self.target - input;
+
+        let p = self.parameters.kp * error;
+
+        self.integral += error;
+        if self.integral < self.parameters.integral_min {
+            self.integral = self.parameters.integral_min;
+        }
+        if self.integral > self.parameters.integral_max {
+            self.integral = self.parameters.integral_max;
+        }
+        let i = self.parameters.ki * self.integral;
+
+        let d = match self.last_input {
+            None => 0.0,
+            Some(last_input) => self.parameters.kd * (last_input - input)
+        };
+        self.last_input = Some(input);
+
+        let mut output = p + i + d;
+        if output < self.parameters.output_min {
+            output = self.parameters.output_min;
+        }
+        if output > self.parameters.output_max {
+            output = self.parameters.output_max;
+        }
+        output
+    }
+
+    pub fn get_target(&self) -> f64 {
+        self.target
+    }
+
+    pub fn set_target(&mut self, target: f64) {
+        self.target = target;
+    }
+
+    pub fn get_parameters(&self) -> &Parameters {
+        &self.parameters
+    }
+
+    pub fn update_parameters<F: FnOnce(&mut Parameters)>(&mut self, f: F) {
+        f(&mut self.parameters);
+    }
+
+    #[allow(dead_code)]
+    pub fn reset(&mut self) {
+        self.integral = 0.0;
+        self.last_input = None;
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    const PARAMETERS: Parameters = Parameters {
+        kp: 0.055,
+        ki: 0.005,
+        kd: 0.04,
+        output_min: -10.0,
+        output_max: 10.0,
+        integral_min: -100.0,
+        integral_max: 100.0,
+    };
+
+    #[test]
+    fn test_controller() {
+        const DEFAULT: f64 = 0.0;
+        const TARGET: f64 = 1234.56;
+        const ERROR: f64 = 0.01;
+        const DELAY: usize = 10;
+
+        let mut pid = Controller::new(PARAMETERS.clone());
+        pid.set_target(TARGET);
+
+        let mut values = [DEFAULT; DELAY];
+        let mut t = 0;
+        let mut total_t = 0;
+        let target = (TARGET - ERROR)..=(TARGET + ERROR);
+        while !values.iter().all(|value| target.contains(value)) {
+            let next_t = (t + 1) % DELAY;
+            // Feed the oldest temperature
+            let output = pid.update(values[next_t]);
+            // Overwrite oldest with previous temperature + output
+            values[next_t] = values[t] + output;
+            t = next_t;
+            total_t += 1;
+        }
+        dbg!(values[t], total_t);
+    }
+}

src/server.rs

@@ -15,7 +15,7 @@ pub struct SocketState<S> {
 /// Number of server sockets and therefore concurrent client
 /// sessions. Many data structures in `Server::run()` correspond to
 /// this const.
-const SOCKET_COUNT: usize = 8;
+const SOCKET_COUNT: usize = 4;
 
 const TCP_RX_BUFFER_SIZE: usize = 2048;
 const TCP_TX_BUFFER_SIZE: usize = 2048;
@@ -52,10 +52,6 @@ impl<'a, 'b, S: Default> Server<'a, 'b, S> {
         create_socket!(sockets, tcp_rx_storage1, tcp_tx_storage1, states[1].handle);
         create_socket!(sockets, tcp_rx_storage2, tcp_tx_storage2, states[2].handle);
         create_socket!(sockets, tcp_rx_storage3, tcp_tx_storage3, states[3].handle);
-        create_socket!(sockets, tcp_rx_storage4, tcp_tx_storage4, states[4].handle);
-        create_socket!(sockets, tcp_rx_storage5, tcp_tx_storage5, states[5].handle);
-        create_socket!(sockets, tcp_rx_storage6, tcp_tx_storage6, states[6].handle);
-        create_socket!(sockets, tcp_rx_storage7, tcp_tx_storage7, states[7].handle);
 
         for state in &mut states {
             state.state = S::default();

src/session.rs

@@ -16,16 +16,13 @@ impl LineReader {
         }
     }
 
-    pub fn feed(&mut self, c: u8) -> Option<LineResult> {
+    pub fn feed(&mut self, c: u8) -> Option<&[u8]> {
         if c == 13 || c == 10 {
             // Enter
             if self.pos > 0 {
                 let len = self.pos;
                 self.pos = 0;
-                Some(LineResult {
-                    buf: self.buf.clone(),
-                    len,
-                })
+                Some(&self.buf[..len])
             } else {
                 None
             }
@@ -41,18 +38,6 @@ impl LineReader {
     }
 }
 
-pub struct LineResult {
-    buf: [u8; MAX_LINE_LEN],
-    len: usize,
-}
-
-impl Deref for LineResult {
-    type Target = [u8];
-    fn deref(&self) -> &Self::Target {
-        &self.buf[..self.len]
-    }
-}
-
 pub enum SessionOutput {
     Nothing,
     Command(Command),

src/steinhart_hart.rs

@@ -0,0 +1,41 @@
+use lexical_core::Float;
+
+/// Steinhart-Hart equation parameters
+#[derive(Clone, Debug)]
+pub struct Parameters {
+    pub t0: f64,
+    pub r: f64,
+    pub r0: f64,
+
+    r_fact: f64,
+}
+
+impl Parameters {
+    /// Update the cached r_fact
+    pub fn update(&mut self) {
+        self.r_fact = (self.r / self.r0).ln();
+    }
+
+    /// Perform the voltage to temperature conversion.
+    ///
+    /// Result unit: Kelvin
+    ///
+    /// TODO: verify
+    pub fn get_temperature(&self, b: f64) -> f64 {
+        let inv_temp = 1.0 / self.t0 + self.r_fact / b;
+        1.0 / inv_temp
+    }
+}
+
+impl Default for Parameters {
+    fn default() -> Self {
+        let mut p = Parameters {
+            t0: 0.001_4,
+            r: 0.000_000_099,
+            r0: 5_110.0,
+            r_fact: 0.0,
+        };
+        p.update();
+        p
+    }
+}