IMU refactor

control-board/Cargo.toml

@@ -35,7 +35,7 @@ embassy-futures = { version = "0.1.0" }
 futures-util = { version = "0.3.17", default-features = false }
 embassy-sync = { version = "0.1.0" }
 panic-probe = { version = "0.3", features = ["print-defmt"] }
-static_cell = "1.0"
+static_cell = { version = "2.0.0", features = ["nightly"] }
 critical-section = "1.1.1"
 const_format = "0.2.30"
 heapless = "0.7.16"
@@ -63,6 +63,15 @@ debug-assertions = false
 debug = true
 lto = 'fat'
 
+[profile.dev-inspect]
+inherits = "dev"
+debug = true
+opt-level = 0
+lto = false
+debug-assertions = true
+overflow-checks = true
+strip = "none"
+
 [lib]
 test = false
 harness = false

control-board/src/bin/control/control.rs

@@ -15,7 +15,7 @@ use ateam_control_board::{
         robot_model::{RobotConstants, RobotModel},
         robot_controller::BodyVelocityController
     },
-    BATTERY_MIN_VOLTAGE, parameter_interface::ParameterInterface
+    BATTERY_MIN_VOLTAGE, parameter_interface::ParameterInterface, tasks::imu::{GyroSub, AccelSub}
 };
 use nalgebra::{Vector3, Vector4};
 
@@ -176,7 +176,10 @@ pub struct Control<'a> {
         MotorDResetPin,
     >,
     ticks_since_packet: u16,
-    gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>,
+    gyro_sub: GyroSub<'a>,
+    accel_sub: AccelSub<'a>,
+    last_gyro_vals: Vector3<f32>,
+    last_accel_vals: Vector3<f32>,
     battery_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>
 }
 
@@ -201,7 +204,8 @@ impl<'a> Control<'a> {
         back_right_reset_pin: MotorBRResetPin,
         drib_reset_pin: MotorDResetPin,
         ball_detected_thresh: f32,
-        gyro_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>,
+        gyro_sub: GyroSub<'a>,
+        accel_sub: AccelSub<'a>,
         battery_sub: Subscriber<'static, ThreadModeRawMutex, f32, 2, 2, 2>
     ) -> Control<'a> {
         let wheel_firmware_image = WHEEL_FW_IMG;
@@ -382,6 +386,9 @@ impl<'a> Control<'a> {
             drib_motor,
             ticks_since_packet: 0,
             gyro_sub,
+            accel_sub,
+            last_gyro_vals: Vector3::new(0., 0., 0.),
+            last_accel_vals: Vector3::new(0., 0., 0.),
             battery_sub
         }
     }
@@ -478,10 +485,20 @@ impl<'a> Control<'a> {
             }
         }
 
+        // update with any new gyro vals
+        if let Some(gyro_vals) = self.gyro_sub.try_next_message_pure() {
+            self.last_gyro_vals = gyro_vals;
+        }
+
+        // update with any new accel vals
+        if let Some(accel_vals) = self.accel_sub.try_next_message_pure() {
+            self.last_accel_vals = accel_vals;
+        }
+
         // now we have setpoint r(t) in self.cmd_vel
         let battery_v = self.battery_sub.next_message_pure().await as f32;
         let controls_enabled = true;
-        let gyro_rads = (self.gyro_sub.next_message_pure().await as f32) * 2.0 * core::f32::consts::PI / 360.0;
+
         let wheel_vels = if battery_v > BATTERY_MIN_VOLTAGE {
             if controls_enabled 
             {
@@ -504,7 +521,7 @@ impl<'a> Control<'a> {
 
                 // TODO read from channel or something
 
-                self.robot_controller.control_update(&self.cmd_vel, &wheel_vels, &wheel_torques, gyro_rads);
+                self.robot_controller.control_update(&self.cmd_vel, &wheel_vels, &wheel_torques, self.last_gyro_vals[2]);
 
                 self.robot_controller.get_wheel_velocities()
             } 

control-board/src/bin/control/main.rs

@@ -13,7 +13,7 @@ use ateam_control_board::{
     queue::Buffer,
     stm32_interface::{get_bootloader_uart_config, Stm32Interface},
     uart_queue::{UartReadQueue, UartWriteQueue},
-    BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE, include_kicker_bin, parameter_interface::ParameterInterface,
+    BATTERY_BUFFER_SIZE, BATTERY_MAX_VOLTAGE, BATTERY_MIN_VOLTAGE, include_kicker_bin, parameter_interface::ParameterInterface, tasks,
 };
 use control::Control;
 use defmt::info;
@@ -80,10 +80,6 @@ static RADIO_TEST: RadioTest<
     RadioReset,
 > = RadioTest::new(unsafe { &mut BUFFERS_TX }, unsafe { &mut BUFFERS_RX });
 
-// pub sub channel for the gyro vals
-// CAP queue size, n_subs, n_pubs
-static GYRO_CHANNEL: PubSubChannel<ThreadModeRawMutex, f32, 2, 2, 2> = PubSubChannel::new();
-
 // pub sub channel for the battery raw adc vals
 // CAP queue size, n_subs, n_pubs
 static BATTERY_CHANNEL: PubSubChannel<ThreadModeRawMutex, f32, 2, 2, 2> = PubSubChannel::new();
@@ -202,42 +198,13 @@ async fn main(_spawner: embassy_executor::Spawner) {
         [BATTERY_MAX_VOLTAGE; BATTERY_BUFFER_SIZE];
     let battery_pub = BATTERY_CHANNEL.publisher().unwrap();
 
-    let mut imu_spi = spi::Spi::new(
-        p.SPI6,
-        p.PA5,
-        p.PA7,
-        p.PA6,
-        p.BDMA_CH0,
-        p.BDMA_CH1,
-        hz(1_000_000),
-        spi::Config::default(),
-    );
-
-    // acceleromter
-    let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh);
-    // gyro
-    let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh);
+    tasks::imu::start_imu_task(_spawner, p.SPI6, p.PA5, p.PA7, p.PA6, p.BDMA_CH0, p.BDMA_CH1, p.PC4, p.PC5, p.PB1, p.PB2, p.EXTI1, p.EXTI2).expect("unable to start IMU task");
+    let accel_sub = tasks::imu::get_accel_sub().expect("accel data channel had no subscribers left");
+    let gyro_sub = tasks::imu::get_gyro_sub().expect("gyro data channel had no subscribers left");
 
+    // TODO remove?
     Timer::after(Duration::from_millis(1)).await;
 
-    let gyro_pub = GYRO_CHANNEL.publisher().unwrap();
-    unsafe {
-        SPI6_BUF[0] = 0x80;
-        // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]);
-        imu_cs1.set_low();
-        let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await;
-        imu_cs1.set_high();
-        let accel_id = SPI6_BUF[1];
-        info!("accelerometer id: 0x{:x}", accel_id);
-
-        SPI6_BUF[0] = 0x80;
-        imu_cs2.set_low();
-        let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await;
-        imu_cs2.set_high();
-        let gyro_id = SPI6_BUF[1];
-        info!("gyro id: 0x{:x}", gyro_id);
-    }
-
     let front_right_int = interrupt::take!(USART1);
     let front_left_int = interrupt::take!(UART4);
     let back_left_int = interrupt::take!(UART7);
@@ -290,7 +257,6 @@ async fn main(_spawner: embassy_executor::Spawner) {
         get_bootloader_uart_config(),
     );
 
-    let gyro_sub = GYRO_CHANNEL.subscriber().unwrap();
     let battery_sub = BATTERY_CHANNEL.subscriber().unwrap();
 
     if kicker_det.is_low() {
@@ -336,6 +302,7 @@ async fn main(_spawner: embassy_executor::Spawner) {
         p.PD12,
         ball_detected_thresh,
         gyro_sub,
+        accel_sub,
         battery_sub,
     );
 
@@ -406,19 +373,6 @@ async fn main(_spawner: embassy_executor::Spawner) {
     loop {
         unsafe { wdg.pet() };
 
-        unsafe {
-            SPI6_BUF[0] = 0x86;
-            imu_cs2.set_low();
-            let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await;
-            imu_cs2.set_high();
-            let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16;
-            // info!("z rate: {}", rate_z);
-            let gyro_conversion = 2000.0 / 32767.0;
-            gyro_pub.publish_immediate((rate_z as f32) * gyro_conversion);
-        }
-
-        // could just feed gyro in here but the comment in control said to use a channel
-
         ///////////////////////
         //  Battery reading  //
         ///////////////////////

control-board/src/bin/hwtest-imu/main.rs

@@ -11,7 +11,6 @@ use panic_probe as _;
 
 use embassy_stm32::{
     dma::NoDma,
-    gpio::{Level, Output, Speed},
     spi,
     time::{hz, mhz},
 };
@@ -20,8 +19,8 @@ use embassy_time::{Duration, Timer};
 use apa102_spi::Apa102;
 use smart_leds::{SmartLedsWrite, RGB8};
 
-#[link_section = ".sram4"]
-static mut SPI6_BUF: [u8; 4] = [0x0; 4];
+use ateam_control_board::*;
+
 
 #[embassy_executor::main]
 async fn main(_spawner: embassy_executor::Spawner) {
@@ -49,50 +48,35 @@ async fn main(_spawner: embassy_executor::Spawner) {
     );
 
     let mut dotstar = Apa102::new(dot_spi);
-    let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }].iter().cloned());
-
-    let mut imu_spi = spi::Spi::new(
-        p.SPI6,
-        p.PA5,
-        p.PA7,
-        p.PA6,
-        p.BDMA_CH0,
-        p.BDMA_CH1,
-        hz(1_000_000),
-        spi::Config::default(),
-    );
+    let _ = dotstar.write([RGB8 { r: 10, g: 0, b: 0 }, RGB8 { r: 0, g: 0, b: 0 }].iter().cloned());
+
+    defmt::warn!("here!");
+
+    tasks::imu::start_imu_task(&_spawner, p.SPI6, p.PA5, p.PA7, p.PA6, p.BDMA_CH0, p.BDMA_CH1, p.PC4, p.PC5, p.PB1, p.PB2, p.EXTI1, p.EXTI2).expect("unable to start IMU task");
 
-    // // acceleromter
-    let mut imu_cs1 = Output::new(p.PC4, Level::High, Speed::VeryHigh);
-    // // gyro
-    let mut imu_cs2 = Output::new(p.PC5, Level::High, Speed::VeryHigh);
-
-    Timer::after(Duration::from_millis(1)).await;
-
-    unsafe {
-        SPI6_BUF[0] = 0x80;
-        // info!("xfer {=[u8]:x}", SPI6_BUF[0..1]);
-        imu_cs1.set_low();
-        let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await;
-        imu_cs1.set_high();
-        let accel_id = SPI6_BUF[1];
-        info!("accelerometer id: 0x{:x}", accel_id);
-
-        SPI6_BUF[0] = 0x80;
-        imu_cs2.set_low();
-        let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..2]).await;
-        imu_cs2.set_high();
-        let gyro_id = SPI6_BUF[1];
-        info!("gyro id: 0x{:x}", gyro_id);
-
-        loop {
-            SPI6_BUF[0] = 0x86;
-            // SPI6_BUF[0] = 0x86;
-            imu_cs2.set_low();
-            let _ = imu_spi.transfer_in_place(&mut SPI6_BUF[0..3]).await;
-            imu_cs2.set_high();
-            let rate_z = (SPI6_BUF[2] as u16 * 256 + SPI6_BUF[1] as u16) as i16;
-            info!("z rate: {}", rate_z);
+    defmt::warn!("here2!");
+
+    let _ = dotstar.write([RGB8 { r: 0, g: 0, b: 10 }, RGB8 { r: 0, g: 0, b: 0 }].iter().cloned());
+
+
+    let mut accel_sub = tasks::imu::get_accel_sub().expect("accel data channel had no subscribers left");
+    let mut gyro_sub = tasks::imu::get_gyro_sub().expect("gyro data channel had no subscribers left");
+
+    loop {
+        let gyro_data = gyro_sub.next_message_pure().await;
+        let accel_data = accel_sub.try_next_message_pure();
+        if let Some(accel_data) = accel_data {
+            defmt::info!("received gyro ({}, {}, {}) and accel ({}, {}, {})", gyro_data.x, gyro_data.y, gyro_data.z, accel_data.x, accel_data.y, accel_data.z);
+        } else {
+            defmt::info!("received gyro ({}, {}, {})", gyro_data.x, gyro_data.y, gyro_data.z);
         }
+
+        const MAX_ANGULAR_RATE_RADS: f32 = 34.91;  // IMU task configures for 2000d/s = 34.91 rad/s
+        let g_val_mag: u8 = ((gyro_data.z / MAX_ANGULAR_RATE_RADS) * u8::MAX as f32) as u8;
+
+        let _ = dotstar.write([
+            RGB8 { r: 0, g: if gyro_data.z > 0.0 { g_val_mag } else { 0 }, b: 0 },
+            RGB8 { r: 0, g: if gyro_data.z < 0.0 { g_val_mag } else { 0 }, b: 0 }]
+            .iter().cloned());
     }
 }

control-board/src/bin/hwtest-imu/pins.rs

@@ -37,7 +37,18 @@ pub type MotorBLResetPin = PF9;
 pub type MotorBRResetPin = PB8;
 pub type MotorDResetPin = PD12;
 
-
 pub type PowerStatePin = PF5;
 pub type PowerStateExti = EXTI5;
-pub type ShutdownCompletePin = PF4;
+pub type ShutdownCompletePin = PF4;
+
+pub type ImuSpi = SPI6;
+pub type ImuSpiMosiPin = PA5;
+pub type ImuSpiMisoPin = PA6;
+pub type ImuSpiSckPin = PA7;
+pub type ImuTxDma = BDMA_CH0;
+pub type ImuRxDma = BDMA_CH1;
+pub type ImuAccelCsPin = PC4;
+pub type ImuGyroCsPin = PC5;
+pub type ImuAccelIntPin = PB1;
+pub type ImuGyroIntPin = PB2;
+pub type ImuDetPin = PF11;