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.
This commit is contained in:
Thomas Kolb 2023-01-15 00:47:55 +01:00
parent 2f8516cce7
commit d50f2cd723
2 changed files with 198 additions and 108 deletions

View file

@ -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"

View file

@ -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_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));
}
}
});
}