Moved switch controller to separate module

src/main.rs

@@ -42,6 +42,7 @@ use critical_section::Mutex;
 
 mod ext_adc;
 mod logger;
+mod switch_control;
 
 use logger::Logger;
 
@@ -67,11 +68,11 @@ const XTAL_FREQ_HZ: u32 = 12_000_000u32;
 const SWITCH_PWM_MAX: i32 = 512;
 const SWITCH_PWM_LIMIT: i32 = 93 * SWITCH_PWM_MAX / 100;
 
-fn convert_adc_measurements(raw: &[u16; 4]) -> (u32, u32, u32)
+fn convert_adc_measurements(raw: &[u16; 4]) -> (i32, i32, i32)
 {
-    let iout = (raw[0] - raw[1]) as u32 * 3300 * 20 / 4096 / 28; // *20 = division by 50 mΩ shunt
-    let vout = raw[2] as u32 * 3300 * (47 + 10) / 10 / 4096;
-    let vin = raw[3] as u32 * 3300 * (10 + 10) / 10 / 4096;
+    let iout = (raw[0] - raw[1]) as i32 * 3300 * 20 / 4096 / 28; // *20 = division by 50 mΩ shunt
+    let vout = raw[2] as i32 * 3300 * (47 + 10) / 10 / 4096;
+    let vin = raw[3] as i32 * 3300 * (10 + 10) / 10 / 4096;
 
     (vin, vout, iout) /* units: mV, mV, mA */
 }
@@ -215,17 +216,7 @@ fn main() -> ! {
         pac::NVIC::unmask(pac::Interrupt::TIMER_IRQ_0);
     }
 
-    let vtarget: i32 =  8000;
-
-    const GAINSCALE: i32 = 10000;
-    let pgain: i32 =   50;
-    let igain: i32 =  100;
-    let dgain: i32 =    0;
-
-    let mut iaccu: i32 = 0;
-    let mut elast: i32 = 0;
-
-    const T: i32 = 1;
+    let mut switchctrl = switch_control::SwitchControl::<1>::new(8000, SWITCH_PWM_LIMIT);
 
     let mut loopcnt: u64 = 0;
 
@@ -280,35 +271,8 @@ fn main() -> ! {
 
             let (vin, vout, iout) = convert_adc_measurements(&adc_value);
 
-            // PID controller
-            let err: i32 = -(vout as i32 - vtarget);
-
-            iaccu += igain * T * err;
-
-            // limit iaccu
-            if iaccu > SWITCH_PWM_LIMIT * GAINSCALE {
-                iaccu = SWITCH_PWM_LIMIT * GAINSCALE;
-            } else if iaccu < 0 {
-                iaccu = 0;
-            }
-
-            let pval = pgain * err / GAINSCALE;
-            let ival = iaccu / GAINSCALE;
-            let dval = dgain * (err - elast) / GAINSCALE;
-
-            elast = err;
-
-            let ctrlout = pval + ival + dval;
-
-            // limit the control value
-            let pwmval;
-            if ctrlout > SWITCH_PWM_LIMIT {
-                pwmval = SWITCH_PWM_LIMIT;
-            } else if ctrlout < 0 {
-                pwmval = 0;
-            } else {
-                pwmval = ctrlout;
-            }
+            // Update the PWM value for the switch
+            let pwmval = switchctrl.update(vin, vout, iout);
 
             pwr_switch_ch.set_duty(pwmval as u16);
 
@@ -337,20 +301,8 @@ fn main() -> ! {
                     }
                 }
 
-                logger.log(b"]; e=").unwrap();
-                data = String::from(err);
-                logger.log(data.as_bytes()).unwrap();
-                logger.log(b" => ").unwrap();
-                data = String::from(pval);
-                logger.log(data.as_bytes()).unwrap();
-                logger.log(b"+").unwrap();
-                data = String::from(ival);
-                logger.log(data.as_bytes()).unwrap();
-                logger.log(b"+").unwrap();
-                data = String::from(dval);
-                logger.log(data.as_bytes()).unwrap();
-                logger.log(b" = ").unwrap();
-                data = String::from(ctrlout);
+                logger.log(b"]; PWM: ").unwrap();
+                data = String::from(pwmval);
                 logger.log(data.as_bytes()).unwrap();
 
                 logger.log(b"\r\n").unwrap();

src/switch_control.rs

@@ -0,0 +1,91 @@
+
+struct PID<const GAINSCALE: i32, const T0: i32>
+{
+    pgain: i32, // proportional gain
+    igain: i32, // integral gain
+    dgain: i32, // differential gain
+
+    iaccu: i32, // accumulator for the integral part
+    elast: i32, // last seen deviation value
+
+    target: i32,
+
+    iaccu_min: i32, // minimum allowed value in iaccu
+    iaccu_max: i32, // maximum allowed value in iaccu
+}
+
+impl<const GAINSCALE: i32, const T0: i32> PID<GAINSCALE, T0>
+{
+    pub fn new(pgain: i32, igain: i32, dgain: i32, target: i32, start_iaccu: i32, iaccu_min: i32,
+               iaccu_max: i32) -> Self
+    {
+        PID::<GAINSCALE, T0> {
+            pgain,
+            igain,
+            dgain,
+            iaccu: start_iaccu,
+            elast: 0,
+            target,
+            iaccu_min,
+            iaccu_max
+        }
+    }
+
+    pub fn update(&mut self, meas: i32) -> i32
+    {
+        let err = self.target - meas;
+
+        self.iaccu += self.igain * T0 * err;
+
+        // limit iaccu
+        if self.iaccu > self.iaccu_max {
+            self.iaccu = self.iaccu_max;
+        } else if self.iaccu < self.iaccu_min {
+            self.iaccu = self.iaccu_min;
+        }
+
+        let pval = self.pgain * err / GAINSCALE;
+        let ival = self.iaccu / GAINSCALE;
+        let dval = self.dgain * (err - self.elast) / GAINSCALE;
+
+        self.elast = err;
+
+        pval + ival + dval
+    }
+
+    pub fn set_target(&mut self, new_target: i32)
+    {
+        self.target = new_target;
+    }
+}
+
+pub struct SwitchControl<const T0: i32>
+{
+    cv_pid: PID<10000, T0>,
+    pwm_max: i32
+}
+
+impl<const T0: i32> SwitchControl<T0>
+{
+    pub fn new(cv_target: i32, pwm_max: i32) -> Self
+    {
+        SwitchControl::<T0> {
+            cv_pid: PID::<10000, T0>::new(100, 50, 0, cv_target, 0, 0, pwm_max * 10000),
+            pwm_max
+        }
+    }
+
+    // returns the new PWM value
+    pub fn update(&mut self, vin: i32, vout: i32, iout: i32) -> i32
+    {
+        let pwm = self.cv_pid.update(vout);
+
+        if pwm > self.pwm_max {
+            self.pwm_max
+        } else if pwm < 0 {
+            0
+        } else {
+            pwm
+        }
+    }
+}