From d50f2cd723a05b886ed0506202d86dd92ab8f21a Mon Sep 17 00:00:00 2001 From: Thomas Kolb Date: Sun, 15 Jan 2023 00:47:55 +0100 Subject: [PATCH] Make the timing more predictable MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 | 22 +--- src/main.rs | 284 ++++++++++++++++++++++++++++++++++++---------------- 2 files changed, 198 insertions(+), 108 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 55833c2..bdc4444 100644 --- a/Cargo.toml +++ b/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" diff --git a/src/main.rs b/src/main.rs index e171dbc..6c424f7 100644 --- a/src/main.rs +++ b/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>> = 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_y = pins.gpio14.into_push_pull_output(); - let mut pin_led_g = pins.gpio15.into_push_pull_output(); + 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(); - pin_led_r.set_high().unwrap(); - pin_led_y.set_high().unwrap(); - pin_led_g.set_high().unwrap(); + // 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(); @@ -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,91 +227,159 @@ fn main() -> ! { const T: i32 = 1; + let mut loopcnt: u64 = 0; + + pin_led_r.set_high().unwrap(); + // main loop loop { - let mut adc_value: [u16; 4] = [0; 4]; + // only execute the main loop if the systick has expired + let mut systick_received = false; - (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); + critical_section::with(|_cs| unsafe { + systick_received = core::ptr::read_volatile(&SYSTICK_FLAG); + }); - let (vin, vout, iout) = convert_adc_measurements(&adc_value); + if systick_received { + pin_led_g.set_high().unwrap(); - // PID controller - let err: i32 = -(vout as i32 - vtarget); + // re-shedule the systick alarm + critical_section::with(|_cs| unsafe { + core::ptr::write_volatile(&mut SYSTICK_FLAG, false); + }); - iaccu += igain * T * err; + next_systick_instant += SYSTICK_INTERVAL_US; - // limit iaccu - if iaccu > SWITCH_PWM_LIMIT * GAINSCALE { - iaccu = SWITCH_PWM_LIMIT * GAINSCALE; - } else if iaccu < 0 { - iaccu = 0; - } + 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"), + }; + } + } - 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; - } - - pwr_switch_ch.set_duty(pwmval as u16); - - - { - let mut data: String<16>; - - uart.write_full_blocking(b"Vin: "); - data = String::from(vin); - uart.write_full_blocking(data.as_bytes()); - uart.write_full_blocking(b"mV - Vout: "); - data = String::from(vout); - uart.write_full_blocking(data.as_bytes()); - uart.write_full_blocking(b"mV - Iout: "); - data = String::from(iout); - uart.write_full_blocking(data.as_bytes()); - - uart.write_full_blocking(b"mA - raw: ["); - - for i in 0..adc_value.len() { - data = String::from(adc_value[i]); - uart.write_full_blocking(data.as_bytes()); - - if i < 3 { - uart.write_full_blocking(b", "); + 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); + (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); + + // 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; } - uart.write_full_blocking(b"]; e="); - data = String::from(err); - uart.write_full_blocking(data.as_bytes()); - uart.write_full_blocking(b" => "); - data = String::from(pval); - uart.write_full_blocking(data.as_bytes()); - uart.write_full_blocking(b"+"); - data = String::from(ival); - uart.write_full_blocking(data.as_bytes()); - uart.write_full_blocking(b"+"); - data = String::from(dval); - uart.write_full_blocking(data.as_bytes()); - uart.write_full_blocking(b" = "); - data = String::from(ctrlout); - uart.write_full_blocking(data.as_bytes()); + let pval = pgain * err / GAINSCALE; + let ival = iaccu / GAINSCALE; + let dval = dgain * (err - elast) / GAINSCALE; - uart.write_full_blocking(b"\r\n"); + 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; + } + + pwr_switch_ch.set_duty(pwmval as u16); + + // do not output status data every loop + if loopcnt % 500 == 250 { + 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 - 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"]; 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(data.as_bytes()).unwrap(); + + 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)); + } + } + }); +}