Calculate total energy and print it, also in JSON format

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.

Cargo.toml

@@ -21,7 +21,7 @@ nb = "1.0"
 rp2040-pac = "0.4.0"
 paste = "1.0"
 pio = "0.2.0"
-rp2040-hal = { path = "../rp-hal/rp2040-hal/" }
+rp2040-hal = { path = "../rp-hal/rp2040-hal/", features = ["rt", "critical-section-impl"] }
 rp2040-hal-macros = "0.1.0"
 usb-device = "0.2.9"
 vcell = "0.1"
@@ -41,25 +41,5 @@ cortex-m-rt = "0.7"
 panic-halt = "0.2.0"
 rp2040-boot2 = "0.2.1"
 
-[features]
-# Minimal startup / runtime for Cortex-M microcontrollers
-rt = ["rp2040-pac/rt"]
-
-# Memoize(cache) ROM function pointers on first use to improve performance
-rom-func-cache = []
-
-# Disable automatic mapping of language features (like floating point math) to ROM functions
-disable-intrinsics = []
-
-# This enables ROM functions for f64 math that were not present in the earliest RP2040s
-rom-v2-intrinsics = []
-
-# This enables a fix for USB errata 5: USB device fails to exit RESET state on busy USB bus.
-# Only required for RP2040 B0 and RP2040 B1, but it also works for RP2040 B2 and above
-rp2040-e5 = []
-
-# critical section that is safe for multicore use
-critical-section-impl = ["critical-section/restore-state-u8"]
-
 [build]
 target = "thumbv6m-none-eabi"

src/ext_adc.rs

@@ -0,0 +1,80 @@
+use cortex_m::singleton;
+use rp2040_hal::dma::{bidirectional, Channel};
+use fugit::HertzU32;
+use fugit::RateExtU32;
+
+pub struct AdcContext<D, TXC, RXC, CSP>
+where
+    TXC: rp2040_hal::dma::ChannelIndex,
+    RXC: rp2040_hal::dma::ChannelIndex,
+    D: rp2040_hal::spi::SpiDevice
+{
+    dma_tx_chan: Channel<TXC>,
+    dma_rx_chan: Channel<RXC>,
+    tx_buf:      &'static mut [u8; 3],
+    rx_buf:      &'static mut [u8; 3],
+    spi:         rp2040_hal::spi::Spi<rp2040_hal::spi::Enabled, D, 8>,
+    cs_pin:      CSP
+}
+
+impl<D, TXC, RXC, E, CSP> AdcContext<D, TXC, RXC, CSP>
+where
+    TXC: rp2040_hal::dma::ChannelIndex,
+    RXC: rp2040_hal::dma::ChannelIndex,
+    D: rp2040_hal::spi::SpiDevice,
+    E: core::fmt::Debug,
+    CSP: embedded_hal::digital::v2::OutputPin<Error=E>
+{
+    pub fn init(
+        device: D,
+        periph_freq: HertzU32,
+        resets: &mut rp2040_pac::RESETS,
+        tx_chan: Channel<TXC>,
+        rx_chan: Channel<RXC>,
+        mut cs_pin: CSP
+        ) -> Self
+    {
+        let spi_if = rp2040_hal::spi::Spi::<_, _, 8>::new(device);
+
+        // Exchange the uninitialised SPI driver for an initialised one
+        let spi_if = spi_if.init(
+            resets,
+            periph_freq,
+            1_000_000u32.Hz(),
+            &embedded_hal::spi::MODE_0,
+        );
+
+        cs_pin.set_high().unwrap();
+
+        AdcContext::<D, TXC, RXC, CSP> {
+            dma_rx_chan: rx_chan,
+            dma_tx_chan: tx_chan,
+            tx_buf:      singleton!(: [u8; 3] = [0x06, 0x80, 0x00]).unwrap(),
+            rx_buf:      singleton!(: [u8; 3] = [0; 3]).unwrap(),
+            spi:         spi_if,
+            cs_pin:      cs_pin
+        }
+    }
+
+    pub fn sample_adc_channel(mut self, chan: u8) -> (Self, u16)
+    {
+        self.tx_buf[1] = chan << 6;
+
+        // clear chip select
+        self.cs_pin.set_low().unwrap();
+
+        // Use BidirectionalConfig to simultaneously write to spi from tx_buf and read into rx_buf
+        let transfer = bidirectional::Config::new(
+            (self.dma_tx_chan, self.dma_rx_chan),
+            self.tx_buf,
+            self.spi,
+            self.rx_buf).start();
+
+        // Wait for both DMA channels to finish
+        ((self.dma_tx_chan, self.dma_rx_chan), self.tx_buf, self.spi, self.rx_buf) = transfer.wait();
+        self.cs_pin.set_high().unwrap();
+
+        let result = (((self.rx_buf[1] & 0x0F) as u16) << 8) | self.rx_buf[2] as u16;
+        (self, result)
+    }
+}

src/logger.rs

@@ -0,0 +1,98 @@
+
+use rp2040_hal::uart::{Enabled, UartDevice, UartPeripheral, ValidUartPinout};
+use embedded_hal::serial::Write;
+use heapless::spsc::Queue;
+
+#[derive(Debug)]
+pub enum LoggerError {
+    OutOfMemory,
+    WriteError
+}
+
+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>
+{
+    uart:        UartPeripheral<Enabled, D, P>,
+    queue:       Queue<u8, QLEN>,
+}
+
+
+impl<D, P, const QLEN: usize> Logger for UartLogger<D, P, QLEN>
+where
+    D: UartDevice,
+    P: ValidUartPinout<D>
+{
+    fn log(&mut self, msg: &[u8]) -> Result<(), LoggerError>
+    {
+        for c in msg.iter() {
+            match self.queue.enqueue(*c) {
+                Ok(_) => {},
+                Err(_c) => return Err(LoggerError::OutOfMemory)
+            }
+        }
+
+        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])
+    {
+        // first flush all data in the queue
+        self.flush();
+
+        // enqueue the message
+        let _ = self.log(msg);
+
+        // block until it is actually handed over to the hardware
+        self.flush();
+    }
+
+    pub fn flush(&mut self)
+    {
+        // call process() until no more data remains in the queue
+        loop {
+            match self.process() {
+                Ok(more_data) => if !more_data {
+                    break;
+                },
+                Err(_) => break
+            }
+        }
+    }
+
+    // returns whether data remains in the queue
+    pub fn process(&mut self) -> Result<bool, LoggerError>
+    {
+        while !self.queue.is_empty() && self.uart.uart_is_writable() {
+            match self.uart.write(self.queue.dequeue().unwrap()) {
+                Ok(_) => {},
+                Err(_) => return Err(LoggerError::WriteError)
+            }
+        }
+
+        Ok(!self.queue.is_empty())
+    }
+}

src/main.rs

@@ -1,16 +1,14 @@
-//! # PWM Blink Example
+//! # Firmware for the handcrank project.
 //!
-//! If you have an LED connected to pin 25, it will fade the LED using the PWM
+//! Implements a voltage boost regulator. Measures the input and output voltages and output current
-//! peripheral.
+//! using an external ADC connected via SPI to pins GP2 to GP5. The switching transistor is
-//!
+//! controlled directly via a PWM signal on pin GP10.
-//! It may need to be adapted to your particular board layout and/or pin assignment.
-//!
-//! See the `Cargo.toml` file for Copyright and license details.
 
 #![no_std]
 #![no_main]
 
 use embedded_hal::digital::v2::OutputPin;
+
 // Ensure we halt the program on panic (if we don't mention this crate it won't
 // be linked)
 use panic_halt as _;
@@ -18,24 +16,37 @@ use panic_halt as _;
 // Alias for our HAL crate
 use rp2040_hal as hal;
 
-// Some traits we need
-use cortex_m::singleton;
-use embedded_hal::PwmPin;
-use fugit::RateExtU32;
-use rp2040_hal::clocks::Clock;
-
 // A shorter alias for the Peripheral Access Crate, which provides low-level
 // register access
 use hal::pac;
 
 // SPI + DMA
-use hal::dma::{bidirectional, DMAExt};
+use hal::dma::DMAExt;
 
 // UART related types
 use hal::uart::{DataBits, StopBits, UartConfig};
 
 use heapless::String;
 
+// Some traits we need
+//use cortex_m::singleton;
+use fugit::MicrosDurationU32;
+use embedded_hal::PwmPin;
+use fugit::RateExtU32;
+use hal::clocks::Clock;
+use hal::timer::Alarm;
+use hal::timer::ScheduleAlarmError;
+use pac::interrupt;
+use core::cell::RefCell;
+use critical_section::Mutex;
+
+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
 /// need this to help the ROM bootloader get our code up and running.
 /// Note: This boot block is not necessary when using a rp-hal based BSP
@@ -44,16 +55,29 @@ use heapless::String;
 #[used]
 pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H;
 
-/// The minimum PWM value (i.e. LED brightness) we want
+const SYSTICK_INTERVAL_US: MicrosDurationU32 = MicrosDurationU32::millis(1);
-const LOW: u16 = 0;
 
-/// The maximum PWM value (i.e. LED brightness) we want
+static mut SYSTICK_ALARM: Mutex<RefCell<Option<hal::timer::Alarm0>>> = Mutex::new(RefCell::new(None));
-const HIGH: u16 = 25000;
+
+// Flag that is set by the alarm interrupt
+static mut SYSTICK_FLAG: bool = false;
 
 /// External high-speed crystal on the Raspberry Pi Pico board is 12 MHz. Adjust
 /// if your board has a different frequency
 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]) -> (i32, i32, i32)
+{
+    let iout = (raw[0] as i32 - raw[1] as i32) * 3300 * 20 / 4096 / 28; // *20 = division by 50 mΩ shunt
+    let vout = raw[2] as i32 * 3300 * (220 + 10) / 10 / 4096;
+    let vin = raw[3] as i32 * 3300 * (100 + 10) / 10 / 4096;
+
+    (vin, vout, iout) /* units: mV, mV, mA */
+}
+
 /// Entry point to our bare-metal application.
 ///
 /// The `#[rp2040_hal::entry]` macro ensures the Cortex-M start-up code calls this function
@@ -65,7 +89,6 @@ const XTAL_FREQ_HZ: u32 = 12_000_000u32;
 fn main() -> ! {
     // Grab our singleton objects
     let mut pac = pac::Peripherals::take().unwrap();
-    let core = pac::CorePeripherals::take().unwrap();
 
     // Set up the watchdog driver - needed by the clock setup code
     let mut watchdog = hal::Watchdog::new(pac.WATCHDOG);
@@ -98,7 +121,7 @@ fn main() -> ! {
 
     // The delay object lets us wait for specified amounts of time (in
     // milliseconds)
-    let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());
+    //let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());
 
     // UART
     let uart_pins = (
@@ -109,44 +132,46 @@ fn main() -> ! {
     );
     let uart = hal::uart::UartPeripheral::new(pac.UART0, uart_pins, &mut pac.RESETS)
         .enable(
-            UartConfig::new(9600.Hz(), DataBits::Eight, None, StopBits::One),
+            UartConfig::new(38400.Hz(), DataBits::Eight, None, StopBits::One),
             clocks.peripheral_clock.freq(),
         )
         .unwrap();
 
+    // initialize the logger
+    let mut logger: UartLogger<_, _, 256> = UartLogger::new(uart);
+    logger.log(b"Logging initialized.\r\n").unwrap();
+
     // Init PWMs
     let mut pwm_slices = hal::pwm::Slices::new(pac.PWM, &mut pac.RESETS);
 
-    // Configure PWM4
+    // Configure switch PWM
     let pwm5 = &mut pwm_slices.pwm5;
     pwm5.set_ph_correct();
     pwm5.set_div_int(4);
     pwm5.set_div_frac((88 << 4) / 100);
-    pwm5.set_top(512);
+    pwm5.set_top(SWITCH_PWM_MAX as u16);
     pwm5.enable();
 
-    let pwm6 = &mut pwm_slices.pwm6;
+    // set up switch PWM output
-    pwm6.set_ph_correct();
+    // note:
-    pwm6.enable();
+    // duty cycle = 0 => output voltage = input voltage
-    let pwm7 = &mut pwm_slices.pwm7;
+    // duty cycle = 512 (100%) => input short circuit
-    pwm7.set_ph_correct();
+    //
-    pwm7.enable();
+    // general: D = duty cycle (0..1)
-
+    // => output voltage = 1/(1-D) * input voltage
-    // Output channel B on PWM4 to GPIO 25
-    let channel1 = &mut pwm6.channel_b;
-    channel1.output_to(pins.gpio13);
-    let channel2 = &mut pwm7.channel_a;
-    channel2.output_to(pins.gpio14);
-    let channel3 = &mut pwm7.channel_b;
-    channel3.output_to(pins.gpio15);
-
     let pwr_switch_ch = &mut pwm5.channel_a;
+    pwr_switch_ch.set_duty(0); // no output by default
     pwr_switch_ch.output_to(pins.gpio10);
-    pwr_switch_ch.set_duty(450);
 
-    let mut ch_val: [u16; 3] = [LOW, LOW + (HIGH - LOW) / 3, LOW + 2 * (HIGH - LOW) / 3];
+    // LED pins
+    let mut pin_led_r = pins.gpio15.into_push_pull_output();
+    let mut pin_led_y = pins.gpio13.into_push_pull_output();
+    let mut pin_led_g = pins.gpio14.into_push_pull_output();
 
-    uart.write_full_blocking(b"Initialization complete!\r\n");
+    // off by default
+    pin_led_r.set_low().unwrap();
+    pin_led_y.set_low().unwrap();
+    pin_led_g.set_low().unwrap();
 
     // SPI CS pin is controlled by software
     let mut spi_cs = pins.gpio5.into_push_pull_output();
@@ -156,54 +181,224 @@ fn main() -> ! {
     let _spi_sclk = pins.gpio2.into_mode::<hal::gpio::FunctionSpi>();
     let _spi_mosi = pins.gpio3.into_mode::<hal::gpio::FunctionSpi>();
     let _spi_miso = pins.gpio4.into_mode::<hal::gpio::FunctionSpi>();
-    let spi = hal::spi::Spi::<_, _, 8>::new(pac.SPI0);
-
-    // Exchange the uninitialised SPI driver for an initialised one
-    let spi = spi.init(
-        &mut pac.RESETS,
-        clocks.peripheral_clock.freq(),
-        1_000_000u32.Hz(),
-        &embedded_hal::spi::MODE_0,
-    );
 
     // Initialize DMA.
     let dma = pac.DMA.split(&mut pac.RESETS);
 
-    // Use DMA to read ADC (0xC0 in byte 1 is the channel mask)
+    let mut adc_ctrl = ext_adc::AdcContext::init(
-    let tx_buf = singleton!(: [u8; 3] = [0x06, 0x40, 0x00]).unwrap();
+        pac.SPI0,
-    let rx_buf = singleton!(: [u8; 3] = [0; 3]).unwrap();
+        clocks.peripheral_clock.freq(),
+        &mut pac.RESETS,
+        dma.ch0,
+        dma.ch1,
+        spi_cs
+        );
 
-    // clear chip select
+    logger.log(b"Initialization complete!\r\n").unwrap();
-    spi_cs.set_low().unwrap();
 
-    // Use BidirectionalConfig to simultaneously write to spi from tx_buf and read into rx_buf
+    // initialize the timer
-    let transfer = bidirectional::Config::new((dma.ch0, dma.ch1), tx_buf, spi, rx_buf).start();
+    let mut timer = hal::Timer::new(pac.TIMER, &mut pac.RESETS);
-    // Wait for both DMA channels to finish
-    let ((_ch0, _ch1), tx_buf, _spi, rx_buf) = transfer.wait();
-    spi_cs.set_high().unwrap();
 
-    let adc_value = (((rx_buf[1] & 0x0F) as u16) << 8) | rx_buf[2] as u16;
+    // set up an alarm that is used for a periodic main loop execution
+    let mut next_systick_instant = timer.get_counter() + MicrosDurationU32::millis(1000);
 
-    {
+    let mut systick_alarm = timer.alarm_0().unwrap();
-        let data: String<16> = String::from(adc_value);
 
-        uart.write_full_blocking(b"Read ADC value: ");
+    systick_alarm.schedule_at(next_systick_instant).unwrap();
-        uart.write_full_blocking(data.as_bytes());
+    systick_alarm.enable_interrupt();
-        uart.write_full_blocking(b"\r\n");
+
+    unsafe {
+        // move the alarm object so the IRQ can access it
+        critical_section::with(|cs| {
+            SYSTICK_ALARM.borrow(cs).replace(Some(systick_alarm));
+        });
+
+        // Unmask the timer0 IRQ so that it will generate an interrupt
+        pac::NVIC::unmask(pac::Interrupt::TIMER_IRQ_0);
     }
 
-    // Infinite loop, fading LED up and down
+    let mut switchctrl = switch_control::SwitchControl::<1>::new(12000, 100, SWITCH_PWM_LIMIT);
+
+    // track nano_joules (== nanoVoltAmpereSeconds) generated, for LED flashing
+    let mut nano_joules: u32 = 0;
+    let mut joules: u32 = 0; // total energy generated
+    let mut loopcnt_led_y_off = 0;
+
+    let mut loopcnt: u64 = 0;
+
+    pin_led_r.set_high().unwrap();
+
+    // main loop
     loop {
-        for i in 0..ch_val.len() {
+        // only execute the main loop if the systick has expired
-            ch_val[i] += 1;
+        let mut systick_received = false;
-            if ch_val[i] > HIGH {
+
-                ch_val[i] -= HIGH - LOW;
+        critical_section::with(|_cs| unsafe {
+            systick_received = core::ptr::read_volatile(&SYSTICK_FLAG);
+        });
+
+        if systick_received {
+            // re-shedule the systick alarm
+            critical_section::with(|_cs| unsafe {
+                core::ptr::write_volatile(&mut SYSTICK_FLAG, false);
+            });
+
+            next_systick_instant += SYSTICK_INTERVAL_US;
+
+            critical_section::with(|cs| {
+                unsafe {
+                    if let Some(mut systick_alarm) = SYSTICK_ALARM.borrow(cs).take() {
+                        let result = systick_alarm.schedule_at(next_systick_instant);
+                        match result {
+                            Ok(_) => {}, // just continue
+                            Err(e) => {
+                                match e {
+                                    ScheduleAlarmError::AlarmTooLate => logger.log_fatal(b"Systick: alarm too late.\r\n"),
+                                };
                             }
                         }
 
-        channel1.set_duty(ch_val[0]);
+                        SYSTICK_ALARM.borrow(cs).replace(Some(systick_alarm));
-        channel2.set_duty(ch_val[1]);
+                    } else {
-        channel3.set_duty(ch_val[2]);
+                        logger.log_fatal(b"Systick: object not set when it should be!\r\n");
-        delay.delay_us(50);
                     }
                 }
+            });
+
+
+            let mut adc_value: [u16; 4] = [0; 4];
+
+            (adc_ctrl, adc_value[0]) = adc_ctrl.sample_adc_channel(0);
+            (adc_ctrl, adc_value[1]) = adc_ctrl.sample_adc_channel(1);
+            (adc_ctrl, adc_value[2]) = adc_ctrl.sample_adc_channel(2);
+            (adc_ctrl, adc_value[3]) = adc_ctrl.sample_adc_channel(3);
+
+            let (vin, vout, iout) = convert_adc_measurements(&adc_value);
+
+            // Update the PWM value for the switch
+            let pwmval = switchctrl.update(vin, vout, iout);
+
+            pwr_switch_ch.set_duty(pwmval as u16);
+
+            // Yellow LED flashes for 10 ms whenever 1 Joule has been generated.
+            // This means that it flashes once per second when output power is 1 Watt, and 3 times
+            // per second if output power is 3 Watt.
+            let nano_joules_generated = vout * iout; // * 1 millisecond, implicitely
+            if nano_joules_generated > 0 {
+                nano_joules += nano_joules_generated as u32;
+            }
+
+            // 1000 mV * 1000 mA * 1000 ms = 1 Joule
+            if nano_joules >= 1000000000 {
+                pin_led_y.set_high().unwrap();
+                loopcnt_led_y_off = loopcnt + 10;
+                nano_joules -= 1000000000;
+                joules += 1;
+            }
+
+            if loopcnt == loopcnt_led_y_off {
+                pin_led_y.set_low().unwrap();
+            }
+
+            // 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::MPPT             => {pin_led_g.set_low().unwrap()},
+                switch_control::ControlMode::ImpedanceControl => {pin_led_g.set_low().unwrap()},
+            }
+
+            // do not output status data every loop
+            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 - E: ").unwrap();
+                    data = String::from(joules);
+                    logger.log(data.as_bytes()).unwrap();
+                    logger.log(b"J / ").unwrap();
+                    data = String::from(joules/3600);
+                    logger.log(data.as_bytes()).unwrap();
+
+                    logger.log(b"Wh - 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"\r\n").unwrap();
+                },
+                2 => {
+                    let mut data: String<16>;
+
+                    logger.log(b"{\"vin\":").unwrap();
+                    data = String::from(vin);
+                    logger.log(data.as_bytes()).unwrap();
+                    logger.log(b",\"vout\":").unwrap();
+                    data = String::from(vout);
+                    logger.log(data.as_bytes()).unwrap();
+                    logger.log(b",\"iout\":").unwrap();
+                    data = String::from(iout);
+                    logger.log(data.as_bytes()).unwrap();
+                    logger.log(b",\"pout\":").unwrap();
+                    data = String::from(vout * iout);
+                    logger.log(data.as_bytes()).unwrap();
+                    logger.log(b",\"energy\":").unwrap();
+                    data = String::from(joules);
+                    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();
+                },
+                3 => {
+                    switchctrl.log_status(&mut logger);
+                },
+                _ => {}
+            }
+
+            loopcnt += 1;
+        }
+
+        logger.process().unwrap();
+
+        // save some power as long as no interrupt occurs
+        // FIXME: not working yet for some reason
+        //cortex_m::asm::wfi();
+    }
+}
+
+#[interrupt]
+fn TIMER_IRQ_0() {
+    critical_section::with(|cs| {
+        unsafe {
+            core::ptr::write_volatile(&mut SYSTICK_FLAG, true);
+
+            if let Some(mut systick_alarm) = SYSTICK_ALARM.borrow(cs).take() {
+                systick_alarm.clear_interrupt();
+
+                SYSTICK_ALARM.borrow(cs).replace(Some(systick_alarm));
+            }
+        }
+    });
+}

src/switch_control.rs

@@ -0,0 +1,391 @@
+use crate::logger::Logger;
+use heapless::String;
+
+const PWR_AVG_EXPONENT: i64 = 12;
+
+#[derive(Copy, Clone)]
+pub enum ControlMode {
+    MPPT,
+    ConstantVoltage,
+    ImpedanceControl
+}
+
+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
+
+    dbg_p: i32,
+    dbg_i: i32,
+    dbg_d: i32,
+}
+
+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,
+            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;
+
+        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 / T0;
+
+        self.elast = err;
+
+        self.dbg_p = pval;
+        self.dbg_i = ival;
+        self.dbg_d = dval;
+
+        pval + ival + dval
+    }
+
+    pub fn set_target(&mut self, new_target: i32)
+    {
+        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 PERCENT_LOSS_TOLERANCE: i32>
+{
+    update_delay: u32,
+
+    pwm: i32,
+    increment: i32,
+
+    avg_power: i64, // averaged power value
+    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 PERCENT_LOSS_TOLERANCE: i32> MPPT<INTERVAL, PERCENT_LOSS_TOLERANCE>
+{
+    pub fn new(start_pwm: i32, min_pwm: i32, max_pwm: i32) -> Self
+    {
+        MPPT::<INTERVAL, PERCENT_LOSS_TOLERANCE> {
+            update_delay: INTERVAL - 1,
+            pwm: start_pwm,
+            increment: 1,
+            avg_power: 0,
+            peak_power: 0,
+            last_power: 0,
+            min_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
+    {
+        let cur_power = vout * iout;
+
+        self.avg_power = self.avg_power - (self.avg_power >> PWR_AVG_EXPONENT) + cur_power as i64;
+
+        let avg_power_uw = (self.avg_power >> PWR_AVG_EXPONENT) as i32;
+
+        if self.update_delay == 0 {
+            self.update_delay = INTERVAL - 1;
+
+            // decrease peak power over time
+            self.peak_power = (990 * (self.peak_power as i64) / 1000) as i32;
+
+            /*
+            if avg_power_uw > self.peak_power {
+                self.peak_power = avg_power_uw;
+            } else if self.peak_power <= 0 {
+                self.peak_power = 1;
+            }
+
+            let peak_power_loss_percent = 100 - (100 * avg_power_uw / self.peak_power);
+
+            // invert search direction if measured power is significantly lower than previous measurement
+            if peak_power_loss_percent > PERCENT_LOSS_TOLERANCE {
+            */
+
+            if avg_power_uw < self.last_power {
+                //self.peak_power = cur_power; // prevent immediate turnaround
+                self.increment = -self.increment;
+            }
+
+            self.last_power = avg_power_uw;
+
+            // enforce the pwm limits
+            if self.pwm == self.max_pwm {
+                self.increment = -1;
+            } else if self.pwm == self.min_pwm {
+                self.increment = 1;
+            }
+
+            // step to the next test PWM value
+            self.pwm += self.increment;
+        } else {
+            self.update_delay -= 1;
+        }
+
+        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();
+
+        logger.log(b" Pavg: ").unwrap();
+
+        data = String::from(self.avg_power >> PWR_AVG_EXPONENT);
+        logger.log(data.as_bytes()).unwrap();
+    }
+}
+
+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, 5400, 1000>,
+    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, cv_max_deviation: i32, pwm_max: i32) -> Self
+    {
+        SwitchControl::<T0> {
+            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, 5400, 1000>::new(1000, 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::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::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);
+
+                        // back to impedance control
+                        self.imp_ctrl.restart(self.pwm_max * 80 / 100);
+                    }
+            }
+        } 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),
+            ControlMode::ImpedanceControl => self.imp_ctrl.update(vin, vout, iout),
+        };
+
+        // 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);
+            }
+
+            ControlMode::ImpedanceControl => {
+                logger.log(b"[Imp] ").unwrap();
+                self.imp_ctrl.log_status(logger);
+            }
+        }
+
+        logger.log(b"\r\n").unwrap();
+    }
+
+    pub fn get_control_mode(&self) -> ControlMode
+    {
+        self.mode
+    }
+}

visualizer/.gitignore

@@ -0,0 +1,3 @@
+config.py
+__pycache__/
+env/

visualizer/main.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+
+from bottle import Bottle, request, response, template, static_file
+import serial
+import json
+import time
+
+app = application = Bottle()
+
+serial_in_use = False
+
+@app.route('/')
+@app.route('/index.html')
+def index():
+    return template('index')
+
+@app.route('/websocket')
+def handle_websocket():
+    global serial_in_use
+
+    wsock = request.environ.get('wsgi.websocket')
+    if not wsock:
+        abort(400, 'Expected WebSocket request.')
+
+    if serial_in_use:
+        abort(503, 'Serial connection already in use.')
+
+    with serial.Serial('/dev/ttyUSB0', 38400, timeout=10) as ser:
+    #with open('/dev/null') as ser:
+        serial_in_use = True
+        print("Serial locked.")
+        while True:
+            try:
+                line = ser.readline()
+                #line = '{"vin": 1240, "vout": 15300, "iout": 817, "pout": 12345, "energy": 9387, "pwm": 314}\r\n'
+                time.sleep(1)
+                if line[0] != '{':
+                    # print all lines that are probably not JSON
+                    print(line)
+                    continue
+
+                data = json.loads(line)
+                wsock.send(json.dumps(data))
+            except json.JSONDecodeError:
+                print(f"Failed to decode line as JSON: »{line}«.")
+            except WebSocketError:
+                break
+        serial_in_use = False
+        print("Serial unlocked.")
+
+
+@app.route('/static/<filename:path>')
+def send_static(filename):
+    return static_file(filename, root='./static')
+
+if __name__ == "__main__":
+    from gevent.pywsgi import WSGIServer
+    from geventwebsocket import WebSocketError
+    from geventwebsocket.handler import WebSocketHandler
+    server = WSGIServer(("0.0.0.0", 8080), app,
+                        handler_class=WebSocketHandler)
+    server.serve_forever()

visualizer/requirements.txt

@@ -0,0 +1,4 @@
+bottle ~= 0.12.25
+gevent ~= 24.2.1
+gevent-websocket ~= 0.10.0
+pyserial ~= 3.5

visualizer/static/style.css

@@ -0,0 +1,8 @@
+body {
+	color: black;
+	background-color: #e0e0e0;
+}
+
+div.plot {
+	display: inline-block;
+}

visualizer/views/index.tpl

@@ -0,0 +1,173 @@
+<!DOCTYPE html>
+<html lang="de">
+	<head>
+		<script type="text/javascript" src="/static/plotly-2.29.1.min.js" charset="utf-8"></script>
+		<meta name="viewport" content="width=device-width, initial-scale=1.0"> 
+		<link rel="stylesheet" href="static/style.css">
+	</head>
+	<body>
+		<div class="row">
+			<div class="plot" id="plotly_pout"></div>
+			<div class="plot" id="plotly_energy"></div>
+			<div class="plot" id="plotly_pwm"></div>
+		</div>
+		<div class="row">
+			<div class="plot" id="plotly_vin"></div>
+			<div class="plot" id="plotly_vout"></div>
+			<div class="plot" id="plotly_iout"></div>
+		</div>
+
+		<script type="text/javascript">
+			/*********************************
+			 * Plot setup                    *
+			 *********************************/
+
+			var plot_w = 400;
+			var plot_h = 300;
+
+			// vin plot
+			var plot_data = [
+				{
+					domain: { x: [0, 1], y: [0, 1] },
+					value: 6.2,
+					number: {"suffix": "V"},
+					title: { text: "Eingangsspannung" },
+					type: "indicator",
+					mode: "gauge+number",
+					gauge: {
+						axis: { range: [0, 10] },
+						bar: { color: "blue" }
+					},
+				}
+			];
+
+			var layout = { width: plot_w, height: plot_h, margin: { t: 0, b: 0 } };
+			Plotly.newPlot('plotly_vin', plot_data, layout);
+
+			var plot_data = [
+				{
+					domain: { x: [0, 1], y: [0, 1] },
+					value: 10,
+					number: {"suffix": "V"},
+					title: { text: "Ausgangsspannung" },
+					type: "indicator",
+					mode: "gauge+number",
+					gauge: {
+						axis: { range: [0, 20] },
+						bar: { color: "yellow" },
+						threshold: {
+							line: { color: "green", width: 4 },
+							thickness: 0.75,
+							value: 12.0
+						},
+						steps: [
+							{ range: [0, 16], color: "white" },
+							{ range: [16, 20], color: "#ff8080" }
+						],
+					},
+				}
+			];
+
+			var layout = { width: plot_w, height: plot_h, margin: { t: 0, b: 0 } };
+			Plotly.newPlot('plotly_vout', plot_data, layout);
+
+			var plot_data = [
+				{
+					domain: { x: [0, 1], y: [0, 1] },
+					value: 5,
+					number: {"suffix": "W"},
+					title: { text: "Leistung" },
+					type: "indicator",
+					mode: "gauge+number",
+					gauge: {
+						axis: { range: [0, 30] },
+						bar: { color: "orange" },
+						steps: [
+							{ range: [0, 20], color: "white" },
+							{ range: [20, 30], color: "#ff8080" }
+						],
+					},
+				}
+			];
+
+			var layout = { width: plot_w, height: plot_h, margin: { t: 0, b: 0 } };
+			Plotly.newPlot('plotly_pout', plot_data, layout);
+
+			var plot_data = [
+				{
+					domain: { x: [0, 1], y: [0, 1] },
+					value: 0.7,
+					number: {"suffix": "A"},
+					title: { text: "Ausgangsstrom" },
+					type: "indicator",
+					mode: "gauge+number",
+					gauge: {
+						axis: { range: [0, 2] },
+						bar: { color: "blue" }
+					},
+				}
+			];
+
+			var layout = { width: plot_w, height: plot_h, margin: { t: 0, b: 0 } };
+			Plotly.newPlot('plotly_iout', plot_data, layout);
+
+			var plot_data = [
+				{
+					domain: { x: [0, 1], y: [0, 1] },
+					value: 0.7,
+					number: {"suffix": "J"},
+					title: { text: "Energie" },
+					type: "indicator",
+					mode: "number"
+				}
+			];
+
+			var layout = { width: plot_w, height: plot_h, margin: { t: 0, b: 0 } };
+			Plotly.newPlot('plotly_energy', plot_data, layout);
+
+			var plot_data = [
+				{
+					domain: { x: [0, 1], y: [0, 1] },
+					value: 321,
+					title: { text: "PWM" },
+					type: "indicator",
+					mode: "gauge+number",
+					gauge: {
+						axis: { range: [0, 486] },
+						bar: { color: "black" }
+					},
+				}
+			];
+
+			var layout = { width: plot_w, height: plot_h, margin: { t: 0, b: 0 } };
+			Plotly.newPlot('plotly_pwm', plot_data, layout);
+
+			/*********************************
+			 * Websocket handling            *
+			 *********************************/
+
+			var ws = new WebSocket("ws://" + window.location.host + "/websocket");
+			ws.onopen = function() {
+				console.log("Websocket connected.");
+			};
+			ws.onmessage = function (evt) {
+				data = JSON.parse(evt.data);
+
+				if(data) {
+					console.log("Data received:");
+					console.log(data);
+
+					Plotly.restyle('plotly_vin', 'value', [data.vin / 1e3]);       // millivolt
+					Plotly.restyle('plotly_vout', 'value', [data.vout / 1e3]);     // millivolt
+					Plotly.restyle('plotly_pout', 'value', [data.pout / 1e6]);     // microwatt
+					Plotly.restyle('plotly_iout', 'value', [data.iout / 1000.0]);  // milliampere
+					Plotly.restyle('plotly_energy', 'value', [data.energy]);       // Joule
+					Plotly.restyle('plotly_pwm', 'value', [data.pwm]);             // counter
+				} else {
+					console.log("Received data, but no valid JSON:");
+					console.log(evt.data);
+				}
+			};
+		</script>
+	</body>
+</html>