From 24c877382088fc782ac542c4a3e45b44565ebe71 Mon Sep 17 00:00:00 2001 From: Gabe Venberg Date: Fri, 11 Jul 2025 14:32:56 +0200 Subject: [PATCH] Completely ported compass to embassy. --- Cargo.toml | 1 - hardware_main/Cargo.toml | 1 + hardware_main/src/main.rs | 83 ++++++++++++++++++------ independent_logic/src/heading_drawing.rs | 3 +- independent_logic/src/line_drawing.rs | 14 +++- 5 files changed, 78 insertions(+), 24 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 1a272a7..4ab865d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3,7 +3,6 @@ members = ["hardware_main", "independent_logic"] resolver = "2" [profile.release] -strip = true # Automatically strip symbols from the binary. opt-level = "z" # Optimize for size. lto = true codegen-units = 1 diff --git a/hardware_main/Cargo.toml b/hardware_main/Cargo.toml index f4a8f23..ba9ad06 100644 --- a/hardware_main/Cargo.toml +++ b/hardware_main/Cargo.toml @@ -20,3 +20,4 @@ embassy-sync = { version = "0.7", features = ["defmt"] } microbit-bsp = { git = "https://github.com/lulf/microbit-bsp.git", rev = "19d555bfbbcfa39db6aac467673386662c39e299" } libm = "0.2.15" +embedded-hal-async = "1.0.0" diff --git a/hardware_main/src/main.rs b/hardware_main/src/main.rs index 14e6ada..be277d9 100644 --- a/hardware_main/src/main.rs +++ b/hardware_main/src/main.rs @@ -5,12 +5,17 @@ use core::f32::consts::PI; use defmt::{debug, info}; use embassy_executor::Spawner; -use embassy_time::Timer; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal}; +use embassy_time::{Duration, Ticker}; use microbit_bsp::{ - Microbit, - display::{Brightness, Frame}, - embassy_nrf::{bind_interrupts, peripherals::TWISPI0, twim::InterruptHandler}, - lsm303agr, + LedMatrix, Microbit, + display::{Bitmap, Brightness, Frame}, + embassy_nrf::{ + bind_interrupts, + peripherals::TWISPI0, + twim::{InterruptHandler, Twim}, + }, + lsm303agr::{self, Lsm303agr, interface::I2cInterface, mode::MagContinuous}, motion::new_lsm303agr, }; use {defmt_rtt as _, panic_probe as _}; @@ -24,8 +29,10 @@ use independent_logic::{ }, }; +static HEADING: Signal = Signal::new(); + #[embassy_executor::main] -async fn main(_s: Spawner) { +async fn main(s: Spawner) { let board = Microbit::default(); defmt::info!("Application started!"); @@ -62,24 +69,62 @@ async fn main(_s: Spawner) { ) .await .unwrap(); + s.must_spawn(get_data(sensor)); + s.must_spawn(display_data(display)); +} - Timer::after_secs(2).await; - +#[embassy_executor::task] +async fn display_data(mut display: LedMatrix) { + let mut display_matrix: FourQuadrantMatrix<5, 5, bool> = + FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); loop { - let (x, y, z) = sensor.magnetic_field().await.unwrap().xyz_nt(); - let mag_measurement = to_ned(x, y, z); - let (x, y, z) = sensor.acceleration().await.unwrap().xyz_mg(); - let accel_measurement = to_ned(x, y, z); - debug!("Mag: {}, Accel: {}", mag_measurement, accel_measurement); - Timer::after_millis(250).await; - let attitude = calc_attitude(&accel_measurement); - let mag_measurement = calc_tilt_calibrated_measurement(mag_measurement, &attitude); - let heading = heading_from_measurement(&mag_measurement); - debug!("Attitude: {}, Heading: {}", attitude, heading.0*(180.0/PI)); + let heading = HEADING.wait().await; + info!("Heading: {}", heading.0 * (180.0 / PI)); + draw_constant_heading(heading, &mut display_matrix); + display + .display(to_frame(&display_matrix), Duration::from_hz(25)) + .await; } } -pub fn to_ned(x: i32, y: i32, z: i32) -> NedMeasurement { +#[embassy_executor::task] +async fn get_data(mut sensor: Lsm303agr>, MagContinuous>) { + let mut ticker = Ticker::every(Duration::from_hz(25)); + loop { + let (x, y, z) = sensor + .magnetic_field() + .await + .expect("didnt get mag data") + .xyz_nt(); + let mag_measurement = to_ned(x, y, z); + let (x, y, z) = sensor + .acceleration() + .await + .expect("didnt get accel data") + .xyz_mg(); + let accel_measurement = to_ned(x, y, z); + debug!("Mag: {}, Accel: {}", mag_measurement, accel_measurement); + let attitude = calc_attitude(&accel_measurement); + let mag_measurement = calc_tilt_calibrated_measurement(mag_measurement, &attitude); + HEADING.signal(heading_from_measurement(&mag_measurement)); + ticker.next().await; + } +} + +// TODO: make the line drawing lib produce a slice of bitmaps directly. +fn to_frame(matrix: &FourQuadrantMatrix<5, 5, bool>) -> Frame<5, 5> { + Frame::new( + core::convert::Into::<&[[bool; 5]; 5]>::into(matrix).map(|bools| { + let mut bit: u8 = 0; + for (i, bool) in bools.into_iter().enumerate() { + bit |= (bool as u8) << i; + } + Bitmap::new(bit, 5) + }), + ) +} + +fn to_ned(x: i32, y: i32, z: i32) -> NedMeasurement { NedMeasurement { x: -y as f32, y: x as f32, diff --git a/independent_logic/src/heading_drawing.rs b/independent_logic/src/heading_drawing.rs index 31652a3..f4b2ce0 100644 --- a/independent_logic/src/heading_drawing.rs +++ b/independent_logic/src/heading_drawing.rs @@ -19,5 +19,6 @@ pub fn draw_constant_heading( heading: Heading, matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>, ) { - draw_line::(&heading_to_line(heading, X.min(Y)), matrix); + matrix.reset_matrix(); + draw_line::(&heading_to_line(Heading(-heading.0), X.min(Y)), matrix); } diff --git a/independent_logic/src/line_drawing.rs b/independent_logic/src/line_drawing.rs index d2caaa6..25966d0 100644 --- a/independent_logic/src/line_drawing.rs +++ b/independent_logic/src/line_drawing.rs @@ -5,8 +5,10 @@ use core::{ #[cfg(test)] use std::dbg; +use defmt::Format; + /// a signed point in 2d space -#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] pub struct Point { pub x: isize, pub y: isize, @@ -25,7 +27,7 @@ impl Point { } /// an unsigned point in 2d space -#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] pub struct UPoint { pub x: usize, pub y: usize, @@ -45,7 +47,7 @@ impl UPoint { /// A matrix that allows negative co-oordinates. Will panic if referencing out of bounds, just like /// a normal 2d array. -#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] pub struct FourQuadrantMatrix { matrix: [[T; X]; Y], max_point: Point, @@ -139,6 +141,12 @@ impl From } } +impl<'a, T, const X: usize, const Y: usize> From<&'a FourQuadrantMatrix<{ X }, { Y }, T>> for &'a [[T; X]; Y] { + fn from(value:&'a FourQuadrantMatrix<{ X }, { Y }, T>) -> Self { + &value.matrix + } +} + /// a line segment in 2d space, described by its two endpoints #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct Line(pub Point, pub Point);