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
parent
2f8516cce7
commit
d50f2cd723
22
Cargo.toml
22
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"
|
||||
|
|
|
|||
284
src/main.rs
284
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_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));
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue