diff --git a/.cargo/config.toml b/.cargo/config.toml index da541f4..4f5e39a 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -1,6 +1,6 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] # TODO(2) replace `$CHIP` with your chip's name (see `probe-rs chip list` output) -runner = "probe-rs run --chip $CHIP" +runner = "probe-rs run --chip STM32H743ZITx" rustflags = [ "-C", "linker=flip-link", "-C", "link-arg=-Tlink.x", @@ -17,9 +17,10 @@ rustflags = [ # target = "thumbv6m-none-eabi" # Cortex-M0 and Cortex-M0+ # target = "thumbv7m-none-eabi" # Cortex-M3 # target = "thumbv7em-none-eabi" # Cortex-M4 and Cortex-M7 (no FPU) -# target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) + target = "thumbv7em-none-eabihf" # Cortex-M4F and Cortex-M7F (with FPU) [alias] rb = "run --bin" rrb = "run --release --bin" bbr = "build --release --bin" +rr = "run --release" diff --git a/Cargo.toml b/Cargo.toml index 67fa538..0312de8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,18 +1,21 @@ [package] # TODO fix `authors` and `name` if you didn't use `cargo-generate` -name = "test-app" -edition = "2021" +name = "pid-control" +edition = "2024" version = "0.1.0" +default-run = "pid-control" [dependencies] cortex-m = { version = "0.7", features = ["critical-section-single-core"] } -defmt = { version = "0.3", features = ["encoding-rzcobs"] } -defmt-brtt = { version = "0.1", default-features = false, features = ["rtt"] } -panic-probe = { version = "0.3", features = ["print-defmt"] } +defmt = { version = "1.1.0", features = ["encoding-rzcobs"] } +defmt-rtt = "1.2.0" +embedded-cli = "0.2.1" +fugit = "0.4.0" +panic-probe = { version = "1.0.0", features = ["print-defmt"] } # TODO(4) Select the correct rtic backend -rtic = { version = "2.0.0", features = [ "$RTIC_BACKEND" ] } +rtic = { version = "2.0.0", features = [ "thumbv7-backend" ] } # TODO(5) Add hal as dependency -some-hal = "1.2.3" +stm32h7xx-hal = { version = "0.16.0", features = [ "stm32h743" ] } # TODO add a monotonic if you use scheduling # rtic-monotonics = { version = "1.0.0", features = [ "cortex-m-systick" ]} diff --git a/memory.x b/memory.x new file mode 100644 index 0000000..e87d681 --- /dev/null +++ b/memory.x @@ -0,0 +1,63 @@ +MEMORY +{ + /* FLASH and RAM are mandatory memory regions */ + + /* STM32H742xI/743xI/753xI */ + /* STM32H745xI/747xI/755xI/757xI */ + /* STM32H7A3xI/7B3xI */ + FLASH : ORIGIN = 0x08000000, LENGTH = 2M + + /* STM32H742xG/743xG */ + /* STM32H745xG/STM32H747xG */ + /* STM32H7A3xG */ + /* FLASH : ORIGIN = 0x08000000, LENGTH = 512K */ + /* FLASH1 : ORIGIN = 0x08100000, LENGTH = 512K */ + + /* STM32H750xB */ + /* STM32H7B0 */ + /* FLASH : ORIGIN = 0x08000000, LENGTH = 128K */ + + /* DTCM */ + RAM : ORIGIN = 0x20000000, LENGTH = 128K + + /* AXISRAM */ + AXISRAM : ORIGIN = 0x24000000, LENGTH = 512K + + /* SRAM */ + SRAM1 : ORIGIN = 0x30000000, LENGTH = 128K + SRAM2 : ORIGIN = 0x30020000, LENGTH = 128K + SRAM3 : ORIGIN = 0x30040000, LENGTH = 32K + SRAM4 : ORIGIN = 0x38000000, LENGTH = 64K + + /* Backup SRAM */ + BSRAM : ORIGIN = 0x38800000, LENGTH = 4K + + /* Instruction TCM */ + ITCM : ORIGIN = 0x00000000, LENGTH = 64K +} + +/* The location of the stack can be overridden using the + `_stack_start` symbol. Place the stack at the end of RAM */ +_stack_start = ORIGIN(RAM) + LENGTH(RAM); + +/* The location of the .text section can be overridden using the + `_stext` symbol. By default it will place after .vector_table */ +/* _stext = ORIGIN(FLASH) + 0x40c; */ + +/* These sections are used for some of the examples */ +SECTIONS { + .axisram (NOLOAD) : ALIGN(8) { + *(.axisram .axisram.*); + . = ALIGN(8); + } > AXISRAM + /* The SRAM1 and SRAM2 section are commonly used as the stack and heap for the + CM4 core in dualcore versions and should thus not be used in examples*/ + .sram3 (NOLOAD) : ALIGN(4) { + *(.sram3 .sram3.*); + . = ALIGN(4); + } > SRAM3 + .sram4 (NOLOAD) : ALIGN(4) { + *(.sram4 .sram4.*); + . = ALIGN(4); + } > SRAM4 +}; diff --git a/src/bin/minimal.rs b/src/bin/minimal.rs index ca09ad5..4fede88 100644 --- a/src/bin/minimal.rs +++ b/src/bin/minimal.rs @@ -2,15 +2,12 @@ #![no_std] #![feature(type_alias_impl_trait)] -use test_app as _; // global logger + panicking-behavior + memory layout +use pid_control as _; // global logger + panicking-behavior + memory layout // TODO(7) Configure the `rtic::app` macro #[rtic::app( - // TODO: Replace `some_hal::pac` with the path to the PAC - device = some_hal::pac, - // TODO: Replace the `FreeInterrupt1, ...` with free interrupt vectors if software tasks are used - // You can usually find the names of the interrupt vectors in the some_hal::pac::interrupt enum. - dispatchers = [FreeInterrupt1, ...] + device = stm32h7xx_hal::pac, + dispatchers = [EXTI0] )] mod app { // Shared resources go here diff --git a/src/lib.rs b/src/lib.rs index 3ac29a5..1090c8e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2,12 +2,12 @@ #![no_std] use core::sync::atomic::{AtomicUsize, Ordering}; -use defmt_brtt as _; // global logger +use defmt_rtt as _; // global logger use panic_probe as _; // TODO(6) Import your HAL -use some_hal as _; // memory layout +use stm32h7xx_hal as _; // memory layout // same panicking *behavior* as `panic-probe` but doesn't print a panic message // this prevents the panic message being printed *twice* when `defmt::panic` is invoked diff --git a/src/main.rs b/src/main.rs new file mode 100644 index 0000000..cdc10d8 --- /dev/null +++ b/src/main.rs @@ -0,0 +1,259 @@ +#![no_main] +#![no_std] +#![feature(type_alias_impl_trait)] + +use pid_control as _; // global logger + panicking-behavior + memory layout + +// TODO +// - protect from i windup -> done? +// - uart configurable reference +// - impl timer +#[rtic::app( + device = stm32h7xx_hal::pac, + dispatchers = [EXTI0] +)] +mod app { + use defmt::info; + + use embedded_cli::Command; + + use stm32h7xx_hal::{ + adc::{self, Adc}, + dac::{self, C1}, + gpio::{Analog, Pin, PinExt}, + pac, + prelude::*, + serial::{self, Serial}, + stm32::{ADC1, ADC2, DAC, USART1}, + timer::{Event, Timer}, + traits::DacOut, + }; + + const KP: f32 = 0.15; + const KI: f32 = 0.5; + const KD: f32 = 0f32; + + // Shared resources go here + #[shared] + struct Shared { + target: f32, + } + + // Local resources go here + #[local] + struct Local { + timer3: Timer, + timer2: Timer, + p: f32, + i: f32, + d: f32, + input: Pin<'C', 3, Analog>, + adc1: Adc, + adc2: Adc, + dac: C1, + serial: Serial, + // freq: fugit::Kilohertz, + } + + #[init] + fn init(ctx: init::Context) -> (Shared, Local) { + info!("===============init==============="); + info!("----------Setup PWR----------"); + let pwr = ctx.device.PWR.constrain(); + let pwrcfg = pwr.freeze(); + + info!("----------Setup RCC----------"); + let rcc = ctx.device.RCC.constrain(); + + // Configure clocks + let ccdr = rcc.sys_ck(400.MHz()).freeze(pwrcfg, &ctx.device.SYSCFG); + + let mut delay = ctx.core.SYST.delay(ccdr.clocks); + + info!("----------Setup GPIO---------"); + let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); + let gpioc = ctx.device.GPIOC.split(ccdr.peripheral.GPIOC); + + let reference = gpioc.pc2.into_analog(); + let input = gpioc.pc3.into_analog(); + let output = gpioa.pa4; + + let tx = gpioa.pa9.into_alternate(); + let rx = gpioa.pa10.into_alternate(); + + info!( + "reference input: P{}{}", + (reference.port_id() + b'A') as char, + reference.pin_id() + ); + info!( + "controller input: P{}{}", + (input.port_id() + b'A') as char, + input.pin_id() + ); + info!( + "controller output: P{}{}", + (output.port_id() + b'A') as char, + output.pin_id() + ); + + info!("uart tx: P{}{}", (tx.port_id() + b'A') as char, tx.pin_id()); + info!("uart rx: P{}{}", (rx.port_id() + b'A') as char, rx.pin_id()); + + info!("----------Setup USART--------"); + + let baud_rate = 115_200.bps(); + info!("baudrate: {}", baud_rate.to_Hz()); + + let mut serial = ctx + .device + .USART1 + .serial( + (tx, rx), + 115_200.bps(), + ccdr.peripheral.USART1, + &ccdr.clocks, + ) + .unwrap(); + + info!("----------Setup DAC----------"); + let dac = ctx.device.DAC.dac(output, ccdr.peripheral.DAC12); + + // Calibrate output buffer then enable DAC channel + let dac = dac.calibrate_buffer(&mut delay).enable(); + + info!("----------Setup ADC----------"); + let (adc1, adc2) = adc::adc12( + ctx.device.ADC1, + ctx.device.ADC2, + 4.MHz(), + &mut delay, + ccdr.peripheral.ADC12, + &ccdr.clocks, + ); + + let mut adc1 = adc1.enable(); + adc1.set_resolution(adc::Resolution::TwelveBit); + + let mut adc2 = adc2.enable(); + adc2.set_resolution(adc::Resolution::TwelveBit); + + info!("----------Setup TIM----------"); + + let freq = 10.Hz(); + info!("TIM3:"); + info!("\tfreq: {} Hz", freq.to_Hz()); + + let mut timer3 = ctx + .device + .TIM3 + .timer(freq, ccdr.peripheral.TIM3, &ccdr.clocks); + + let freq = 10.kHz(); + info!("TIM2:"); + info!("\tfreq: {} kHz", freq.to_kHz()); + // Create TIM2 at 10 kHz = 100 µs period + let mut timer2 = ctx + .device + .TIM2 + .timer(freq, ccdr.peripheral.TIM2, &ccdr.clocks); + + info!("===========init complete=========="); + + // Enable interrupts + timer3.listen(Event::TimeOut); // NOTE: comment for digital reference via uart + timer2.listen(Event::TimeOut); + serial.listen(serial::Event::Rxne); + + let target = 0.7 / 3.3 * 4095f32; + + ( + Shared { target }, + Local { + timer3, + timer2, + p: 0f32, + i: 0f32, + d: 0f32, + input, + adc1, + adc2, + dac, + serial, + // TODO: freq, + }, + ) + } + + #[derive(Debug, Command)] + enum TargetCommand { + /// Set Voltage + Voltage { voltage: f32 }, + /// Set Value + Value { val: u32 }, + } + + #[derive(Debug, Command)] + enum BaseCommand { + /// Set controller target + Target { + #[command(subcommand)] + command: TargetCommand, + }, + } + + #[task(binds = USART1, local = [serial], shared = [target])] + fn uart_target(ctx: uart_target::Context) { + // TODO: Do I need to clear the interrupt flag? + todo!() // TODO: use more embedded-cli + } + + #[task(binds = TIM3, local = [timer3, adc1], shared = [target])] + fn adc_target(ctx: adc_target::Context) { + // Clear interrupt flag + ctx.local.timer3.clear_irq(); + } + + #[task(binds = TIM2, local = [timer2, p, i, d, input, adc2, dac], shared = [target], priority = 1)] + fn pid(mut ctx: pid::Context) { + // Clear interrupt flag + ctx.local.timer2.clear_irq(); + + // TODO get sensible T + let t = 1.0; + + let input: u32 = ctx.local.adc2.read(ctx.local.input).unwrap(); // WHY COMPILER WHY + let input: f32 = input as f32; + + let p = ctx.local.p; + let i = ctx.local.i; + let d = ctx.local.d; + let target: f32 = ctx.shared.target.lock(|t| *t); + + let last_p = *p; // last error + let last_i = *i; // NOTE: to protect from I windup + + *p = target - input; + *i += *p * t; + *d = (*p - last_p) / t; + + let mut output = (KP * *p + KI * *i + KD * *d) as i32; + + // TODO? limit to control voltage of attenuation curve + // limit output to output ∈ [0V..3V3] + if output < 0 { + output = 0; + *i = last_i + } else if output > 4095 { + output = 4095; + *i = last_i; + } + + ctx.local.dac.set_value(output as u16); + + info!( + "target: {:05},\tinput: {:05},\toutput: {:05},\tp: {:05},\ti: {:05},\td: {:05}", + target, input, output, p, i, d + ); + } +}