diff --git a/src/logger.rs b/src/logger.rs
index e8e7140..c1a9fc5 100644
--- a/src/logger.rs
+++ b/src/logger.rs
@@ -9,7 +9,12 @@ pub enum LoggerError {
     WriteError
 }
 
-pub struct Logger<D, P, const QLEN: usize>
+pub trait Logger
+{
+    fn log(&mut self, msg: &[u8]) -> Result<(), LoggerError>;
+}
+
+pub struct UartLogger<D, P, const QLEN: usize>
 where
     D: UartDevice,
     P: ValidUartPinout<D>
@@ -18,22 +23,13 @@ where
     queue:       Queue<u8, QLEN>,
 }
 
-impl<D, P, const QLEN: usize> Logger<D, P, QLEN>
+
+impl<D, P, const QLEN: usize> Logger for UartLogger<D, P, QLEN>
 where
     D: UartDevice,
     P: ValidUartPinout<D>
 {
-    pub fn new(
-        uart: UartPeripheral<Enabled, D, P>
-        ) -> Self
-    {
-        Logger::<D, P, QLEN> {
-            uart,
-            queue: Queue::new()
-        }
-    }
-
-    pub fn log(&mut self, msg: &[u8]) -> Result<(), LoggerError>
+    fn log(&mut self, msg: &[u8]) -> Result<(), LoggerError>
     {
         for c in msg.iter() {
             match self.queue.enqueue(*c) {
@@ -44,6 +40,23 @@ where
 
         self.process().map(|_| ())
     }
+}
+
+
+impl<D, P, const QLEN: usize> UartLogger<D, P, QLEN>
+where
+    D: UartDevice,
+    P: ValidUartPinout<D>
+{
+    pub fn new(
+        uart: UartPeripheral<Enabled, D, P>
+        ) -> Self
+    {
+        UartLogger::<D, P, QLEN> {
+            uart,
+            queue: Queue::new()
+        }
+    }
 
     pub fn log_fatal(&mut self, msg: &[u8])
     {
diff --git a/src/main.rs b/src/main.rs
index 208d323..bb3e4ca 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -44,6 +44,7 @@ mod ext_adc;
 mod logger;
 mod switch_control;
 
+use logger::UartLogger;
 use logger::Logger;
 
 /// The linker will place this boot block at the start of our program image. We
@@ -137,7 +138,7 @@ fn main() -> ! {
         .unwrap();
 
     // initialize the logger
-    let mut logger: Logger<_, _, 256> = Logger::new(uart);
+    let mut logger: UartLogger<_, _, 256> = UartLogger::new(uart);
     logger.log(b"Logging initialized.\r\n").unwrap();
 
     // Init PWMs
@@ -216,7 +217,7 @@ fn main() -> ! {
         pac::NVIC::unmask(pac::Interrupt::TIMER_IRQ_0);
     }
 
-    let mut switchctrl = switch_control::SwitchControl::<1>::new(8000, SWITCH_PWM_LIMIT);
+    let mut switchctrl = switch_control::SwitchControl::<1>::new(8000, 100, SWITCH_PWM_LIMIT);
 
     let mut loopcnt: u64 = 0;
 
@@ -277,38 +278,44 @@ fn main() -> ! {
             pwr_switch_ch.set_duty(pwmval as u16);
 
             // do not output status data every loop
-            if loopcnt % 500 == 250 {
-                let mut data: String<16>;
+            match loopcnt % 500 {
+                1 => {
+                    let mut data: String<16>;
 
-                logger.log(b"Vin: ").unwrap();
-                data = String::from(vin);
-                logger.log(data.as_bytes()).unwrap();
-                logger.log(b"mV - Vout: ").unwrap();
-                data = String::from(vout);
-                logger.log(data.as_bytes()).unwrap();
-                logger.log(b"mV - Iout: ").unwrap();
-                data = String::from(iout);
-                logger.log(data.as_bytes()).unwrap();
-                logger.log(b"mA - Pout: ").unwrap();
-                data = String::from(vout * iout / 1000);
-                logger.log(data.as_bytes()).unwrap();
-
-                logger.log(b"mW - raw: [").unwrap();
-
-                for i in 0..adc_value.len() {
-                    data = String::from(adc_value[i]);
+                    logger.log(b"Vin: ").unwrap();
+                    data = String::from(vin);
+                    logger.log(data.as_bytes()).unwrap();
+                    logger.log(b"mV - Vout: ").unwrap();
+                    data = String::from(vout);
+                    logger.log(data.as_bytes()).unwrap();
+                    logger.log(b"mV - Iout: ").unwrap();
+                    data = String::from(iout);
+                    logger.log(data.as_bytes()).unwrap();
+                    logger.log(b"mA - Pout: ").unwrap();
+                    data = String::from(vout * iout / 1000);
                     logger.log(data.as_bytes()).unwrap();
 
-                    if i < 3 {
-                        logger.log(b", ").unwrap();
+                    logger.log(b"mW - raw: [").unwrap();
+
+                    for i in 0..adc_value.len() {
+                        data = String::from(adc_value[i]);
+                        logger.log(data.as_bytes()).unwrap();
+
+                        if i < 3 {
+                            logger.log(b", ").unwrap();
+                        }
                     }
-                }
 
-                logger.log(b"]; PWM: ").unwrap();
-                data = String::from(pwmval);
-                logger.log(data.as_bytes()).unwrap();
+                    logger.log(b"]; PWM: ").unwrap();
+                    data = String::from(pwmval);
+                    logger.log(data.as_bytes()).unwrap();
 
-                logger.log(b"\r\n").unwrap();
+                    logger.log(b"\r\n").unwrap();
+                },
+                2 => {
+                    switchctrl.log_status(&mut logger);
+                },
+                _ => {}
             }
 
             loopcnt += 1;
diff --git a/src/switch_control.rs b/src/switch_control.rs
index 567ecd0..da3825a 100644
--- a/src/switch_control.rs
+++ b/src/switch_control.rs
@@ -1,3 +1,6 @@
+use crate::logger::Logger;
+use heapless::String;
+
 enum ControlMode {
     MPPT,
     ConstantVoltage
@@ -16,6 +19,10 @@ struct PID<const GAINSCALE: i32, const T0: i32>
 
     iaccu_min: i32, // minimum allowed value in iaccu
     iaccu_max: i32, // maximum allowed value in iaccu
+
+    dbg_p: i32,
+    dbg_i: i32,
+    dbg_d: i32,
 }
 
 impl<const GAINSCALE: i32, const T0: i32> PID<GAINSCALE, T0>
@@ -31,10 +38,19 @@ impl<const GAINSCALE: i32, const T0: i32> PID<GAINSCALE, T0>
             elast: 0,
             target,
             iaccu_min,
-            iaccu_max
+            iaccu_max,
+            dbg_p: 0,
+            dbg_i: 0,
+            dbg_d: 0
         }
     }
 
+    pub fn restart_from_output_value(&mut self, oval: i32)
+    {
+        self.iaccu = oval * GAINSCALE;
+        self.elast = 0;
+    }
+
     pub fn update(&mut self, meas: i32) -> i32
     {
         let err = self.target - meas;
@@ -50,10 +66,14 @@ impl<const GAINSCALE: i32, const T0: i32> PID<GAINSCALE, T0>
 
         let pval = self.pgain * err / GAINSCALE;
         let ival = self.iaccu / GAINSCALE;
-        let dval = self.dgain * (err - self.elast) / GAINSCALE;
+        let dval = self.dgain * (err - self.elast) / GAINSCALE / T0;
 
         self.elast = err;
 
+        self.dbg_p = pval;
+        self.dbg_i = ival;
+        self.dbg_d = dval;
+
         pval + ival + dval
     }
 
@@ -61,9 +81,24 @@ impl<const GAINSCALE: i32, const T0: i32> PID<GAINSCALE, T0>
     {
         self.target = new_target;
     }
+
+    pub fn log_status(&self, logger: &mut dyn Logger)
+    {
+        let mut data: String<16>;
+
+        logger.log(b"Last P: ").unwrap();
+        data = String::from(self.dbg_p);
+        logger.log(data.as_bytes()).unwrap();
+        logger.log(b" I: ").unwrap();
+        data = String::from(self.dbg_i);
+        logger.log(data.as_bytes()).unwrap();
+        logger.log(b" D: ").unwrap();
+        data = String::from(self.dbg_d);
+        logger.log(data.as_bytes()).unwrap();
+    }
 }
 
-struct MPPT<const INTERVAL: u32, const PWR_TOLERANCE: i32>
+struct MPPT<const INTERVAL: u32, const PERCENT_LOSS_TOLERANCE: i32>
 {
     update_delay: u32,
 
@@ -71,24 +106,34 @@ struct MPPT<const INTERVAL: u32, const PWR_TOLERANCE: i32>
     increment: i32,
     last_power: i32,
 
+    peak_power: i32, // highest power seen so far. Decreases over time.
+
     min_pwm: i32,
     max_pwm: i32,
 }
 
-impl<const INTERVAL: u32, const PWR_TOLERANCE: i32> MPPT<INTERVAL, PWR_TOLERANCE>
+impl<const INTERVAL: u32, const PERCENT_LOSS_TOLERANCE: i32> MPPT<INTERVAL, PERCENT_LOSS_TOLERANCE>
 {
     pub fn new(start_pwm: i32, min_pwm: i32, max_pwm: i32) -> Self
     {
-        MPPT::<INTERVAL, PWR_TOLERANCE> {
+        MPPT::<INTERVAL, PERCENT_LOSS_TOLERANCE> {
             update_delay: INTERVAL - 1,
             pwm: start_pwm,
             increment: 1,
             last_power: 0,
+            peak_power: 0,
             min_pwm,
-            max_pwm 
+            max_pwm
         }
     }
 
+    pub fn restart(&mut self, start_pwm: i32, start_increment: i32)
+    {
+        self.peak_power = 1;
+        self.pwm = start_pwm;
+        self.increment = start_increment;
+    }
+
     pub fn update(&mut self, vout: i32, iout: i32) -> i32
     {
         if self.update_delay == 0 {
@@ -96,10 +141,20 @@ impl<const INTERVAL: u32, const PWR_TOLERANCE: i32> MPPT<INTERVAL, PWR_TOLERANCE
 
             let cur_power = vout * iout;
 
-            let power_change = cur_power - self.last_power;
+            // decrease peak power over time
+            self.peak_power = (998 * (self.peak_power as i64) / 1000) as i32;
+
+            if cur_power > self.peak_power {
+                self.peak_power = cur_power;
+            } else if self.peak_power <= 0 {
+                self.peak_power = 1;
+            }
+
+            let peak_power_loss_percent = 100 - (100 * cur_power / self.peak_power);
 
             // invert search direction if measured power is significantly lower than previous measurement
-            if power_change.abs() > PWR_TOLERANCE && cur_power < self.last_power {
+            if peak_power_loss_percent > PERCENT_LOSS_TOLERANCE {
+                //self.peak_power = cur_power; // prevent immediate turnaround
                 self.increment = -self.increment;
             }
 
@@ -121,42 +176,117 @@ impl<const INTERVAL: u32, const PWR_TOLERANCE: i32> MPPT<INTERVAL, PWR_TOLERANCE
 
         self.pwm
     }
+
+    pub fn log_status(&self, logger: &mut dyn Logger)
+    {
+        let mut data: String<16>;
+
+        match self.increment {
+            1 => logger.log(b"\xE2\x86\x91").unwrap(), // unicode for ↑
+            -1 => logger.log(b"\xE2\x86\x93").unwrap(), // unicode for ↓
+            _ => {
+                logger.log(b"inc: ").unwrap();
+
+                data = String::from(self.increment);
+                logger.log(data.as_bytes()).unwrap();
+            }
+        }
+
+        logger.log(b" Ppk: ").unwrap();
+
+        data = String::from(self.peak_power);
+        logger.log(data.as_bytes()).unwrap();
+    }
 }
 
 pub struct SwitchControl<const T0: i32>
 {
     mode: ControlMode,
+    mode_switch_delay_counter: u32,
     cv_pid: PID<10000, T0>,
-    mppt: MPPT<20, 50000 /* microwatts */>,
-    pwm_max: i32
+    mppt: MPPT<20, 3 /* percent loss of peak power */>,
+    pwm_max: i32,
+    pwm_cur: i32,
+    cv_target: i32,
+    cv_max_deviation: i32
 }
 
 impl<const T0: i32> SwitchControl<T0>
 {
-    pub fn new(cv_target: i32, pwm_max: i32) -> Self
+    pub fn new(cv_target: i32, cv_max_deviation: i32, pwm_max: i32) -> Self
     {
         SwitchControl::<T0> {
             mode: ControlMode::MPPT,
-            cv_pid: PID::<10000, T0>::new(100, 50, 0, cv_target, 0, 0, pwm_max * 10000),
-            mppt: MPPT::<20, 50000>::new(pwm_max/2, 0, pwm_max),
-            pwm_max
+            mode_switch_delay_counter: 0,
+            cv_pid: PID::<10000, T0>::new(100, 50, 50, cv_target, 0, 0, pwm_max * 10000),
+            mppt: MPPT::<20, 3>::new(pwm_max/2, 0, pwm_max),
+            pwm_max,
+            pwm_cur: 0,
+            cv_target,
+            cv_max_deviation
         }
     }
 
     // returns the new PWM value
     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 {
+            match self.mode {
+                ControlMode::MPPT =>
+                    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;
+
+                        // 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_cur, -1);
+                    }
+            }
+        } else {
+            self.mode_switch_delay_counter -= 1;
+        }
+
+        // update the PWM value
         let pwm = match self.mode {
             ControlMode::ConstantVoltage => self.cv_pid.update(vout),
             ControlMode::MPPT => self.mppt.update(vout, iout),
         };
 
-        if pwm > self.pwm_max {
-            self.pwm_max
-        } else if pwm < 0 {
-            0
-        } else {
-            pwm
+        // store and limit pwm value
+        self.pwm_cur = pwm;
+        if self.pwm_cur > self.pwm_max {
+            self.pwm_cur = self.pwm_max;
+        } else if self.pwm_cur < 0 {
+            self.pwm_cur = 0;
         }
+
+        self.pwm_cur
+    }
+
+    pub fn log_status(&self, logger: &mut dyn Logger)
+    {
+        match self.mode {
+            ControlMode::MPPT => {
+                logger.log(b"[MPPT] ").unwrap();
+                self.mppt.log_status(logger);
+            }
+
+            ControlMode::ConstantVoltage => {
+                logger.log(b"[CV] ").unwrap();
+                self.cv_pid.log_status(logger);
+            }
+        }
+
+        logger.log(b"\r\n").unwrap();
     }
 }