Make the timing more predictable

This implements multiple measures for a more consistent control loop:

- Use Alarm0 to generate a trigger signal every millisecond. This
  ensures that the loop’s update rate is constant.
- Introduces the Logger module to handle UART communication in a
  non-blocking way (no interrupts, though). The Logger module has a
  internal, statically allocated queue to accomplish this.

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/main.rs

@@ -8,6 +8,7 @@
 #![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 _;
@@ -15,12 +16,6 @@ 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;
@@ -33,7 +28,22 @@ 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;
+
+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.
@@ -43,6 +53,13 @@ mod ext_adc;
 #[used]
 pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H;
 
+const SYSTICK_INTERVAL_US: MicrosDurationU32 = MicrosDurationU32::millis(1);
+
+static mut SYSTICK_ALARM: Mutex<RefCell<Option<hal::timer::Alarm0>>> = Mutex::new(RefCell::new(None));
+
+// 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;
@@ -70,7 +87,6 @@ fn convert_adc_measurements(raw: &[u16; 4]) -> (u32, u32, u32)
 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);
@@ -119,6 +135,10 @@ fn main() -> ! {
         )
         .unwrap();
 
+    // initialize the logger
+    let mut logger: Logger<_, _, 256> = Logger::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);
 
@@ -142,13 +162,14 @@ fn main() -> ! {
     pwr_switch_ch.output_to(pins.gpio10);
 
     // LED pins
-    let mut pin_led_r = pins.gpio13.into_push_pull_output();
+    let mut pin_led_r = pins.gpio15.into_push_pull_output();
-    let mut pin_led_y = pins.gpio14.into_push_pull_output();
+    let mut pin_led_y = pins.gpio13.into_push_pull_output();
-    let mut pin_led_g = pins.gpio15.into_push_pull_output();
+    let mut pin_led_g = pins.gpio14.into_push_pull_output();
 
-    pin_led_r.set_high().unwrap();
+    // off by default
-    pin_led_y.set_high().unwrap();
+    pin_led_r.set_low().unwrap();
-    pin_led_g.set_high().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();
@@ -171,7 +192,28 @@ fn main() -> ! {
         spi_cs
         );
 
-    uart.write_full_blocking(b"Initialization complete!\r\n");
+    logger.log(b"Initialization complete!\r\n").unwrap();
+
+    // initialize the timer
+    let mut timer = hal::Timer::new(pac.TIMER, &mut pac.RESETS);
+
+    // 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();
+
+    systick_alarm.schedule_at(next_systick_instant).unwrap();
+    systick_alarm.enable_interrupt();
+
+    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);
+    }
 
     let vtarget: i32 =  8000;
 
@@ -185,8 +227,50 @@ fn main() -> ! {
 
     const T: i32 = 1;
 
+    let mut loopcnt: u64 = 0;
+
+    pin_led_r.set_high().unwrap();
+
     // main loop
     loop {
+        // only execute the main loop if the systick has expired
+        let mut systick_received = false;
+
+        critical_section::with(|_cs| unsafe {
+            systick_received = core::ptr::read_volatile(&SYSTICK_FLAG);
+        });
+
+        if systick_received {
+            pin_led_g.set_high().unwrap();
+
+            // 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"),
+                                };
+                            }
+                        }
+
+                        SYSTICK_ALARM.borrow(cs).replace(Some(systick_alarm));
+                    } else {
+                        logger.log_fatal(b"Systick: object not set when it should be!\r\n");
+                    }
+                }
+            });
+
+
             let mut adc_value: [u16; 4] = [0; 4];
 
             (adc_ctrl, adc_value[0]) = adc_ctrl.sample_adc_channel(0);
@@ -228,48 +312,74 @@ 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>;
 
-            uart.write_full_blocking(b"Vin: ");
+                logger.log(b"Vin: ").unwrap();
                 data = String::from(vin);
-            uart.write_full_blocking(data.as_bytes());
+                logger.log(data.as_bytes()).unwrap();
-            uart.write_full_blocking(b"mV - Vout: ");
+                logger.log(b"mV - Vout: ").unwrap();
                 data = String::from(vout);
-            uart.write_full_blocking(data.as_bytes());
+                logger.log(data.as_bytes()).unwrap();
-            uart.write_full_blocking(b"mV - Iout: ");
+                logger.log(b"mV - Iout: ").unwrap();
                 data = String::from(iout);
-            uart.write_full_blocking(data.as_bytes());
+                logger.log(data.as_bytes()).unwrap();
 
-            uart.write_full_blocking(b"mA - raw: [");
+                logger.log(b"mA - raw: [").unwrap();
 
                 for i in 0..adc_value.len() {
                     data = String::from(adc_value[i]);
-                uart.write_full_blocking(data.as_bytes());
+                    logger.log(data.as_bytes()).unwrap();
 
                     if i < 3 {
-                    uart.write_full_blocking(b", ");
+                        logger.log(b", ").unwrap();
                     }
                 }
 
-            uart.write_full_blocking(b"]; e=");
+                logger.log(b"]; e=").unwrap();
                 data = String::from(err);
-            uart.write_full_blocking(data.as_bytes());
+                logger.log(data.as_bytes()).unwrap();
-            uart.write_full_blocking(b" => ");
+                logger.log(b" => ").unwrap();
                 data = String::from(pval);
-            uart.write_full_blocking(data.as_bytes());
+                logger.log(data.as_bytes()).unwrap();
-            uart.write_full_blocking(b"+");
+                logger.log(b"+").unwrap();
                 data = String::from(ival);
-            uart.write_full_blocking(data.as_bytes());
+                logger.log(data.as_bytes()).unwrap();
-            uart.write_full_blocking(b"+");
+                logger.log(b"+").unwrap();
                 data = String::from(dval);
-            uart.write_full_blocking(data.as_bytes());
+                logger.log(data.as_bytes()).unwrap();
-            uart.write_full_blocking(b" = ");
+                logger.log(b" = ").unwrap();
                 data = String::from(ctrlout);
-            uart.write_full_blocking(data.as_bytes());
+                logger.log(data.as_bytes()).unwrap();
 
-            uart.write_full_blocking(b"\r\n");
+                logger.log(b"\r\n").unwrap();
+            }
+
+            loopcnt += 1;
+        }
+
+        logger.process().unwrap();
+
+        pin_led_y.set_high().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));
+            }
+        }
+    });
 }