From 415b8bb48df068dfbfc6b855b1b7d7043235f594 Mon Sep 17 00:00:00 2001 From: Michael Kamprath Date: Thu, 28 Nov 2024 16:07:54 -0800 Subject: [PATCH 1/4] first semi-working wip for core1 --- .cargo/config.toml | 2 +- src/driver.rs | 22 +- src/main.rs | 72 ++--- src/model/heading.rs | 7 +- src/model/heading_core1.rs | 262 +++++++++++++++++++ src/model/mod.rs | 3 +- src/robot.rs | 140 ++++++---- src/robot/file_storage.rs | 15 +- src/robot/file_storage/logger.rs | 18 +- src/robot/file_storage/sd_card_spi_device.rs | 144 +++++----- src/robot/file_storage/sd_file.rs | 16 +- 11 files changed, 502 insertions(+), 199 deletions(-) create mode 100644 src/model/heading_core1.rs diff --git a/.cargo/config.toml b/.cargo/config.toml index 19590b7..6ce3637 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -27,4 +27,4 @@ rustflags = [ target = "thumbv6m-none-eabi" [env] -DEFMT_LOG = "embedded_sdmmc=info,debug" +DEFMT_LOG = "embedded_sdmmc=debug,debug" diff --git a/src/driver.rs b/src/driver.rs index a8db4ca..ef56f20 100644 --- a/src/driver.rs +++ b/src/driver.rs @@ -31,13 +31,12 @@ pub struct Driver< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0: I2c, TWI1: I2c, SPI_DEV: SpiDevice, DELAY: DelayNs + Clone, LED1: OutputPin, > { - robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY>, + robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY>, delay: DELAY, led1: LED1, selected_path: Option, @@ -53,29 +52,14 @@ impl< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0: I2c, TWI1: I2c, SPI_DEV: SpiDevice, DELAY: DelayNs + Clone, LED1: OutputPin, - > Driver<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY, LED1> + > Driver<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY, LED1> { pub fn new( - robot: Robot< - 'a, - INA1, - INA2, - INB1, - INB2, - ENA, - ENB, - BUTT1, - BUTT2, - TWI0, - TWI1, - SPI_DEV, - DELAY, - >, + robot: Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY>, delay: DELAY, led1_pin: LED1, ) -> Self { diff --git a/src/main.rs b/src/main.rs index 554cb33..145f7cf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -24,7 +24,7 @@ use bsp::{ }, }; use driver::Driver; -use rp_pico as bsp; +use rp_pico::{self as bsp, hal::multicore::Multicore}; use system::millis::init_millis; extern crate alloc; @@ -46,7 +46,7 @@ fn main() -> ! { let mut pac = pac::Peripherals::take().unwrap(); let _core = pac::CorePeripherals::take().unwrap(); let mut watchdog = Watchdog::new(pac.WATCHDOG); - let sio = Sio::new(pac.SIO); + let mut sio = Sio::new(pac.SIO); // External high-speed crystal on the pico board is 12Mhz let external_xtal_freq_hz = 12_000_000u32; @@ -61,6 +61,7 @@ fn main() -> ! { ) .ok() .unwrap(); + let sys_freq = clocks.system_clock.freq(); let pins = bsp::Pins::new( pac.IO_BANK0, @@ -94,16 +95,16 @@ fn main() -> ! { let sda1_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio6.reconfigure(); let scl1_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio7.reconfigure(); // set up I2C - let i2c0 = bsp::hal::I2C::new_controller( - pac.I2C0, - sda0_pin, - scl0_pin, - HertzU32::from_raw(400_000), - &mut pac.RESETS, - clocks.system_clock.freq(), - ); - let i2c0_ref_cell = RefCell::new(i2c0); - let i2c0_mutex = critical_section::Mutex::new(i2c0_ref_cell); + // let i2c0 = bsp::hal::I2C::new_controller( + // pac.I2C0, + // sda0_pin, + // scl0_pin, + // HertzU32::from_raw(400_000), + // &mut pac.RESETS, + // clocks.system_clock.freq(), + // ); + // let i2c0_ref_cell = RefCell::new(i2c0); + // let i2c0_mutex = critical_section::Mutex::new(i2c0_ref_cell); let i2c1 = bsp::hal::I2C::new_controller( pac.I2C1, @@ -130,30 +131,30 @@ fn main() -> ! { HertzU32::from_raw(400_000), // card initialization happens at low baud rate embedded_hal::spi::MODE_0, ); + let spi_mutex = critical_section::Mutex::new(RefCell::new(spi)); - let sd = crate::robot::file_storage::FileStorage::new( - crate::robot::file_storage::sd_card_spi_device::SDCardSPIDevice::new( - spi, - pins.gpio1.into_push_pull_output(), - timer, - ), - timer, + let sd_card_device = embedded_hal_bus::spi::CriticalSectionDevice::new( + &spi_mutex, + pins.gpio1.into_push_pull_output(), + timer.clone(), ); + let sd = crate::robot::file_storage::FileStorage::new(sd_card_device, timer.clone()); // If SD card is successfully initialized, we can increase the SPI speed - match sd.spi(|spi| { - spi.bus.set_baudrate( - clocks.peripheral_clock.freq(), - HertzU32::from_raw(16_000_000), - ) - }) { - Some(speed) => { - info!("SPI speed increased to {}", speed.raw()); - } - None => { - error!("Error increasing SPI speed"); - } - } + info!( + "SPI speed increased to {}", + critical_section::with(|cs| { + spi_mutex.borrow_ref_mut(cs).set_baudrate( + clocks.peripheral_clock.freq(), + HertzU32::from_raw(16_000_000), + ) + }) + .raw() + ); + // get core1 ready + let mut mc = Multicore::new(&mut pac.PSM, &mut pac.PPB, &mut sio.fifo); + let cores = mc.cores(); + let core1 = &mut cores[1]; let mut robot = Robot::new( pins.gpio10.into_push_pull_output(), @@ -164,16 +165,19 @@ fn main() -> ! { channel_b, pins.gpio14.into_pull_up_input(), pins.gpio15.into_pull_up_input(), - &i2c0_mutex, &i2c1_mutex, pins.gpio21.into_pull_up_input(), pins.gpio20.into_pull_up_input(), sd, &mut timer, + core1, + sys_freq, + sda0_pin, + scl0_pin, ); if let Err(e) = robot.init() { - panic!("Error initializing SD card: {:?}", e); + panic!("Error initializing Robot: {:?}", e); } let mut driver = Driver::new(robot, timer, pins.led.into_push_pull_output()); diff --git a/src/model/heading.rs b/src/model/heading.rs index 38a83f8..c3a6cfd 100644 --- a/src/model/heading.rs +++ b/src/model/heading.rs @@ -6,6 +6,7 @@ use defmt::{error, info}; use embedded_hal::{delay::DelayNs, i2c::I2c}; use micromath::vector::Vector3d; use mpu6050::Mpu6050; +use rp_pico::hal::multicore::Stack; // My MCU6050 calibration values // .. XAccel YAccel ZAccel XGyro YGyro ZGyro @@ -28,7 +29,11 @@ impl HeadingCalculator where TWI: I2c, { - pub fn new(i2c: TWI, timer: &mut DELAY) -> Self + pub fn new( + i2c: TWI, + timer: &mut DELAY, + core1: &mut rp_pico::hal::multicore::Core, + ) -> Self where DELAY: DelayNs, { diff --git a/src/model/heading_core1.rs b/src/model/heading_core1.rs new file mode 100644 index 0000000..71a91f6 --- /dev/null +++ b/src/model/heading_core1.rs @@ -0,0 +1,262 @@ +use bsp::hal::{ + fugit::{HertzU32, Rate}, + gpio::{ + bank0::{Gpio4, Gpio5}, + FunctionI2C, Pin, PullUp, + }, + multicore::Stack, + pac, + sio::Sio, + Timer, +}; +use core::cell::{Cell, RefCell}; +use critical_section::Mutex; +use embedded_hal::i2c::{self, I2c}; +use mpu6050::Mpu6050; +use rp_pico as bsp; + +const HEADING_UPDATE_MICROS: u32 = 2 * 1000; // 2ms + +pub enum HeadingTaskCommands { + Shutdown, + Status, + SetOffsets, + Unknown, +} + +impl Into for HeadingTaskCommands { + fn into(self) -> u32 { + match self { + HeadingTaskCommands::Shutdown => 0xFFFFFFFF, + HeadingTaskCommands::Status => 1, + HeadingTaskCommands::SetOffsets => 2, + HeadingTaskCommands::Unknown => 0, + } + } +} + +impl From for HeadingTaskCommands { + fn from(val: u32) -> Self { + match val { + 0xFFFFFFFF => HeadingTaskCommands::Shutdown, + 1 => HeadingTaskCommands::Status, + 2 => HeadingTaskCommands::SetOffsets, + _ => HeadingTaskCommands::Unknown, + } + } +} + +pub enum HeadingTaskStatus { + CalculatingHeading, + Calibrating, + Done, + Unknown, +} + +impl Into for HeadingTaskStatus { + fn into(self) -> u32 { + match self { + HeadingTaskStatus::CalculatingHeading => 1, + HeadingTaskStatus::Calibrating => 2, + HeadingTaskStatus::Done => 0xFFFFFFFF, + HeadingTaskStatus::Unknown => 0, + } + } +} + +impl From for HeadingTaskStatus { + fn from(val: u32) -> Self { + match val { + 1 => HeadingTaskStatus::CalculatingHeading, + 2 => HeadingTaskStatus::Calibrating, + 0xFFFFFFFF => HeadingTaskStatus::Done, + _ => HeadingTaskStatus::Unknown, + } + } +} + +/// Stack for core 1 +/// +/// Core 0 gets its stack via the normal route - any memory not used by static +/// values is reserved for stack and initialised by cortex-m-rt. +/// To get the same for Core 1, we would need to compile everything separately +/// and modify the linker file for both programs, and that's quite annoying. +/// So instead, core1.spawn takes a [usize] which gets used for the stack. +/// NOTE: We use the `Stack` struct here to ensure that it has 32-byte +/// alignment, which allows the stack guard to take up the least amount of +/// usable RAM. +static mut CORE1_STACK: Stack<4096> = Stack::new(); + +// The basic design for dedicating core1 to heading calculation +// +// There are two tasks that core1 will be responsible for: +// 1. Reading the gyro sensor and calculating the heading +// 2. Calibrating the gyro on demand +// +// The issue is each are seperate tasks, so we will need to communication from core0 to core1 +// when it needs to stop the heading calculation and start the calibration. This will be done +// by sending a sentinal value from core0 to core1 using the SIO FIFO. The sentinal value for +// shutting down a task will be `HeadingTaskCommands::Shutdown`. When a task receives this value, it +// will close up any critical sections and enter into a busy wait loop it only listens to +// the SIO FIFO for a status request, `HeadingTaskCommands::Status`. When it receives this value +// it will send back the current status of the task, which will be `HeadingTaskCommands::Shutdown`. +// To start a new task, whether it be the same task or a different one, core0 will simply +// spawn the task again, which shuts down the current task and starts the new one. +// +// ** Calculating the heading task ** +// The task for calculating heading will get the updated gyro readings from the sensor at +// a fixed interval, calculate the heading, and the setting it to a global variable that +// is wrapped in a Mutex. A critical section will be used to access the global variable. +// While running the task will monitor the SIO FIFO for either a shutdown or status request. +// If it receives a shutdown, it will close up any critical sections and enter into a busy +// wait loop. If it receives a status request, it will send back the current status of the +// task, which will be either TASK_HEADING_CALCULATING = 0xFFFFFFFD or TASK_DONE = 0xFFFFFFFF. +// +// ** Calibrating the gyro task ** + +static HEADING_VALUE: Mutex> = Mutex::new(Cell::new(HeadingData { + heading: 0.0, + last_update_rate: 0.0, + last_update_ticks: 0, +})); + +#[derive(Debug, Clone, Copy)] +struct HeadingData { + heading: f64, + last_update_rate: f64, + last_update_ticks: u64, +} +pub struct HeadingManager<'a> { + core1: &'a mut rp_pico::hal::multicore::Core<'a>, + sys_freq: Rate, +} + +impl<'a> HeadingManager<'a> { + pub fn new( + core1: &'a mut rp_pico::hal::multicore::Core<'a>, + sys_freq: Rate, + ) -> Self { + Self { + core1: core1, + sys_freq: sys_freq, + } + } + + pub fn start_heading_calculation( + &mut self, + timer: Timer, + i2c0_sda_pin: Pin, + i2c0_scl_pin: Pin, + ) { + // spawn the heading calculation task on core1 + let sys_freq = self.sys_freq; // value that can be moved into the closure + let mut timer = timer.clone(); + let res = self.core1.spawn(unsafe { &mut CORE1_STACK.mem }, move || { + // do the initialization in a critical section + // set up environment + let mut pac = unsafe { pac::Peripherals::steal() }; + let core = unsafe { pac::CorePeripherals::steal() }; + let mut sio = Sio::new(pac.SIO); + + let mut delay = cortex_m::delay::Delay::new(core.SYST, sys_freq.to_Hz()); + + // let sda0_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio4.reconfigure(); + // let scl0_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio5.reconfigure(); + let i2c0 = bsp::hal::I2C::new_controller( + pac.I2C0, + i2c0_sda_pin, + i2c0_scl_pin, + HertzU32::from_raw(400_000), + &mut pac.RESETS, + sys_freq, + ); + let i2c0_ref_cell = RefCell::new(i2c0); + let i2c0_mutex = critical_section::Mutex::new(i2c0_ref_cell); + + // set up the gyro sensor ] + let mut gyro = Mpu6050::new(embedded_hal_bus::i2c::CriticalSectionDevice::new( + &i2c0_mutex, + )); + if let Err(_e) = gyro.init(&mut timer) { + panic!("Error initializing gyro"); + } + // start the heading calculation task + loop { + // get the current gyro readings + // calculate the heading + // set the heading to the global variable + // wait for the next update interval + // check the SIO FIFO for a shutdown or status request + delay.delay_us(HEADING_UPDATE_MICROS / 10); + if let Some(cmd) = sio.fifo.read() { + match cmd.into() { + HeadingTaskCommands::Shutdown => { + // close up any critical sections + // enter into a busy wait loop + // only listen to the SIO FIFO for a status request + // send back the current status of the task + // which will be `HeadingTaskCommands::Shutdown` + break; + } + HeadingTaskCommands::Status => { + // send back the current status of the task + // which will be `HeadingTaskCommands::Shutdown` + sio.fifo.write(HeadingTaskStatus::CalculatingHeading.into()); + } + _ => {} + } + } + HeadingManager::update_heading(&mut gyro, &mut timer); + } + }); + } + /// runs on core 1 + fn update_heading(gyro: &mut Mpu6050, timer: &mut Timer) + where + I2C: embedded_hal::i2c::I2c, + { + let current_micros = timer.get_counter().ticks(); + let last_data = critical_section::with(|cs| HEADING_VALUE.borrow(cs).get()); + + if current_micros - last_data.last_update_ticks < HEADING_UPDATE_MICROS as u64 { + // too soon to update the heading + return; + } + + let gyro_reading = match gyro.get_gyro_deg() { + Ok(reading) => reading, + Err(_e) => { + // error!("Error getting gyro readings"); + return; + } + }; + + let delta_micros = current_micros - last_data.last_update_ticks; + let delta_time_s = delta_micros as f64 / 1_000_000.0; + + let delta_heading = + (last_data.last_update_rate + gyro_reading.z as f64) * delta_time_s / 2.0; + let mut new_heading = last_data.heading + delta_heading; + if new_heading > 180.0 { + new_heading -= 360.0; + } else if new_heading < -180.0 { + new_heading += 360.0; + } + let new_rate = gyro_reading.z as f64; + critical_section::with(|cs| { + HEADING_VALUE.borrow(cs).set(HeadingData { + heading: new_heading, + last_update_rate: new_rate, + last_update_ticks: current_micros, + }); + }); + } + + /// runs on core 0 + pub fn get_heading(&self) -> f32 { + // DEBUG get status + // send status request to core1 + + critical_section::with(|cs| HEADING_VALUE.borrow(cs).get().heading as f32) + } +} diff --git a/src/model/mod.rs b/src/model/mod.rs index aff8863..04821fd 100644 --- a/src/model/mod.rs +++ b/src/model/mod.rs @@ -1,3 +1,4 @@ -pub mod heading; +// pub mod heading; +pub mod heading_core1; pub mod pid_controller; pub mod point; diff --git a/src/robot.rs b/src/robot.rs index b3ade60..418e048 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -6,7 +6,10 @@ mod motor_controller; mod telemetry; use crate::{ - model::{heading::HeadingCalculator, pid_controller::PIDController}, + model::{ + heading_core1::{self, HeadingManager}, + pid_controller::PIDController, + }, robot::{ config::Config, debouncer::DebouncedButton, @@ -37,8 +40,17 @@ use embedded_hal::{ use embedded_hal_bus::i2c::CriticalSectionDevice; use i2c_character_display::{AdafruitLCDBackpack, LcdDisplayType}; use micromath::F32Ext; -use rp_pico::hal::{gpio, Timer}; use rp_pico::hal::{ + fugit::Rate, + gpio::{ + self, + bank0::{Gpio4, Gpio5}, + FunctionI2C, PinId, PullUp, + }, + Timer, I2C, +}; +use rp_pico::hal::{ + gpio::Pin, gpio::{ bank0::{Gpio20, Gpio21}, FunctionSio, Interrupt, SioInput, @@ -99,20 +111,16 @@ pub struct Robot< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0, TWI1, SPI_DEV: embedded_hal::spi::SpiDevice, DELAY, > where - TWI0: embedded_hal::i2c::I2c, TWI1: embedded_hal::i2c::I2c, DELAY: DelayNs, { motors: MotorController, button1: DebouncedButton, button2: DebouncedButton, - pub heading_calculator: - HeadingCalculator>, lcd: AdafruitLCDBackpack, Timer>, pub sd_card: FileStorage, reset_display_start_millis: u64, @@ -121,6 +129,7 @@ pub struct Robot< idle_message_line2: Option, config: Config, delay: Timer, + heading_core1: HeadingManager<'a>, } #[allow(dead_code, non_camel_case_types)] @@ -134,13 +143,11 @@ impl< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0, TWI1, SPI_DEV: embedded_hal::spi::SpiDevice, DELAY, - > Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY> + > Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> where - TWI0: embedded_hal::i2c::I2c, TWI1: embedded_hal::i2c::I2c, DELAY: DelayNs + Clone, { @@ -154,12 +161,15 @@ where duty_b: ENB, button1_pin: BUTT1, button2_pin: BUTT2, - i2c0_refcell: &'a critical_section::Mutex>, i2c1_refcell: &'a critical_section::Mutex>, left_counter_pin: LeftWheelCounterPin, right_counter_pin: RightWheelCounterPin, mut sd_card: FileStorage, timer: &mut Timer, + core1: &'a mut rp_pico::hal::multicore::Core<'a>, + sys_freq: Rate, + i2c0_sda_pin: Pin, + i2c0_scl_pin: Pin, ) -> Self { // create the motor controller let motors = MotorController::new(ina1_pin, ina2_pin, inb1_pin, inb2_pin, duty_a, duty_b); @@ -217,11 +227,12 @@ where }; debug!("LCD characters created"); - let mut heading_calculator = HeadingCalculator::new( - embedded_hal_bus::i2c::CriticalSectionDevice::new(i2c0_refcell), - timer, - ); - heading_calculator.reset(); + // let mut heading_calculator = HeadingCalculator::new( + // embedded_hal_bus::i2c::CriticalSectionDevice::new(i2c0_refcell), + // timer, + // core1, + // ); + // heading_calculator.reset(); if let Err(_e) = lcd .home() @@ -240,11 +251,14 @@ where } let logger = Logger::new(Logger::::init_log_file(&mut sd_card)); + + let mut heading_core1 = HeadingManager::new(core1, sys_freq); + heading_core1.start_heading_calculation(timer.clone(), i2c0_sda_pin, i2c0_scl_pin); + Self { motors, button1: DebouncedButton::::new(button1_pin), button2: DebouncedButton::::new(button2_pin), - heading_calculator, lcd, sd_card, reset_display_start_millis: millis(), // start the display reset timer to clear the "started" message @@ -253,6 +267,7 @@ where idle_message_line2: None, config: Config::new(), delay: *timer, + heading_core1, } } @@ -262,10 +277,10 @@ where if self.sd_card.root_dir().is_some() { let root_dir = self.sd_card.root_dir().unwrap(); let config_dir = match self.sd_card.open_directory(root_dir, CONFIG_DIRECTORY) { - Some(d) => d, - None => { - error!("Error opening config directory"); - return Err(embedded_sdmmc::Error::NotFound); + Ok(d) => d, + Err(e) => { + error!("Error opening config directory: {}", e); + return Err(e); } }; if let Ok(mut config_file) = self.sd_card.open_file_in_dir( @@ -285,13 +300,13 @@ where self.config.set_config_values(config_str); // set the gyro offsets from the config file - if self.heading_calculator.is_inited() { - self.heading_calculator.set_gyro_offsets( - self.config.gyro_offset_x, - self.config.gyro_offset_y, - self.config.gyro_offset_z, - ); - } + // if self.heading_calculator.is_inited() { + // self.heading_calculator.set_gyro_offsets( + // self.config.gyro_offset_x, + // self.config.gyro_offset_y, + // self.config.gyro_offset_z, + // ); + // } } else { debug!("Config file not found, using defaults"); } @@ -305,11 +320,11 @@ where } pub fn calibrate_gyro(&mut self, delay: &mut DELAY) { - self.heading_calculator.calibrate(delay, |step| { - if let Ok(lcd) = self.lcd.set_cursor(0, 1) { - write!(lcd, "iteration: {}", step).ok(); - } - }); + // self.heading_calculator.calibrate(delay, |step| { + // if let Ok(lcd) = self.lcd.set_cursor(0, 1) { + // write!(lcd, "iteration: {}", step).ok(); + // } + // }); } /// This function is called in the main loop to allow the robot to handle state updates @@ -317,7 +332,7 @@ where if self.reset_display_start_millis != 0 && millis() - self.reset_display_start_millis > DISPLAY_RESET_DELAY_MS { - self.heading_calculator.update(); + // self.heading_calculator.update(); debug!("Resetting LCD to idle message"); let idle_message = self.config.idle_message.clone(); if let Err(error) = @@ -338,7 +353,7 @@ where self.button1.handle_loop(); self.button2.handle_loop(); - self.heading_calculator.update(); + // self.heading_calculator.update(); } pub fn set_idle_message_line2(&mut self, message: Option) { @@ -453,7 +468,7 @@ where ); // we want to go straight, so the setpoint is 0 controller.set_setpoint(0.); - self.heading_calculator.reset(); + // self.heading_calculator.reset(); self.motors.set_duty( self.config.straight_left_power, @@ -469,7 +484,8 @@ where 0, 100, 100, - self.heading_calculator.heading(), + // self.heading_calculator.heading(), + 180.0, // FIXME: remove this hardcoded value 0., ), ) @@ -490,7 +506,8 @@ where if millis() - last_update_millis > CONTROLLER_SAMPLE_PERIOD_MS { last_update_millis = millis(); - let heading = self.heading_calculator.heading(); + // let heading = self.heading_calculator.heading(); + let heading = 0.0; // FIXME: remove this hardcoded value let control_signal = controller.update(heading as f32, last_update_millis); let mut cs_indicator: &str = direction_arrow; @@ -523,7 +540,7 @@ where ), ) .ok(); - self.heading_calculator.update(); + // self.heading_calculator.update(); let wheel_ticks_per_mm = self.config.wheel_ticks_per_mm; if let Err(error) = core::write!( @@ -535,7 +552,7 @@ where ) { error!("Error writing to LCD: {}", error.to_string().as_str()); } - self.heading_calculator.update(); + // self.heading_calculator.update(); if let Err(error) = core::write!( self.set_lcd_cursor(0, 1), "{} : cs={:.3}", @@ -572,7 +589,8 @@ where right_wheel_ticks, 0, 0, - self.heading_calculator.heading(), + // self.heading_calculator.heading(), + 0.0, // FIXME: remove this hardcoded value 0., ), ) @@ -618,9 +636,10 @@ where ) { error!("Error writing to LCD: {}", error.to_string().as_str()); } - self.heading_calculator.reset(); + // self.heading_calculator.reset(); self.handle_loop(); - let mut current_angle = self.heading_calculator.heading() as f32; + // let mut current_angle = self.heading_calculator.heading() as f32; + let mut current_angle = 0.0; // FIXME: remove this hardcoded value let mut last_adjust_angle = current_angle; // start the motors per right hand rule (postive angle = left turn, negative angle = right turn) @@ -646,7 +665,8 @@ where }; while current_angle.abs() < (turn_degrees.abs() + stop_angle_delta) as f32 { - current_angle = self.heading_calculator.heading() as f32; + // current_angle = self.heading_calculator.heading() as f32; + current_angle = 0.0; // FIXME: remove this hardcoded value if (current_angle - last_adjust_angle).abs() > 10.0 { last_adjust_angle = current_angle; if let Err(error) = core::write!( @@ -662,11 +682,13 @@ where self.handle_loop(); } - let motors_off_heading = self.heading_calculator.heading(); + // let motors_off_heading = self.heading_calculator.heading(); + let motors_off_heading = 0.0; // FIXME: remove this hardcoded value self.motors.brake(); self.delay.delay_ms(100); self.motors.stop(); - current_angle = self.heading_calculator.heading() as f32; + // current_angle = self.heading_calculator.heading() as f32; + current_angle = 0.0; // FIXME: remove this hardcoded value if let Err(error) = core::write!( self.clear_lcd().set_lcd_cursor(0, 0), "{} {} / {}\x03", @@ -700,7 +722,7 @@ where ) -> Result<(), i2c_character_display::CharacterDisplayError>> { write!(self.lcd.clear()?.set_cursor(0, 0)?, "Heading:").ok(); - self.heading_calculator.reset(); + // self.heading_calculator.reset(); let mut continue_loop = true; let mut last_update_millis = 0; @@ -709,15 +731,24 @@ where if self.button1_pressed() || self.button2_pressed() { continue_loop = false; } - if millis() - last_update_millis > 500 { - let heading = self.heading_calculator.heading(); - if let Err(error) = core::write!( + if millis() - last_update_millis > 200 { + // let heading = self.heading_calculator.heading(); + // if let Err(error) = core::write!( + // self.lcd.set_cursor(0, 1)?, + // "{:.2}{: <16}", + // heading, + // DEGREES_STRING + // ) { + // error!("Error writing to LCD: {}", error.to_string().as_str()); + // } + let heading = self.heading_core1.get_heading(); + if let Err(e) = core::write!( self.lcd.set_cursor(0, 1)?, "{:.2}{: <16}", heading, DEGREES_STRING ) { - error!("Error writing to LCD: {}", error.to_string().as_str()); + error!("Error writing to LCD: {}", e.to_string().as_str()); } last_update_millis = millis(); } @@ -753,14 +784,11 @@ impl< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0, TWI1, SPI_DEV: SpiDevice, DELAY, - > Format - for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY> + > Format for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> where - TWI0: I2c, TWI1: I2c, DELAY: DelayNs + Clone, { @@ -785,14 +813,12 @@ impl< ENB: SetDutyCycle, BUTT1: InputPin, BUTT2: InputPin, - TWI0, TWI1, SPI_DEV: SpiDevice, DELAY, > core::fmt::Write - for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI0, TWI1, SPI_DEV, DELAY> + for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> where - TWI0: I2c, TWI1: I2c, DELAY: DelayNs + Clone, { diff --git a/src/robot/file_storage.rs b/src/robot/file_storage.rs index a3b37e5..f639339 100644 --- a/src/robot/file_storage.rs +++ b/src/robot/file_storage.rs @@ -130,17 +130,22 @@ where self.root_dir } - pub fn open_directory(&self, parent: RawDirectory, name: &str) -> Option { + pub fn open_directory( + &self, + parent: RawDirectory, + name: &str, + ) -> Result> { match self .volume_mgr - .as_ref()? + .as_ref() + .unwrap() .borrow_mut() .open_dir(parent, name) { - Ok(dir) => Some(dir), + Ok(dir) => Ok(dir), Err(e) => { error!("Error opening directory '{}': {:?}", name, e); - None + Err(e) } } } @@ -195,7 +200,7 @@ where ext: &str, ) -> Result, embedded_sdmmc::Error> { let mut files: Vec = Vec::new(); - let dir = self.open_directory(parent_dir, target_dir).unwrap(); + let dir = self.open_directory(parent_dir, target_dir)?; self.iterate_dir(dir, |entry| { if entry.name.to_string().ends_with(ext) { debug!("Found path file: {:?}", entry.name.to_string().as_str()); diff --git a/src/robot/file_storage/logger.rs b/src/robot/file_storage/logger.rs index 98c148b..3321a6f 100644 --- a/src/robot/file_storage/logger.rs +++ b/src/robot/file_storage/logger.rs @@ -51,9 +51,9 @@ where } }; let log_dir = match sdcard.open_directory(root_dir, LOG_DIR_NAME) { - Some(dir) => dir, - None => { - error!("Error opening log dir"); + Ok(dir) => dir, + Err(e) => { + error!("Error opening log dir: {}", e); return None; } }; @@ -104,9 +104,9 @@ where // write the new log index back to the file. Need to re-open the containing directory first. let log_dir = match sdcard.open_directory(root_dir, LOG_DIR_NAME) { - Some(dir) => dir, - None => { - error!("Error opening log dir"); + Ok(dir) => dir, + Err(e) => { + error!("Error opening log dir: {}", e); return None; } }; @@ -135,9 +135,9 @@ where // now creater the logger let log_filename = format!("log_{}.txt", log_index); let log_dir = match sdcard.open_directory(root_dir, LOG_DIR_NAME) { - Some(dir) => dir, - None => { - error!("Error opening log dir"); + Ok(dir) => dir, + Err(e) => { + error!("Error opening log dir: {}", e); return None; } }; diff --git a/src/robot/file_storage/sd_card_spi_device.rs b/src/robot/file_storage/sd_card_spi_device.rs index 9283c20..83d6101 100644 --- a/src/robot/file_storage/sd_card_spi_device.rs +++ b/src/robot/file_storage/sd_card_spi_device.rs @@ -1,3 +1,6 @@ +use core::cell::RefCell; + +use critical_section::Mutex; use defmt::{error, trace}; use embedded_hal::{ delay::DelayNs, @@ -35,18 +38,16 @@ impl core::fmt::Display for SDCardSPIDeviceError { } } -pub struct SDCardSPIDevice +pub struct SDCardSPIDevice<'a, SPI, CS, DELAY> where SPI: SpiBus, CS: OutputPin, DELAY: DelayNs, { - pub bus: SPI, - cs: CS, - delay: DELAY, + pub bus: embedded_hal_bus::spi::CriticalSectionDevice<'a, SPI, CS, DELAY>, } -impl ErrorType for SDCardSPIDevice +impl<'a, SPI, CS, DELAY> ErrorType for SDCardSPIDevice<'a, SPI, CS, DELAY> where SPI: SpiBus, CS: OutputPin, @@ -55,80 +56,91 @@ where type Error = SDCardSPIDeviceError; } -impl SpiDevice for SDCardSPIDevice +impl<'a, SPI, CS, DELAY> SpiDevice for SDCardSPIDevice<'a, SPI, CS, DELAY> where SPI: SpiBus, CS: OutputPin, DELAY: DelayNs, { fn transaction(&mut self, operations: &mut [Operation<'_, u8>]) -> Result<(), Self::Error> { - trace!( - "SDCardSPIDevice::transaction - operations.len = {}", - operations.len() - ); - // Implement the SPI transaction here - if self.cs.set_low().is_err() { - return Err(SDCardSPIDeviceError::ChipSelect); - } - for operation in operations { - if let Err(_e) = match operation { - Operation::Write(data) => { - trace!( - "SDCardSPIDevice::transaction - write data.len = {}", - data.len() - ); - self.bus.write(data) - } - Operation::Transfer(read_buf, write_buf) => { - trace!("SDCardSPIDevice::transaction - transfer read_buf.len = {}, write_buf.len = {}", read_buf.len(), write_buf.len()); - self.bus.transfer(read_buf, write_buf) - } - Operation::Read(data) => { - trace!( - "SDCardSPIDevice::transaction - read data.len = {}", - data.len() - ); - self.bus.read(data) - } - Operation::TransferInPlace(data) => { - trace!( - "SDCardSPIDevice::transaction - transfer_in_place data.len = {}", - data.len() - ); - self.bus.transfer_in_place(data) - } - Operation::DelayNs(time) => { - trace!("SDCardSPIDevice::transaction - delay_ns time = {}", time); - self.delay.delay_ns(*time); - Ok(()) - } - } { - // if an error occurs, deassert CS pin and return error - self.cs.set_high().ok(); - error!("SDCardSPIDevice::transaction - error"); - return Err(SDCardSPIDeviceError::Spi); - } - } - if self.bus.flush().is_err() { - self.cs.set_high().ok(); - error!("SDCardSPIDevice::transaction - flush error"); - return Err(SDCardSPIDeviceError::Spi); - } - if self.cs.set_high().is_err() { - error!("SDCardSPIDevice::transaction - chip select error"); - return Err(SDCardSPIDeviceError::ChipSelect); - } - Ok(()) + // trace!( + // "SDCardSPIDevice::transaction - operations.len = {}", + // operations.len() + // ); + // // Implement the SPI transaction here + // if self.cs.set_low().is_err() { + // return Err(SDCardSPIDeviceError::ChipSelect); + // } + // for operation in operations { + // if let Err(_e) = match operation { + // Operation::Write(data) => { + // trace!( + // "SDCardSPIDevice::transaction - write data.len = {}", + // data.len() + // ); + // self.bus.write(data) + // } + // Operation::Transfer(read_buf, write_buf) => { + // trace!("SDCardSPIDevice::transaction - transfer read_buf.len = {}, write_buf.len = {}", read_buf.len(), write_buf.len()); + // self.bus.transfer(read_buf, write_buf) + // } + // Operation::Read(data) => { + // trace!( + // "SDCardSPIDevice::transaction - read data.len = {}", + // data.len() + // ); + // self.bus.read(data) + // } + // Operation::TransferInPlace(data) => { + // trace!( + // "SDCardSPIDevice::transaction - transfer_in_place data.len = {}", + // data.len() + // ); + // self.bus.transfer_in_place(data) + // } + // Operation::DelayNs(time) => { + // trace!("SDCardSPIDevice::transaction - delay_ns time = {}", time); + // self.delay.delay_ns(*time); + // Ok(()) + // } + // } { + // // if an error occurs, deassert CS pin and return error + // self.cs.set_high().ok(); + // error!("SDCardSPIDevice::transaction - error"); + // return Err(SDCardSPIDeviceError::Spi); + // } + // } + // if self.bus.flush().is_err() { + // self.cs.set_high().ok(); + // error!("SDCardSPIDevice::transaction - flush error"); + // return Err(SDCardSPIDeviceError::Spi); + // } + // if self.cs.set_high().is_err() { + // error!("SDCardSPIDevice::transaction - chip select error"); + // return Err(SDCardSPIDeviceError::ChipSelect); + // } + // Ok(()) + self.bus + .transaction(operations) + .map_err(|_| SDCardSPIDeviceError::Spi) } } -impl SDCardSPIDevice +impl<'a, SPI, CS, DELAY> SDCardSPIDevice<'a, SPI, CS, DELAY> where SPI: SpiBus, CS: OutputPin, DELAY: DelayNs, { - pub fn new(bus: SPI, cs: CS, delay: DELAY) -> Self { - Self { bus, cs, delay } + pub fn new(bus: &'a Mutex>, cs: CS, delay: DELAY) -> Self { + Self { + bus: embedded_hal_bus::spi::CriticalSectionDevice::new(bus, cs, delay), + } + } + + pub fn spi_device( + &mut self, + ) -> &mut embedded_hal_bus::spi::CriticalSectionDevice<'a, SPI, CS, DELAY> { + &mut self.bus } } diff --git a/src/robot/file_storage/sd_file.rs b/src/robot/file_storage/sd_file.rs index 57f7196..1fd19c3 100644 --- a/src/robot/file_storage/sd_file.rs +++ b/src/robot/file_storage/sd_file.rs @@ -38,11 +38,13 @@ where pub fn init(&mut self) -> Result<(), embedded_sdmmc::Error> { // first open the file per the passed mode - let file = match self.volume_manager.borrow_mut().open_file_in_dir( - self.directory, - self.filename.as_str(), - self.mode, - ) { + let file = match critical_section::with(|_| { + self.volume_manager.borrow_mut().open_file_in_dir( + self.directory, + self.filename.as_str(), + self.mode, + ) + }) { Ok(f) => { debug!( "Opened file '{}' with mode {}", @@ -57,7 +59,9 @@ where } }; // then close the file - if let Err(e) = self.volume_manager.borrow_mut().close_file(file) { + if let Err(e) = + critical_section::with(|_| self.volume_manager.borrow_mut().close_file(file)) + { error!("Error closing file '{}': {:?}", self.filename.as_str(), e); return Err(e); } From b28491fa1782caea3b58640faaa3e7bc13c6c57e Mon Sep 17 00:00:00 2001 From: Michael Kamprath Date: Fri, 29 Nov 2024 10:47:57 -0800 Subject: [PATCH 2/4] fully updarted to heading on core 1 --- .cargo/config.toml | 2 +- src/driver.rs | 19 +- src/main.rs | 11 +- src/model/heading.rs | 152 ------------- src/model/heading_core1.rs | 217 +++++++------------ src/robot.rs | 93 +++----- src/robot/file_storage.rs | 1 - src/robot/file_storage/sd_card_spi_device.rs | 146 ------------- src/system/millis.rs | 1 + 9 files changed, 128 insertions(+), 514 deletions(-) delete mode 100644 src/model/heading.rs delete mode 100644 src/robot/file_storage/sd_card_spi_device.rs diff --git a/.cargo/config.toml b/.cargo/config.toml index 6ce3637..19590b7 100644 --- a/.cargo/config.toml +++ b/.cargo/config.toml @@ -27,4 +27,4 @@ rustflags = [ target = "thumbv6m-none-eabi" [env] -DEFMT_LOG = "embedded_sdmmc=debug,debug" +DEFMT_LOG = "embedded_sdmmc=info,debug" diff --git a/src/driver.rs b/src/driver.rs index ef56f20..ac32f5b 100644 --- a/src/driver.rs +++ b/src/driver.rs @@ -218,9 +218,8 @@ impl< // - button one will select the current function and execute it // - if no button is pressed for the idle wait time, return to idle state - const FUNCTIONS: [&str; 6] = [ + const FUNCTIONS: [&str; 5] = [ "Select Path", - "Calibrate Gyro", "Heading Display", "Drive Straight", "Turn Left", @@ -266,20 +265,10 @@ impl< self.robot.start_display_reset_timer(); } 1 => { - write!( - self.robot.clear_lcd().set_lcd_cursor(0, 0), - "Calibrating gyro", - ) - .ok(); - self.robot.calibrate_gyro(&mut self.delay); - write!(self.robot.set_lcd_cursor(0, 1), " Done ",).ok(); - self.robot.start_display_reset_timer(); - } - 2 => { self.robot.display_heading().ok(); self.robot.set_display_to_idle(); } - 3 => { + 2 => { write!( self.robot.clear_lcd().set_lcd_cursor(0, 0), "Driving straight", @@ -289,7 +278,7 @@ impl< self.robot.straight(1500, true); self.robot.start_display_reset_timer(); } - 4 => { + 3 => { write!( self.robot.clear_lcd().set_lcd_cursor(0, 0), "Turning left \x7F", @@ -299,7 +288,7 @@ impl< self.robot.turn(90); self.robot.start_display_reset_timer(); } - 5 => { + 4 => { write!( self.robot.clear_lcd().set_lcd_cursor(0, 0), "Turning right \x7E", diff --git a/src/main.rs b/src/main.rs index 145f7cf..fa60829 100644 --- a/src/main.rs +++ b/src/main.rs @@ -6,7 +6,7 @@ mod robot; mod system; use core::cell::RefCell; -use defmt::{error, info, panic}; +use defmt::{info, panic}; use defmt_rtt as _; use panic_probe as _; @@ -136,9 +136,9 @@ fn main() -> ! { let sd_card_device = embedded_hal_bus::spi::CriticalSectionDevice::new( &spi_mutex, pins.gpio1.into_push_pull_output(), - timer.clone(), + timer, ); - let sd = crate::robot::file_storage::FileStorage::new(sd_card_device, timer.clone()); + let sd = crate::robot::file_storage::FileStorage::new(sd_card_device, timer); // If SD card is successfully initialized, we can increase the SPI speed info!( @@ -172,11 +172,10 @@ fn main() -> ! { &mut timer, core1, sys_freq, - sda0_pin, - scl0_pin, ); - if let Err(e) = robot.init() { + let led_pin = pins.gpio16.into_push_pull_output(); + if let Err(e) = robot.init(sda0_pin, scl0_pin, led_pin) { panic!("Error initializing Robot: {:?}", e); } diff --git a/src/model/heading.rs b/src/model/heading.rs deleted file mode 100644 index c3a6cfd..0000000 --- a/src/model/heading.rs +++ /dev/null @@ -1,152 +0,0 @@ -#![allow(non_camel_case_types)] - -use crate::system::millis::millis_tenths; - -use defmt::{error, info}; -use embedded_hal::{delay::DelayNs, i2c::I2c}; -use micromath::vector::Vector3d; -use mpu6050::Mpu6050; -use rp_pico::hal::multicore::Stack; - -// My MCU6050 calibration values -// .. XAccel YAccel ZAccel XGyro YGyro ZGyro -// [-2281,-2280] --> [-3,14] [1725,1726] --> [-15,4] [1581,1581] --> [16381,16389] [97,98] --> [-2,1] [43,44] --> [-1,1] [20,20] --> [0,1] -// [-2241,-2240] --> [-1,15] [1803,1804] --> [-10,6] [1573,1574] --> [16371,16394] [98,99] --> [-2,1] [44,45] --> [0,5] [19,20] --> [0,4] -// [-2239,-2238] --> [-3,16] [1804,1804] --> [0,5] [1575,1576] --> [16378,16395] [98,99] --> [-1,2] [43,44] --> [-2,1] [19,20] --> [-1,2] - -const HEADING_UPDATE_INTERVAL_MS_TENTHS: u64 = 20; - -pub struct HeadingCalculator { - heading: f64, - gyro: Mpu6050, - last_update_rate: f64, - last_update_millis_tenths: u64, - inited: bool, -} - -#[allow(dead_code, non_camel_case_types)] -impl HeadingCalculator -where - TWI: I2c, -{ - pub fn new( - i2c: TWI, - timer: &mut DELAY, - core1: &mut rp_pico::hal::multicore::Core, - ) -> Self - where - DELAY: DelayNs, - { - let mut gyro = Mpu6050::new(i2c); - let mut inited = false; - match gyro.init(timer) { - Ok(_) => { - info!("Gyro initialized"); - inited = true; - } - Err(_e) => { - error!("Error initializing gyro"); - } - }; - if inited { - if let Err(_error) = gyro.set_gyro_range(mpu6050::device::GyroRange::D250) { - error!("Error setting gyro range"); - } - - let cur_offsets = match gyro.get_gyro_offsets() { - Ok(offsets) => offsets, - Err(_error) => { - error!("Error getting gyro offsets"); - Vector3d::::default() - } - }; - - info!( - "Got gyro offsets: x = {}, y = {}, z = {}", - cur_offsets.x, cur_offsets.y, cur_offsets.z - ); - } - - Self { - heading: 0.0, - gyro, - last_update_rate: 0.0, - last_update_millis_tenths: millis_tenths(), - inited, - } - } - - pub fn is_inited(&self) -> bool { - self.inited - } - - pub fn calibrate(&mut self, delay: &mut DELAY, callback: F) { - if !self.is_inited() { - error!("Gyro not initialized"); - return; - } - if let Err(_e) = self.gyro.calibrate_gyro(delay, callback) { - error!("Error calibrating gyro"); - } - } - - pub fn set_gyro_offsets(&mut self, x_offset: i16, y_offset: i16, z_offset: i16) { - if !self.is_inited() { - error!("Cannot set gyro offsets before gyro is initialized"); - return; - } - if self - .gyro - .set_gyro_offsets(x_offset, y_offset, z_offset) - .is_err() - { - error!("Error setting gyro offsets"); - } - info!( - "Set gyro offsets: x = {}, y = {}, z = {}", - x_offset, y_offset, z_offset - ); - } - - pub fn reset(&mut self) { - self.heading = 0.0; - self.last_update_rate = 0.0; - self.last_update_millis_tenths = millis_tenths(); - } - - pub fn update(&mut self) -> f64 { - if !self.is_inited() { - return 0.0; - } - let now = millis_tenths(); - let delta_time = now - self.last_update_millis_tenths; - if delta_time >= HEADING_UPDATE_INTERVAL_MS_TENTHS { - match self.gyro.get_gyro_deg() { - Ok(gyro) => { - self.last_update_millis_tenths = now; - // the heding is about the sensor's Z-axis - let delta_degs = (self.last_update_rate + gyro.z as f64) - * (delta_time as f64 / 10000.0) - / 2.0; - self.heading += delta_degs; - self.last_update_rate = gyro.z as f64; - - if self.heading > 180.0 { - self.heading -= 360.0; - } else if self.heading < -180.0 { - self.heading += 360.0; - } - } - Err(_error) => { - error!("Error reading gyro"); - } - } - } - - self.heading - } - - pub fn heading(&mut self) -> f64 { - self.update() - } -} diff --git a/src/model/heading_core1.rs b/src/model/heading_core1.rs index 71a91f6..b5d3a73 100644 --- a/src/model/heading_core1.rs +++ b/src/model/heading_core1.rs @@ -5,163 +5,86 @@ use bsp::hal::{ FunctionI2C, Pin, PullUp, }, multicore::Stack, - pac, - sio::Sio, - Timer, + pac, Timer, }; use core::cell::{Cell, RefCell}; use critical_section::Mutex; -use embedded_hal::i2c::{self, I2c}; +use defmt::error; +use embedded_hal::digital::OutputPin; +use micromath::vector::Vector3d; use mpu6050::Mpu6050; use rp_pico as bsp; -const HEADING_UPDATE_MICROS: u32 = 2 * 1000; // 2ms - -pub enum HeadingTaskCommands { - Shutdown, - Status, - SetOffsets, - Unknown, -} - -impl Into for HeadingTaskCommands { - fn into(self) -> u32 { - match self { - HeadingTaskCommands::Shutdown => 0xFFFFFFFF, - HeadingTaskCommands::Status => 1, - HeadingTaskCommands::SetOffsets => 2, - HeadingTaskCommands::Unknown => 0, - } - } -} - -impl From for HeadingTaskCommands { - fn from(val: u32) -> Self { - match val { - 0xFFFFFFFF => HeadingTaskCommands::Shutdown, - 1 => HeadingTaskCommands::Status, - 2 => HeadingTaskCommands::SetOffsets, - _ => HeadingTaskCommands::Unknown, - } - } -} - -pub enum HeadingTaskStatus { - CalculatingHeading, - Calibrating, - Done, - Unknown, -} - -impl Into for HeadingTaskStatus { - fn into(self) -> u32 { - match self { - HeadingTaskStatus::CalculatingHeading => 1, - HeadingTaskStatus::Calibrating => 2, - HeadingTaskStatus::Done => 0xFFFFFFFF, - HeadingTaskStatus::Unknown => 0, - } - } -} - -impl From for HeadingTaskStatus { - fn from(val: u32) -> Self { - match val { - 1 => HeadingTaskStatus::CalculatingHeading, - 2 => HeadingTaskStatus::Calibrating, - 0xFFFFFFFF => HeadingTaskStatus::Done, - _ => HeadingTaskStatus::Unknown, - } - } -} +const HEADING_UPDATE_MICROS: u32 = 5 * 1000; // 5ms /// Stack for core 1 -/// -/// Core 0 gets its stack via the normal route - any memory not used by static -/// values is reserved for stack and initialised by cortex-m-rt. -/// To get the same for Core 1, we would need to compile everything separately -/// and modify the linker file for both programs, and that's quite annoying. -/// So instead, core1.spawn takes a [usize] which gets used for the stack. -/// NOTE: We use the `Stack` struct here to ensure that it has 32-byte -/// alignment, which allows the stack guard to take up the least amount of -/// usable RAM. static mut CORE1_STACK: Stack<4096> = Stack::new(); -// The basic design for dedicating core1 to heading calculation -// -// There are two tasks that core1 will be responsible for: -// 1. Reading the gyro sensor and calculating the heading -// 2. Calibrating the gyro on demand -// -// The issue is each are seperate tasks, so we will need to communication from core0 to core1 -// when it needs to stop the heading calculation and start the calibration. This will be done -// by sending a sentinal value from core0 to core1 using the SIO FIFO. The sentinal value for -// shutting down a task will be `HeadingTaskCommands::Shutdown`. When a task receives this value, it -// will close up any critical sections and enter into a busy wait loop it only listens to -// the SIO FIFO for a status request, `HeadingTaskCommands::Status`. When it receives this value -// it will send back the current status of the task, which will be `HeadingTaskCommands::Shutdown`. -// To start a new task, whether it be the same task or a different one, core0 will simply -// spawn the task again, which shuts down the current task and starts the new one. -// -// ** Calculating the heading task ** -// The task for calculating heading will get the updated gyro readings from the sensor at -// a fixed interval, calculate the heading, and the setting it to a global variable that -// is wrapped in a Mutex. A critical section will be used to access the global variable. -// While running the task will monitor the SIO FIFO for either a shutdown or status request. -// If it receives a shutdown, it will close up any critical sections and enter into a busy -// wait loop. If it receives a status request, it will send back the current status of the -// task, which will be either TASK_HEADING_CALCULATING = 0xFFFFFFFD or TASK_DONE = 0xFFFFFFFF. -// -// ** Calibrating the gyro task ** - static HEADING_VALUE: Mutex> = Mutex::new(Cell::new(HeadingData { heading: 0.0, last_update_rate: 0.0, last_update_ticks: 0, })); +static GYRO_OFFSETS: Mutex>> = + Mutex::new(Cell::new(Vector3d:: { x: 0, y: 0, z: 0 })); + #[derive(Debug, Clone, Copy)] struct HeadingData { heading: f64, last_update_rate: f64, last_update_ticks: u64, } + +/// A core 0 object that manages the heading calculation on core 1. pub struct HeadingManager<'a> { core1: &'a mut rp_pico::hal::multicore::Core<'a>, sys_freq: Rate, + timer: Timer, } impl<'a> HeadingManager<'a> { pub fn new( core1: &'a mut rp_pico::hal::multicore::Core<'a>, sys_freq: Rate, + timer: &mut Timer, ) -> Self { Self { - core1: core1, - sys_freq: sys_freq, + core1, + sys_freq, + timer: *timer, } } - pub fn start_heading_calculation( + /// Start the heading calculation task on core 1, initializing I2C bus 0 and the gyro sensor. + pub fn start_heading_calculation( &mut self, timer: Timer, i2c0_sda_pin: Pin, i2c0_scl_pin: Pin, + mut led_pin: LEDPIN, + gyro_offsets: Vector3d, ) { + critical_section::with(|cs| { + GYRO_OFFSETS.borrow(cs).set(gyro_offsets); + }); + // spawn the heading calculation task on core1 let sys_freq = self.sys_freq; // value that can be moved into the closure - let mut timer = timer.clone(); - let res = self.core1.spawn(unsafe { &mut CORE1_STACK.mem }, move || { + let mut timer = timer; + let _res = self.core1.spawn(unsafe { &mut CORE1_STACK.mem }, move || { + // LED is used to indicate that the gyro is good to go. make sure it is off to start + if let Err(_e) = led_pin.set_low() { + error!("Error setting LED pin low"); + }; + // do the initialization in a critical section // set up environment let mut pac = unsafe { pac::Peripherals::steal() }; let core = unsafe { pac::CorePeripherals::steal() }; - let mut sio = Sio::new(pac.SIO); let mut delay = cortex_m::delay::Delay::new(core.SYST, sys_freq.to_Hz()); - - // let sda0_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio4.reconfigure(); - // let scl0_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio5.reconfigure(); + // set up the I2C controller let i2c0 = bsp::hal::I2C::new_controller( pac.I2C0, i2c0_sda_pin, @@ -180,32 +103,45 @@ impl<'a> HeadingManager<'a> { if let Err(_e) = gyro.init(&mut timer) { panic!("Error initializing gyro"); } + + // sett the offsets + let gyro_offsets = critical_section::with(|cs| GYRO_OFFSETS.borrow(cs).get()); + // if the offsets are all zero, then we calibrate. otherwise, we set the offsets + if gyro_offsets.x == 0 && gyro_offsets.y == 0 && gyro_offsets.z == 0 { + if let Err(_e) = gyro.calibrate_gyro(&mut timer, |count| { + defmt::info!("Calibrating gyro: {}", count); + }) { + defmt::error!("Error calibrating gyro"); + } + } else { + defmt::debug!( + "Setting gyro offsets: {{x: {}, y: {}, z: {} }}", + gyro_offsets.x, + gyro_offsets.y, + gyro_offsets.z + ); + if let Err(_e) = + gyro.set_gyro_offsets(gyro_offsets.x, gyro_offsets.y, gyro_offsets.z) + { + defmt::error!("Error setting gyro offsets"); + } + } + defmt::info!("Gyro initialized : {}", gyro); + if let Err(_e) = led_pin.set_high() { + error!("Error setting LED pin high"); + }; + + critical_section::with(|cs| { + HEADING_VALUE.borrow(cs).set(HeadingData { + heading: 0.0, + last_update_rate: 0.0, + last_update_ticks: timer.get_counter().ticks(), + }); + }); + // start the heading calculation task loop { - // get the current gyro readings - // calculate the heading - // set the heading to the global variable - // wait for the next update interval - // check the SIO FIFO for a shutdown or status request delay.delay_us(HEADING_UPDATE_MICROS / 10); - if let Some(cmd) = sio.fifo.read() { - match cmd.into() { - HeadingTaskCommands::Shutdown => { - // close up any critical sections - // enter into a busy wait loop - // only listen to the SIO FIFO for a status request - // send back the current status of the task - // which will be `HeadingTaskCommands::Shutdown` - break; - } - HeadingTaskCommands::Status => { - // send back the current status of the task - // which will be `HeadingTaskCommands::Shutdown` - sio.fifo.write(HeadingTaskStatus::CalculatingHeading.into()); - } - _ => {} - } - } HeadingManager::update_heading(&mut gyro, &mut timer); } }); @@ -215,10 +151,9 @@ impl<'a> HeadingManager<'a> { where I2C: embedded_hal::i2c::I2c, { - let current_micros = timer.get_counter().ticks(); let last_data = critical_section::with(|cs| HEADING_VALUE.borrow(cs).get()); - - if current_micros - last_data.last_update_ticks < HEADING_UPDATE_MICROS as u64 { + if timer.get_counter().ticks() - last_data.last_update_ticks < HEADING_UPDATE_MICROS as u64 + { // too soon to update the heading return; } @@ -226,10 +161,11 @@ impl<'a> HeadingManager<'a> { let gyro_reading = match gyro.get_gyro_deg() { Ok(reading) => reading, Err(_e) => { - // error!("Error getting gyro readings"); + defmt::error!("Error getting gyro readings vis I2C"); return; } }; + let current_micros = timer.get_counter().ticks(); let delta_micros = current_micros - last_data.last_update_ticks; let delta_time_s = delta_micros as f64 / 1_000_000.0; @@ -259,4 +195,15 @@ impl<'a> HeadingManager<'a> { critical_section::with(|cs| HEADING_VALUE.borrow(cs).get().heading as f32) } + + /// reset the heading to 0 + pub fn reset_heading(&self) { + critical_section::with(|cs| { + HEADING_VALUE.borrow(cs).set(HeadingData { + heading: 0.0, + last_update_rate: 0.0, + last_update_ticks: self.timer.get_counter().ticks(), + }); + }); + } } diff --git a/src/robot.rs b/src/robot.rs index 418e048..e7b2147 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -6,10 +6,7 @@ mod motor_controller; mod telemetry; use crate::{ - model::{ - heading_core1::{self, HeadingManager}, - pid_controller::PIDController, - }, + model::{heading_core1::HeadingManager, pid_controller::PIDController}, robot::{ config::Config, debouncer::DebouncedButton, @@ -39,15 +36,15 @@ use embedded_hal::{ }; use embedded_hal_bus::i2c::CriticalSectionDevice; use i2c_character_display::{AdafruitLCDBackpack, LcdDisplayType}; -use micromath::F32Ext; +use micromath::{vector::Vector3d, F32Ext}; use rp_pico::hal::{ fugit::Rate, gpio::{ self, bank0::{Gpio4, Gpio5}, - FunctionI2C, PinId, PullUp, + FunctionI2C, PullUp, }, - Timer, I2C, + Timer, }; use rp_pico::hal::{ gpio::Pin, @@ -168,8 +165,6 @@ where timer: &mut Timer, core1: &'a mut rp_pico::hal::multicore::Core<'a>, sys_freq: Rate, - i2c0_sda_pin: Pin, - i2c0_scl_pin: Pin, ) -> Self { // create the motor controller let motors = MotorController::new(ina1_pin, ina2_pin, inb1_pin, inb2_pin, duty_a, duty_b); @@ -252,8 +247,7 @@ where let logger = Logger::new(Logger::::init_log_file(&mut sd_card)); - let mut heading_core1 = HeadingManager::new(core1, sys_freq); - heading_core1.start_heading_calculation(timer.clone(), i2c0_sda_pin, i2c0_scl_pin); + let heading_core1 = HeadingManager::new(core1, sys_freq, timer); Self { motors, @@ -272,7 +266,12 @@ where } /// Inits the Robot software. This function should be called after the robot is created. - pub fn init(&mut self) -> Result<(), embedded_sdmmc::Error> { + pub fn init( + &mut self, + i2c0_sda_pin: Pin, + i2c0_scl_pin: Pin, + led_pin: LEDPIN, + ) -> Result<(), embedded_sdmmc::Error> { // load configuration from the SD card if self.sd_card.root_dir().is_some() { let root_dir = self.sd_card.root_dir().unwrap(); @@ -298,35 +297,29 @@ where })?; debug!("Config file contents:\n{}", config_str); self.config.set_config_values(config_str); - - // set the gyro offsets from the config file - // if self.heading_calculator.is_inited() { - // self.heading_calculator.set_gyro_offsets( - // self.config.gyro_offset_x, - // self.config.gyro_offset_y, - // self.config.gyro_offset_z, - // ); - // } } else { debug!("Config file not found, using defaults"); } } else { warn!("Could not load configuration. Using defaults."); } + self.heading_core1.start_heading_calculation( + self.delay, + i2c0_sda_pin, + i2c0_scl_pin, + led_pin, + Vector3d { + x: self.config.gyro_offset_x, + y: self.config.gyro_offset_y, + z: self.config.gyro_offset_z, + }, + ); writeln!(self.logger, "Robot started\n{}\n", self.config) .map_err(|_e| embedded_sdmmc::SdCardError::WriteError)?; info!("Robot initialized\n{}\n", self.config); Ok(()) } - pub fn calibrate_gyro(&mut self, delay: &mut DELAY) { - // self.heading_calculator.calibrate(delay, |step| { - // if let Ok(lcd) = self.lcd.set_cursor(0, 1) { - // write!(lcd, "iteration: {}", step).ok(); - // } - // }); - } - /// This function is called in the main loop to allow the robot to handle state updates pub fn handle_loop(&mut self) { if self.reset_display_start_millis != 0 @@ -482,10 +475,9 @@ where millis(), 0, 0, - 100, - 100, - // self.heading_calculator.heading(), - 180.0, // FIXME: remove this hardcoded value + self.config.straight_left_power, + self.config.straight_right_power, + self.heading_core1.get_heading() as f64, 0., ), ) @@ -507,8 +499,8 @@ where last_update_millis = millis(); // let heading = self.heading_calculator.heading(); - let heading = 0.0; // FIXME: remove this hardcoded value - let control_signal = controller.update(heading as f32, last_update_millis); + let heading = self.heading_core1.get_heading(); + let control_signal = controller.update(heading, last_update_millis); let mut cs_indicator: &str = direction_arrow; // positive control signal means turn left, a negative control signal means turn right @@ -535,12 +527,11 @@ where right_wheel_ticks, left_power as u8, right_power as u8, - heading, + heading as f64, control_signal, ), ) .ok(); - // self.heading_calculator.update(); let wheel_ticks_per_mm = self.config.wheel_ticks_per_mm; if let Err(error) = core::write!( @@ -552,7 +543,6 @@ where ) { error!("Error writing to LCD: {}", error.to_string().as_str()); } - // self.heading_calculator.update(); if let Err(error) = core::write!( self.set_lcd_cursor(0, 1), "{} : cs={:.3}", @@ -589,8 +579,7 @@ where right_wheel_ticks, 0, 0, - // self.heading_calculator.heading(), - 0.0, // FIXME: remove this hardcoded value + self.heading_core1.get_heading() as f64, 0., ), ) @@ -636,10 +625,9 @@ where ) { error!("Error writing to LCD: {}", error.to_string().as_str()); } - // self.heading_calculator.reset(); + self.heading_core1.reset_heading(); self.handle_loop(); - // let mut current_angle = self.heading_calculator.heading() as f32; - let mut current_angle = 0.0; // FIXME: remove this hardcoded value + let mut current_angle = self.heading_core1.get_heading(); let mut last_adjust_angle = current_angle; // start the motors per right hand rule (postive angle = left turn, negative angle = right turn) @@ -665,8 +653,7 @@ where }; while current_angle.abs() < (turn_degrees.abs() + stop_angle_delta) as f32 { - // current_angle = self.heading_calculator.heading() as f32; - current_angle = 0.0; // FIXME: remove this hardcoded value + current_angle = self.heading_core1.get_heading(); if (current_angle - last_adjust_angle).abs() > 10.0 { last_adjust_angle = current_angle; if let Err(error) = core::write!( @@ -682,13 +669,11 @@ where self.handle_loop(); } - // let motors_off_heading = self.heading_calculator.heading(); - let motors_off_heading = 0.0; // FIXME: remove this hardcoded value + let motors_off_heading = self.heading_core1.get_heading(); self.motors.brake(); self.delay.delay_ms(100); self.motors.stop(); - // current_angle = self.heading_calculator.heading() as f32; - current_angle = 0.0; // FIXME: remove this hardcoded value + current_angle = self.heading_core1.get_heading(); if let Err(error) = core::write!( self.clear_lcd().set_lcd_cursor(0, 0), "{} {} / {}\x03", @@ -721,6 +706,7 @@ where &mut self, ) -> Result<(), i2c_character_display::CharacterDisplayError>> { + self.heading_core1.reset_heading(); write!(self.lcd.clear()?.set_cursor(0, 0)?, "Heading:").ok(); // self.heading_calculator.reset(); let mut continue_loop = true; @@ -732,15 +718,6 @@ where continue_loop = false; } if millis() - last_update_millis > 200 { - // let heading = self.heading_calculator.heading(); - // if let Err(error) = core::write!( - // self.lcd.set_cursor(0, 1)?, - // "{:.2}{: <16}", - // heading, - // DEGREES_STRING - // ) { - // error!("Error writing to LCD: {}", error.to_string().as_str()); - // } let heading = self.heading_core1.get_heading(); if let Err(e) = core::write!( self.lcd.set_cursor(0, 1)?, diff --git a/src/robot/file_storage.rs b/src/robot/file_storage.rs index f639339..b2a4f1c 100644 --- a/src/robot/file_storage.rs +++ b/src/robot/file_storage.rs @@ -1,6 +1,5 @@ #![allow(clippy::type_complexity)] pub mod logger; -pub mod sd_card_spi_device; pub mod sd_file; use alloc::{rc::Rc, string::ToString, vec::Vec}; diff --git a/src/robot/file_storage/sd_card_spi_device.rs b/src/robot/file_storage/sd_card_spi_device.rs deleted file mode 100644 index 83d6101..0000000 --- a/src/robot/file_storage/sd_card_spi_device.rs +++ /dev/null @@ -1,146 +0,0 @@ -use core::cell::RefCell; - -use critical_section::Mutex; -use defmt::{error, trace}; -use embedded_hal::{ - delay::DelayNs, - digital::OutputPin, - spi::{ErrorType, Operation, SpiBus, SpiDevice}, -}; - -#[derive(Debug)] -pub enum SDCardSPIDeviceError { - Spi, - Delay, - ChipSelect, // Add other error variants as needed -} - -impl embedded_hal::spi::Error for SDCardSPIDeviceError { - fn kind(&self) -> embedded_hal::spi::ErrorKind { - match self { - SDCardSPIDeviceError::Spi => embedded_hal::spi::ErrorKind::Other, - SDCardSPIDeviceError::Delay => embedded_hal::spi::ErrorKind::Other, - SDCardSPIDeviceError::ChipSelect => embedded_hal::spi::ErrorKind::ChipSelectFault, - // Map other error variants as needed - } - } -} - -impl core::fmt::Display for SDCardSPIDeviceError { - fn fmt(&self, f: &mut core::fmt::Formatter) -> core::fmt::Result { - match self { - SDCardSPIDeviceError::Spi => write!(f, "SD Card SPI bus error"), - SDCardSPIDeviceError::Delay => write!(f, "SD Card Delay error"), - SDCardSPIDeviceError::ChipSelect => write!(f, "SD Card chip select error"), - // Map other error variants as needed - }?; - Ok(()) - } -} - -pub struct SDCardSPIDevice<'a, SPI, CS, DELAY> -where - SPI: SpiBus, - CS: OutputPin, - DELAY: DelayNs, -{ - pub bus: embedded_hal_bus::spi::CriticalSectionDevice<'a, SPI, CS, DELAY>, -} - -impl<'a, SPI, CS, DELAY> ErrorType for SDCardSPIDevice<'a, SPI, CS, DELAY> -where - SPI: SpiBus, - CS: OutputPin, - DELAY: DelayNs, -{ - type Error = SDCardSPIDeviceError; -} - -impl<'a, SPI, CS, DELAY> SpiDevice for SDCardSPIDevice<'a, SPI, CS, DELAY> -where - SPI: SpiBus, - CS: OutputPin, - DELAY: DelayNs, -{ - fn transaction(&mut self, operations: &mut [Operation<'_, u8>]) -> Result<(), Self::Error> { - // trace!( - // "SDCardSPIDevice::transaction - operations.len = {}", - // operations.len() - // ); - // // Implement the SPI transaction here - // if self.cs.set_low().is_err() { - // return Err(SDCardSPIDeviceError::ChipSelect); - // } - // for operation in operations { - // if let Err(_e) = match operation { - // Operation::Write(data) => { - // trace!( - // "SDCardSPIDevice::transaction - write data.len = {}", - // data.len() - // ); - // self.bus.write(data) - // } - // Operation::Transfer(read_buf, write_buf) => { - // trace!("SDCardSPIDevice::transaction - transfer read_buf.len = {}, write_buf.len = {}", read_buf.len(), write_buf.len()); - // self.bus.transfer(read_buf, write_buf) - // } - // Operation::Read(data) => { - // trace!( - // "SDCardSPIDevice::transaction - read data.len = {}", - // data.len() - // ); - // self.bus.read(data) - // } - // Operation::TransferInPlace(data) => { - // trace!( - // "SDCardSPIDevice::transaction - transfer_in_place data.len = {}", - // data.len() - // ); - // self.bus.transfer_in_place(data) - // } - // Operation::DelayNs(time) => { - // trace!("SDCardSPIDevice::transaction - delay_ns time = {}", time); - // self.delay.delay_ns(*time); - // Ok(()) - // } - // } { - // // if an error occurs, deassert CS pin and return error - // self.cs.set_high().ok(); - // error!("SDCardSPIDevice::transaction - error"); - // return Err(SDCardSPIDeviceError::Spi); - // } - // } - // if self.bus.flush().is_err() { - // self.cs.set_high().ok(); - // error!("SDCardSPIDevice::transaction - flush error"); - // return Err(SDCardSPIDeviceError::Spi); - // } - // if self.cs.set_high().is_err() { - // error!("SDCardSPIDevice::transaction - chip select error"); - // return Err(SDCardSPIDeviceError::ChipSelect); - // } - // Ok(()) - self.bus - .transaction(operations) - .map_err(|_| SDCardSPIDeviceError::Spi) - } -} - -impl<'a, SPI, CS, DELAY> SDCardSPIDevice<'a, SPI, CS, DELAY> -where - SPI: SpiBus, - CS: OutputPin, - DELAY: DelayNs, -{ - pub fn new(bus: &'a Mutex>, cs: CS, delay: DELAY) -> Self { - Self { - bus: embedded_hal_bus::spi::CriticalSectionDevice::new(bus, cs, delay), - } - } - - pub fn spi_device( - &mut self, - ) -> &mut embedded_hal_bus::spi::CriticalSectionDevice<'a, SPI, CS, DELAY> { - &mut self.bus - } -} diff --git a/src/system/millis.rs b/src/system/millis.rs index c2a5633..c73fd7a 100644 --- a/src/system/millis.rs +++ b/src/system/millis.rs @@ -28,6 +28,7 @@ pub fn millis() -> u64 { }) } +#[allow(dead_code)] pub fn millis_tenths() -> u64 { cortex_m::interrupt::free(|cs| { MILLIS_TIMER From 646e34cae7812d441500649c51c72fcdabc149a3 Mon Sep 17 00:00:00 2001 From: Michael Kamprath Date: Fri, 29 Nov 2024 11:00:38 -0800 Subject: [PATCH 3/4] linting issues --- src/main.rs | 5 ++++- src/robot.rs | 6 ++---- src/system/data.rs | 4 ++-- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main.rs b/src/main.rs index fa60829..8ce8b11 100644 --- a/src/main.rs +++ b/src/main.rs @@ -39,7 +39,10 @@ fn main() -> ! { use core::mem::MaybeUninit; const HEAP_SIZE: usize = 4048; static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; - unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + #[allow(static_mut_refs)] + unsafe { + HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) + } } info!("Program start"); diff --git a/src/robot.rs b/src/robot.rs index e7b2147..fffa622 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -752,7 +752,6 @@ where /// Implements the defmt::Format trait for the Robot struct, allowing the Robot object to be printed with defmt impl< - 'a, INA1: OutputPin, INA2: OutputPin, INB1: OutputPin, @@ -764,7 +763,7 @@ impl< TWI1, SPI_DEV: SpiDevice, DELAY, - > Format for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> + > Format for Robot<'_, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> where TWI1: I2c, DELAY: DelayNs + Clone, @@ -781,7 +780,6 @@ where /// Implement the `core::fmt::Write` trait for the robot, allowing us to use the `core::write!` macro impl< - 'a, INA1: OutputPin, INA2: OutputPin, INB1: OutputPin, @@ -794,7 +792,7 @@ impl< SPI_DEV: SpiDevice, DELAY, > core::fmt::Write - for Robot<'a, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> + for Robot<'_, INA1, INA2, INB1, INB2, ENA, ENB, BUTT1, BUTT2, TWI1, SPI_DEV, DELAY> where TWI1: I2c, DELAY: DelayNs + Clone, diff --git a/src/system/data.rs b/src/system/data.rs index 7ce1a38..b9bff83 100644 --- a/src/system/data.rs +++ b/src/system/data.rs @@ -153,7 +153,7 @@ where } } -impl<'a, T, const M: usize> Format for DataTable<'a, T, M> +impl Format for DataTable<'_, T, M> where T: Copy + Default + Format, { @@ -173,7 +173,7 @@ where } } -impl<'a, T, const M: usize> core::fmt::Display for DataTable<'a, T, M> +impl core::fmt::Display for DataTable<'_, T, M> where T: Copy + Default + Format + core::fmt::Display, { From c203504f77596a44876c438dff7085ce4718aeec Mon Sep 17 00:00:00 2001 From: Michael Kamprath Date: Fri, 29 Nov 2024 11:04:04 -0800 Subject: [PATCH 4/4] linting issues again --- src/model/heading_core1.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/model/heading_core1.rs b/src/model/heading_core1.rs index b5d3a73..7245913 100644 --- a/src/model/heading_core1.rs +++ b/src/model/heading_core1.rs @@ -72,6 +72,7 @@ impl<'a> HeadingManager<'a> { // spawn the heading calculation task on core1 let sys_freq = self.sys_freq; // value that can be moved into the closure let mut timer = timer; + #[allow(static_mut_refs)] let _res = self.core1.spawn(unsafe { &mut CORE1_STACK.mem }, move || { // LED is used to indicate that the gyro is good to go. make sure it is off to start if let Err(_e) = led_pin.set_low() {