boards/kakute-f7-mini-v3: add IMU + UART debug

Signed-off-by: Onur Özkan <work@onurozkan.dev>

boards/kakute-f7-mini-v3/src/main.rs

@@ -6,9 +6,20 @@ use panic_halt as _;
 
 #[rtic::app(device = stm32f7::stm32f722, peripherals = true)]
 mod app {
+    use core::fmt::{self, Write as _};
+
+    #[derive(Clone, Copy, Debug)]
+    pub struct RawImuSample {
+        pub acceleration: [i16; 3],
+        pub temperature: i16,
+        pub angular_rate: [i16; 3],
+    }
+
     #[shared]
     struct Shared {
         imu_data_ready_count: u32,
+        heartbeat_count: u32,
+        latest_imu_sample: RawImuSample,
     }
 
     #[local]
@@ -20,8 +31,11 @@ mod app {
         imu_spi: stm32f7::stm32f722::SPI1,
         imu_chip_select: stm32f7::stm32f722::GPIOB,
         data_ready_interrupt: stm32f7::stm32f722::EXTI,
+        telemetry: Uart1,
     }
 
+    struct Uart1(stm32f7::stm32f722::USART1);
+
     #[init]
     fn init(context: init::Context) -> (Shared, Local) {
         let device = context.device;
@@ -62,7 +76,14 @@ mod app {
                     .bits(0b0101)
             }
         });
-
+        device
+            .GPIOA
+            .moder()
+            .modify(|_, write| write.moder9().alternate());
+        device.GPIOA.afrh().modify(|_, write| {
+            // SAFETY: AF7 selects USART1 transmit for PA9, routed to T1.
+            unsafe { write.afrh9().bits(0b0111) }
+        });
         device
             .RCC
             .ahb1enr()
@@ -91,9 +112,24 @@ mod app {
         device.SPI1.cr2().modify(|_, write| write.frxth().set_bit());
         device.SPI1.cr1().modify(|_, write| write.spe().set_bit());
 
+        device
+            .RCC
+            .apb2enr()
+            .modify(|_, write| write.usart1en().set_bit());
+        device.USART1.brr().write(|write| {
+            // SAFETY: 139 configures approximately 115200 baud from the 16 MHz reset clock.
+            unsafe { write.bits(139) }
+        });
+        device
+            .USART1
+            .cr1()
+            .write(|write| write.te().set_bit().ue().set_bit());
+
         let mpu6000_detected = is_mpu6000(&device.SPI1, &device.GPIOB);
         if mpu6000_detected {
             write_mpu_register(&device.SPI1, &device.GPIOB, 0x6B, 0x01);
+            write_mpu_register(&device.SPI1, &device.GPIOB, 0x1A, 0x03);
+            write_mpu_register(&device.SPI1, &device.GPIOB, 0x19, 0x09);
             write_mpu_register(&device.SPI1, &device.GPIOB, 0x37, 0);
             write_mpu_register(&device.SPI1, &device.GPIOB, 0x38, 0x01);
 
@@ -138,6 +174,12 @@ mod app {
         (
             Shared {
                 imu_data_ready_count: 0,
+                heartbeat_count: 0,
+                latest_imu_sample: RawImuSample {
+                    acceleration: [0; 3],
+                    temperature: 0,
+                    angular_rate: [0; 3],
+                },
             },
             Local {
                 led: device.GPIOD,
@@ -147,21 +189,41 @@ mod app {
                 imu_spi: device.SPI1,
                 imu_chip_select: device.GPIOB,
                 data_ready_interrupt: device.EXTI,
+                telemetry: Uart1(device.USART1),
             },
         )
     }
 
-    #[idle]
+    #[idle(
-    fn idle(_context: idle::Context) -> ! {
+        shared = [heartbeat_count, latest_imu_sample],
+        local = [telemetry]
+    )]
+    fn idle(mut context: idle::Context) -> ! {
+        let mut last_heartbeat_count = 0;
+
         loop {
-            cortex_m::asm::wfi();
+            let heartbeat_count = context.shared.heartbeat_count.lock(|count| *count);
+            if heartbeat_count == last_heartbeat_count {
+                cortex_m::asm::wfi();
+                continue;
+            }
+
+            last_heartbeat_count = heartbeat_count;
+            let imu_sample = context.shared.latest_imu_sample.lock(|sample| *sample);
+            let _ = core::writeln!(
+                context.local.telemetry,
+                "accel={:?} temp={} gyro={:?}",
+                imu_sample.acceleration,
+                imu_sample.temperature,
+                imu_sample.angular_rate,
+            );
         }
     }
 
     #[task(
         binds = EXTI4,
         priority = 15,
-        shared = [imu_data_ready_count],
+        shared = [imu_data_ready_count, latest_imu_sample],
         local = [imu_spi, imu_chip_select, data_ready_interrupt]
     )]
     fn imu_data_ready(mut context: imu_data_ready::Context) {
@@ -170,6 +232,10 @@ mod app {
             .data_ready_interrupt
             .pr()
             .write(|write| write.pr4().clear());
+        let imu_sample = read_mpu6000_sample(context.local.imu_spi, context.local.imu_chip_select);
+        context.shared.latest_imu_sample.lock(|sample| {
+            *sample = imu_sample;
+        });
         context.shared.imu_data_ready_count.lock(|count| {
             *count = count.wrapping_add(1);
         });
@@ -178,7 +244,7 @@ mod app {
     #[task(
         binds = TIM2,
         priority = 1,
-        shared = [imu_data_ready_count],
+        shared = [imu_data_ready_count, heartbeat_count],
         local = [led, led_output_high, heartbeat_timer, last_data_ready_count]
     )]
     fn led_heartbeat(mut context: led_heartbeat::Context) {
@@ -187,7 +253,9 @@ mod app {
             .heartbeat_timer
             .sr()
             .modify(|_, write| write.uif().clear_bit());
-
+        context.shared.heartbeat_count.lock(|count| {
+            *count = count.wrapping_add(1);
+        });
         let imu_data_ready_count = context.shared.imu_data_ready_count.lock(|count| *count);
         if imu_data_ready_count == *context.local.last_data_ready_count {
             *context.local.led_output_high = false;
@@ -244,4 +312,56 @@ mod app {
         while spi.sr().read().bsy().is_busy() {}
         chip_select.bsrr().write(|write| write.bs2().set_bit());
     }
+
+    fn read_mpu6000_sample(
+        spi: &stm32f7::stm32f722::SPI1,
+        chip_select: &stm32f7::stm32f722::GPIOB,
+    ) -> RawImuSample {
+        let mut bytes = [0; 14];
+
+        chip_select.bsrr().write(|write| write.br2().set_bit());
+        let _ = spi_transfer(spi, 0xBB);
+        for byte in &mut bytes {
+            *byte = spi_transfer(spi, 0);
+        }
+        while spi.sr().read().bsy().is_busy() {}
+        chip_select.bsrr().write(|write| write.bs2().set_bit());
+
+        RawImuSample {
+            acceleration: [
+                i16::from_be_bytes([bytes[0], bytes[1]]),
+                i16::from_be_bytes([bytes[2], bytes[3]]),
+                i16::from_be_bytes([bytes[4], bytes[5]]),
+            ],
+            temperature: i16::from_be_bytes([bytes[6], bytes[7]]),
+            angular_rate: [
+                i16::from_be_bytes([bytes[8], bytes[9]]),
+                i16::from_be_bytes([bytes[10], bytes[11]]),
+                i16::from_be_bytes([bytes[12], bytes[13]]),
+            ],
+        }
+    }
+
+    impl fmt::Write for Uart1 {
+        fn write_str(&mut self, string: &str) -> fmt::Result {
+            for byte in string.bytes() {
+                if byte == b'\n' {
+                    self.write_byte(b'\r');
+                }
+                self.write_byte(byte);
+            }
+
+            Ok(())
+        }
+    }
+
+    impl Uart1 {
+        fn write_byte(&mut self, byte: u8) {
+            while self.0.isr().read().txe().bit_is_clear() {}
+            self.0.tdr().write(|write| {
+                // SAFETY: `byte` fits in USART1's 9-bit data field.
+                unsafe { write.tdr().bits(byte.into()) }
+            });
+        }
+    }
 }