From 30f3cc2885251f42a938addf2196b4c7224242e5 Mon Sep 17 00:00:00 2001 From: Caleb Fletcher Date: Fri, 6 Dec 2024 22:25:47 +1100 Subject: [PATCH 1/2] feat(gui): read 2 motor phase current channels onto gui --- gui/firmware/src/main.rs | 39 +++++++++++++++++++++++++++++++++++---- gui/gui/src/connection.rs | 4 ++++ gui/gui/src/main.rs | 10 ++++++++++ gui/icd/src/lib.rs | 1 + 4 files changed, 50 insertions(+), 4 deletions(-) diff --git a/gui/firmware/src/main.rs b/gui/firmware/src/main.rs index 5bddc3b..18a8397 100644 --- a/gui/firmware/src/main.rs +++ b/gui/firmware/src/main.rs @@ -18,6 +18,10 @@ mod app { use icd::Endpoint; use rtt_target::{rtt_init, ChannelMode}; use stm32g4xx_hal::{ + adc::{ + config::{AdcConfig, SampleTime}, + Adc, AdcClaim, ClockSource, Configured, + }, cordic::{ self, func::dynamic::{Any, Mode as _}, @@ -25,15 +29,17 @@ mod app { types::Q31, Cordic, Ext, }, + delay::SYSTDelayExt as _, gpio::{ + gpioa::{PA0, PA1}, gpioc::{PC10, PC11, PC12, PC15, PC9}, - Alternate, GpioExt as _, Output, PushPull, AF6, + Alternate, Analog, GpioExt as _, Output, PushPull, AF6, }, prelude::OutputPin as _, pwr::PwrExt as _, rcc::{Config, PllMDiv, PllNMul, PllRDiv, PllSrc, RccExt as _}, spi::{Spi, SpiExt as _}, - stm32::SPI3, + stm32::{ADC1, SPI3}, time::{ExtU32 as _, RateExtU32 as _}, timer::{CountDownTimer, Event, Timer}, }; @@ -60,6 +66,9 @@ mod app { timer_handler: CountDownTimer, cordic: DynamicCordic, encoder: AS5048A>>, + motor_current_a: PA0, + motor_current_b: PA1, + adc: Adc, } #[init] @@ -101,6 +110,7 @@ mod app { let cordic = cx.device.CORDIC.constrain(&mut rcc).into_dynamic(); + let gpioa = cx.device.GPIOA.split(&mut rcc); let gpioc = cx.device.GPIOC.split(&mut rcc); let cs0 = gpioc.pc9.into_push_pull_output(); @@ -121,6 +131,17 @@ mod app { let encoder = AS5048A::new(spi, cs0); + let mut delay = cx.core.SYST.delay(&rcc.clocks); + let adc = cx.device.ADC1.claim_and_configure( + ClockSource::SystemClock, + &rcc, + AdcConfig::default(), + &mut delay, + false, + ); + let motor_current_a = gpioa.pa0.into_analog(); + let motor_current_b = gpioa.pa1.into_analog(); + let led = gpioc.pc15.into_push_pull_output(); let timer2 = Timer::new(cx.device.TIM2, &rcc.clocks); @@ -138,6 +159,9 @@ mod app { timer_handler: timer2, cordic, encoder, + adc, + motor_current_a, + motor_current_b, }, ) } @@ -154,7 +178,7 @@ mod app { cx.local.timer_handler.clear_interrupt(Event::TimeOut); } - #[task(local = [rpc_channel, cordic, encoder])] + #[task(local = [rpc_channel, cordic, encoder, adc, motor_current_a, motor_current_b])] async fn rtt_rpc(cx: rtt_rpc::Context) { let mut rx_buffer_raw = [0; 64]; let mut rx_buffer_frame = [0; 64]; @@ -178,7 +202,7 @@ mod app { icd::generate_endpoint_handler! { frame, tx_buffer, (icd::PingEndpoint, ping) - (icd::ReadEndpoint, |_: ()| -> u32 { stored_value }) + (icd::ReadEndpoint, |_: ()| { stored_value }) (icd::WriteEndpoint, |value: u32| { stored_value = value; }) (icd::SinCosEndpoint, |value: f32| { let (sin, cos) = cx.local.cordic.run::(I1F31::from_num(value)); @@ -187,6 +211,13 @@ mod app { (icd::EncoderAngle, |_: ()| { cx.local.encoder.angle().unwrap() }) + (icd::MotorPhaseCurrents, |_: ()| { + let current_a_raw = cx.local.adc.convert(cx.local.motor_current_a, SampleTime::Cycles_640_5); + let current_a_mv = cx.local.adc.sample_to_millivolts(current_a_raw); + let current_b_raw = cx.local.adc.convert(cx.local.motor_current_b, SampleTime::Cycles_640_5); + let current_b_mv = cx.local.adc.sample_to_millivolts(current_b_raw); + [current_a_mv, current_b_mv] + }) } }; diff --git a/gui/gui/src/connection.rs b/gui/gui/src/connection.rs index a608107..3becfd2 100644 --- a/gui/gui/src/connection.rs +++ b/gui/gui/src/connection.rs @@ -60,6 +60,10 @@ impl Device { pub fn encoder_angle(&mut self) -> Result { self.rpc_channel.call::(()) } + + pub fn motor_currents(&mut self) -> Result<[u16; 2], anyhow::Error> { + self.rpc_channel.call::(()) + } } struct RpcChannel { diff --git a/gui/gui/src/main.rs b/gui/gui/src/main.rs index ef0e9d9..da3b8c7 100644 --- a/gui/gui/src/main.rs +++ b/gui/gui/src/main.rs @@ -28,6 +28,7 @@ struct Behaviour { value_to_write: u32, angle: f32, sin_cos: Option<(f32, f32)>, + currents: Option<[u16; 2]>, } impl Behaviour { @@ -54,6 +55,8 @@ impl Behaviour { self.motor_state.mechanical_angle_rad = device.encoder_angle().unwrap() as f32 / 2f32.powi(14); + + self.currents = Some(device.motor_currents().unwrap()); } } } @@ -225,6 +228,12 @@ impl egui_tiles::Behavior for Behaviour { ui.label(format!("Cos: {cos:.3}")); } }); + if let Some([a, b]) = self.currents { + ui.horizontal(|ui| { + ui.label(format!("A: {a:.3}")); + ui.label(format!("B: {b:.3}")); + }); + } }); } @@ -377,6 +386,7 @@ fn main() -> Result<(), eframe::Error> { value_to_write: 0, angle: 0., sin_cos: None, + currents: None, }; behavior.update_probe_list(); diff --git a/gui/icd/src/lib.rs b/gui/icd/src/lib.rs index 10f1e10..c31171d 100644 --- a/gui/icd/src/lib.rs +++ b/gui/icd/src/lib.rs @@ -53,3 +53,4 @@ endpoint!(1, ReadEndpoint, (), u32); endpoint!(2, WriteEndpoint, u32, ()); endpoint!(3, SinCosEndpoint, f32, (f32, f32)); endpoint!(4, EncoderAngle, (), u16); +endpoint!(5, MotorPhaseCurrents, (), [u16; 2]); From 7f0fbbc303a1ce79495c78c3d613bbd65abdcf14 Mon Sep 17 00:00:00 2001 From: Caleb Fletcher Date: Fri, 6 Dec 2024 23:59:42 +1100 Subject: [PATCH 2/2] perf(gui): batch adc reads into a single sequence --- gui/firmware/src/main.rs | 50 ++++++++++++++++++++++------------------ 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/gui/firmware/src/main.rs b/gui/firmware/src/main.rs index 18a8397..8df705e 100644 --- a/gui/firmware/src/main.rs +++ b/gui/firmware/src/main.rs @@ -19,7 +19,7 @@ mod app { use rtt_target::{rtt_init, ChannelMode}; use stm32g4xx_hal::{ adc::{ - config::{AdcConfig, SampleTime}, + config::{Continuous, SampleTime, Sequence}, Adc, AdcClaim, ClockSource, Configured, }, cordic::{ @@ -31,9 +31,8 @@ mod app { }, delay::SYSTDelayExt as _, gpio::{ - gpioa::{PA0, PA1}, gpioc::{PC10, PC11, PC12, PC15, PC9}, - Alternate, Analog, GpioExt as _, Output, PushPull, AF6, + Alternate, GpioExt as _, Output, PushPull, AF6, }, prelude::OutputPin as _, pwr::PwrExt as _, @@ -66,9 +65,7 @@ mod app { timer_handler: CountDownTimer, cordic: DynamicCordic, encoder: AS5048A>>, - motor_current_a: PA0, - motor_current_b: PA1, - adc: Adc, + adc: Option>, } #[init] @@ -131,17 +128,23 @@ mod app { let encoder = AS5048A::new(spi, cs0); - let mut delay = cx.core.SYST.delay(&rcc.clocks); - let adc = cx.device.ADC1.claim_and_configure( - ClockSource::SystemClock, - &rcc, - AdcConfig::default(), - &mut delay, - false, - ); + // These don't implement drop, so it should be fine to drop them but + // still use the ADC let motor_current_a = gpioa.pa0.into_analog(); let motor_current_b = gpioa.pa1.into_analog(); + let mut delay = cx.core.SYST.delay(&rcc.clocks); + let mut adc = cx + .device + .ADC1 + .claim(ClockSource::SystemClock, &rcc, &mut delay, false); + + adc.set_continuous(Continuous::Single); + adc.reset_sequence(); + adc.configure_channel(&motor_current_a, Sequence::One, SampleTime::Cycles_640_5); + adc.configure_channel(&motor_current_b, Sequence::Two, SampleTime::Cycles_640_5); + let adc = adc.enable(); + let led = gpioc.pc15.into_push_pull_output(); let timer2 = Timer::new(cx.device.TIM2, &rcc.clocks); @@ -159,9 +162,7 @@ mod app { timer_handler: timer2, cordic, encoder, - adc, - motor_current_a, - motor_current_b, + adc: Some(adc), }, ) } @@ -178,7 +179,7 @@ mod app { cx.local.timer_handler.clear_interrupt(Event::TimeOut); } - #[task(local = [rpc_channel, cordic, encoder, adc, motor_current_a, motor_current_b])] + #[task(local = [rpc_channel, cordic, encoder, adc])] async fn rtt_rpc(cx: rtt_rpc::Context) { let mut rx_buffer_raw = [0; 64]; let mut rx_buffer_frame = [0; 64]; @@ -212,10 +213,15 @@ mod app { cx.local.encoder.angle().unwrap() }) (icd::MotorPhaseCurrents, |_: ()| { - let current_a_raw = cx.local.adc.convert(cx.local.motor_current_a, SampleTime::Cycles_640_5); - let current_a_mv = cx.local.adc.sample_to_millivolts(current_a_raw); - let current_b_raw = cx.local.adc.convert(cx.local.motor_current_b, SampleTime::Cycles_640_5); - let current_b_mv = cx.local.adc.sample_to_millivolts(current_b_raw); + let adc = cx.local.adc.take().unwrap().start_conversion(); + + let adc = adc.wait_for_conversion_sequence().unwrap_active(); + let current_a_mv = adc.sample_to_millivolts(adc.current_sample()); + let adc = adc.wait_for_conversion_sequence().unwrap_stopped(); + let current_b_mv = adc.sample_to_millivolts(adc.current_sample()); + + *cx.local.adc = Some(adc); + [current_a_mv, current_b_mv] }) }