Completely ported compass to embassy.

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

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"

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<CriticalSectionRawMutex, Heading> = 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<I2cInterface<Twim<'static, TWISPI0>>, 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,

independent_logic/src/heading_drawing.rs

@@ -17,7 +17,8 @@ fn heading_to_line(heading: Heading, square_size: usize) -> Line {
 // draws a line always pointing towards heading 0
 pub fn draw_constant_heading<const X: usize, const Y: usize>(
     heading: Heading,
-    matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
+    matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>,
 ) {
-    draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), matrix);
+    matrix.reset_matrix();
+    draw_line::<X, Y>(&heading_to_line(Heading(-heading.0), X.min(Y)), matrix);
 }

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<const X: usize, const Y: usize, T> {
     matrix: [[T; X]; Y],
     max_point: Point,
@@ -139,6 +141,12 @@ impl<T, const X: usize, const Y: usize> From<FourQuadrantMatrix<{ X }, { Y }, T>
     }
 }
 
+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);
@@ -148,7 +156,7 @@ pub struct Line(pub Point, pub Point);
 /// extend past its edges.
 pub fn draw_line<const X: usize, const Y: usize>(
     line: &Line,
-    matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
+    matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>,
 ) {
     let mut line = *line;
     #[cfg(test)]
@@ -201,7 +209,7 @@ pub fn draw_line<const X: usize, const Y: usize>(
         dbg!(draw_point);
 
         if matrix.is_in_bounds(&draw_point) {
-            matrix[draw_point] = 1;
+            matrix[draw_point] = true;
             prev_out_of_bounds = false;
         } else {
             if !prev_out_of_bounds {
@@ -249,195 +257,195 @@ mod tests {
 
     #[test]
     fn four_quadrant_matrix() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
-        canvas[Point { x: 0, y: 0 }] = 1;
+        canvas[Point { x: 0, y: 0 }] = true;
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 0, 0, 0],
-                [0, 0, 0, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 0, 0, 0],
-                [0, 0, 0, 0, 0],
+                [false, false, false, false, false],
+                [false, false, false, false, false],
+                [false, false, true, false, false],
+                [false, false, false, false, false],
+                [false, false, false, false, false],
             ]
         );
-        canvas[Point { x: -2, y: 1 }] = 1;
+        canvas[Point { x: -2, y: 1 }] = true;
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 0, 0, 0],
-                [1, 0, 0, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 0, 0, 0],
-                [0, 0, 0, 0, 0]
+                [false, false, false, false, false],
+                [true, false, false, false, false],
+                [false, false, true, false, false],
+                [false, false, false, false, false],
+                [false, false, false, false, false]
             ]
         );
     }
 
     #[test]
     fn diagonal_unsigned_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 0, y: 4 });
         draw_line(
             &Line(Point { x: 0, y: 0 }, Point { x: 4, y: 4 }),
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 0, 0, 1],
-                [0, 0, 0, 1, 0],
-                [0, 0, 1, 0, 0],
-                [0, 1, 0, 0, 0],
-                [1, 0, 0, 0, 0],
+                [false, false, false, false, true],
+                [false, false, false, true, false],
+                [false, false, true, false, false],
+                [false, true, false, false, false],
+                [true, false, false, false, false],
             ]
         )
     }
 
     #[test]
     fn diagonal_signed_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
         draw_line(
             &Line(Point { x: -2, y: -2 }, Point { x: 2, y: 2 }),
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 0, 0, 1],
-                [0, 0, 0, 1, 0],
-                [0, 0, 1, 0, 0],
-                [0, 1, 0, 0, 0],
-                [1, 0, 0, 0, 0],
+                [false, false, false, false, true],
+                [false, false, false, true, false],
+                [false, false, true, false, false],
+                [false, true, false, false, false],
+                [true, false, false, false, false],
             ]
         )
     }
 
     #[test]
     fn diagonal_signed_both_oob_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
         draw_line(
             &Line(Point { x: -10, y: -10 }, Point { x: 10, y: 10 }),
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 0, 0, 1],
-                [0, 0, 0, 1, 0],
-                [0, 0, 1, 0, 0],
-                [0, 1, 0, 0, 0],
-                [1, 0, 0, 0, 0],
+                [false, false, false, false, true],
+                [false, false, false, true, false],
+                [false, false, true, false, false],
+                [false, true, false, false, false],
+                [true, false, false, false, false],
             ]
         );
     }
 
     #[test]
     fn diagonal_signed_first_oob_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
         draw_line(
             &Line(Point { x: -10, y: -10 }, Point { x: 2, y: 2 }),
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 0, 0, 1],
-                [0, 0, 0, 1, 0],
-                [0, 0, 1, 0, 0],
-                [0, 1, 0, 0, 0],
-                [1, 0, 0, 0, 0],
+                [false, false, false, false, true],
+                [false, false, false, true, false],
+                [false, false, true, false, false],
+                [false, true, false, false, false],
+                [true, false, false, false, false],
             ]
         );
     }
 
     #[test]
     fn diagonal_signed_second_oob_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
         draw_line(
             &Line(Point { x: -2, y: -2 }, Point { x: 10, y: 10 }),
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 0, 0, 1],
-                [0, 0, 0, 1, 0],
-                [0, 0, 1, 0, 0],
-                [0, 1, 0, 0, 0],
-                [1, 0, 0, 0, 0],
+                [false, false, false, false, true],
+                [false, false, false, true, false],
+                [false, false, true, false, false],
+                [false, true, false, false, false],
+                [true, false, false, false, false],
             ]
         );
     }
 
     #[test]
     fn vertical_signed_both_oob_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
         draw_line(
             &Line(Point { x: 0, y: -10 }, Point { x: 0, y: 10 }),
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
             ]
         );
     }
 
     #[test]
     fn vertical_signed_first_oob_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
         draw_line(
             &Line(Point { x: 0, y: -10 }, Point { x: 0, y: 0 }),
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 0, 0, 0],
-                [0, 0, 0, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
+                [false, false, false, false, false],
+                [false, false, false, false, false],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
             ]
         );
     }
 
     #[test]
     fn vertical_signed_second_oob_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
         draw_line(
             &Line(Point { x: 0, y: 0 }, Point { x: 0, y: 10 }),
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
-                [0, 0, 0, 0, 0],
-                [0, 0, 0, 0, 0],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
+                [false, false, false, false, false],
+                [false, false, false, false, false],
             ]
         );
     }
 
     #[test]
     fn cross_signed_line() {
-        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+        let mut canvas: FourQuadrantMatrix<5, 5, bool> =
             FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
         draw_line(
             &Line(Point { x: 0, y: -2 }, Point { x: 0, y: 2 }),
@@ -448,13 +456,13 @@ mod tests {
             &mut canvas,
         );
         assert_eq!(
-            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            <FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
             [
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
-                [1, 1, 1, 1, 1],
-                [0, 0, 1, 0, 0],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
+                [true, true, true, true, true],
+                [false, false, true, false, false],
+                [false, false, true, false, false],
             ]
         )
     }