Add impedance control mode

This mode regulates the input current such that the load to the
generator has a constant, defined impedance. The impedance is set
such that it matches the ratio between the specified motor voltage and
current. In that case, the power extracted from the motor/generator ist
maximized.

src/main.rs

@@ -300,8 +300,9 @@ fn main() -> ! {
 
             // Green LED is on in constant-voltage mode and off in MPPT mode.
             match switchctrl.get_control_mode() {
-                switch_control::ControlMode::ConstantVoltage => {pin_led_g.set_high().unwrap()},
+                switch_control::ControlMode::ConstantVoltage  => {pin_led_g.set_high().unwrap()},
-                switch_control::ControlMode::MPPT            => {pin_led_g.set_low().unwrap()},
+                switch_control::ControlMode::MPPT             => {pin_led_g.set_low().unwrap()},
+                switch_control::ControlMode::ImpedanceControl => {pin_led_g.set_low().unwrap()},
             }
 
             // do not output status data every loop

src/switch_control.rs

@@ -6,7 +6,8 @@ const PWR_AVG_EXPONENT: i64 = 12;
 #[derive(Copy, Clone)]
 pub enum ControlMode {
     MPPT,
-    ConstantVoltage
+    ConstantVoltage,
+    ImpedanceControl
 }
 
 struct PID<const GAINSCALE: i32, const T0: i32>
@@ -217,12 +218,68 @@ impl<const INTERVAL: u32, const PERCENT_LOSS_TOLERANCE: i32> MPPT<INTERVAL, PERC
     }
 }
 
+struct ImpedanceControl<const T0: i32, const MOTOR_SPEC_U: i32, const MOTOR_SPEC_I: i32>
+{
+    pid: PID::<10000, T0>,
+    iin_target: i32,
+}
+
+impl<const T0: i32, const MOTOR_SPEC_U: i32, const MOTOR_SPEC_I: i32> ImpedanceControl<T0, MOTOR_SPEC_U, MOTOR_SPEC_I>
+{
+    pub fn new(i_target_initial: i32, max_pwm: i32) -> Self
+    {
+        ImpedanceControl::<T0, MOTOR_SPEC_U, MOTOR_SPEC_I> {
+            pid: PID::<10000, T0>::new(10, 5, 2, i_target_initial, 0, 0, max_pwm * 10000),
+            iin_target: i_target_initial
+        }
+    }
+
+    pub fn restart(&mut self, start_pwm: i32)
+    {
+        self.pid.restart_from_output_value(start_pwm);
+    }
+
+    pub fn update(&mut self, vin: i32, vout: i32, iout: i32) -> i32
+    {
+        let pout = vout * iout;
+        let iin;
+
+        if vin <= 0 {
+            iin = 0;
+        } else {
+            iin = (pout / vin) * 85 / 100;
+        }
+
+        self.iin_target = vin * MOTOR_SPEC_I / MOTOR_SPEC_U;
+
+        self.pid.set_target(self.iin_target);
+
+        self.pid.update(iin)
+    }
+
+    pub fn log_status(&self, logger: &mut dyn Logger)
+    {
+        let mut data: String<16>;
+
+        logger.log(b"I_target: ").unwrap();
+
+        data = String::from(self.iin_target);
+        logger.log(data.as_bytes()).unwrap();
+
+        logger.log(b"mA - ").unwrap();
+
+        self.pid.log_status(logger);
+    }
+}
+
+
 pub struct SwitchControl<const T0: i32>
 {
     mode: ControlMode,
     mode_switch_delay_counter: u32,
     cv_pid: PID<10000, T0>,
     mppt: MPPT<200, 0 /* percent loss of peak power */>,
+    imp_ctrl: ImpedanceControl<T0, 11000, 15000>,
     pwm_max: i32,
     pwm_cur: i32,
     cv_target: i32,
@@ -234,10 +291,11 @@ impl<const T0: i32> SwitchControl<T0>
     pub fn new(cv_target: i32, cv_max_deviation: i32, pwm_max: i32) -> Self
     {
         SwitchControl::<T0> {
-            mode: ControlMode::MPPT,
+            mode: ControlMode::ImpedanceControl,
             mode_switch_delay_counter: 0,
             cv_pid: PID::<10000, T0>::new(100, 50, 50, cv_target, 0, 0, pwm_max * 10000),
             mppt: MPPT::<200, 0>::new(3*pwm_max/4, 0, pwm_max),
+            imp_ctrl: ImpedanceControl::<T0, 11000, 15000>::new(1000, pwm_max),
             pwm_max,
             pwm_cur: 0,
             cv_target,
@@ -246,7 +304,7 @@ impl<const T0: i32> SwitchControl<T0>
     }
 
     // returns the new PWM value
-    pub fn update(&mut self, _vin: i32, vout: i32, iout: i32) -> i32
+    pub fn update(&mut self, vin: i32, vout: i32, iout: i32) -> i32
     {
         // Mode switching (only if delay counter has expired)
         if self.mode_switch_delay_counter == 0 {
@@ -260,14 +318,26 @@ impl<const T0: i32> SwitchControl<T0>
                         self.mode_switch_delay_counter = 100;
                     },
 
+                ControlMode::ImpedanceControl =>
+                    if vout > self.cv_target {
+                        self.mode = ControlMode::ConstantVoltage;
+                        self.cv_pid.restart_from_output_value(self.pwm_cur * 80 / 100);
+
+                        // stay in CV mode for at least 100 update cycles
+                        self.mode_switch_delay_counter = 100;
+                    },
+
                 ControlMode::ConstantVoltage =>
                     if vout < self.cv_target - self.cv_max_deviation {
-                        self.mode = ControlMode::MPPT;
+                        self.mode = ControlMode::ImpedanceControl;
 
                         // begin MPPT tracking by searching downwards, because the CV PID controller
                         // probably has pushed the PWM to the upper limit due to too low output
                         // voltage.
-                        self.mppt.restart(self.pwm_max * 80 / 100, -1);
+                        //self.mppt.restart(self.pwm_max * 80 / 100, -1);
+
+                        // back to impedance control
+                        self.imp_ctrl.restart(self.pwm_max * 80 / 100);
                     }
             }
         } else {
@@ -278,6 +348,7 @@ impl<const T0: i32> SwitchControl<T0>
         let pwm = match self.mode {
             ControlMode::ConstantVoltage => self.cv_pid.update(vout),
             ControlMode::MPPT => self.mppt.update(vout, iout),
+            ControlMode::ImpedanceControl => self.imp_ctrl.update(vin, vout, iout),
         };
 
         // store and limit pwm value
@@ -303,6 +374,11 @@ impl<const T0: i32> SwitchControl<T0>
                 logger.log(b"[CV] ").unwrap();
                 self.cv_pid.log_status(logger);
             }
+
+            ControlMode::ImpedanceControl => {
+                logger.log(b"[Imp] ").unwrap();
+                self.imp_ctrl.log_status(logger);
+            }
         }
 
         logger.log(b"\r\n").unwrap();