diff --git a/Cargo.toml b/Cargo.toml index 4ab865d..1a272a7 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3,6 +3,7 @@ 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 ba9ad06..f4a8f23 100644 --- a/hardware_main/Cargo.toml +++ b/hardware_main/Cargo.toml @@ -20,4 +20,3 @@ 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 be277d9..14e6ada 100644 --- a/hardware_main/src/main.rs +++ b/hardware_main/src/main.rs @@ -5,17 +5,12 @@ use core::f32::consts::PI; use defmt::{debug, info}; use embassy_executor::Spawner; -use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal}; -use embassy_time::{Duration, Ticker}; +use embassy_time::Timer; use microbit_bsp::{ - LedMatrix, Microbit, - display::{Bitmap, Brightness, Frame}, - embassy_nrf::{ - bind_interrupts, - peripherals::TWISPI0, - twim::{InterruptHandler, Twim}, - }, - lsm303agr::{self, Lsm303agr, interface::I2cInterface, mode::MagContinuous}, + Microbit, + display::{Brightness, Frame}, + embassy_nrf::{bind_interrupts, peripherals::TWISPI0, twim::InterruptHandler}, + lsm303agr, motion::new_lsm303agr, }; use {defmt_rtt as _, panic_probe as _}; @@ -29,10 +24,8 @@ 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!"); @@ -69,62 +62,24 @@ async fn main(s: Spawner) { ) .await .unwrap(); - s.must_spawn(get_data(sensor)); - s.must_spawn(display_data(display)); -} -#[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 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; - } -} + Timer::after_secs(2).await; -#[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 (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 - .expect("didnt get accel data") - .xyz_mg(); + 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); - HEADING.signal(heading_from_measurement(&mag_measurement)); - ticker.next().await; + let heading = heading_from_measurement(&mag_measurement); + debug!("Attitude: {}, Heading: {}", attitude, heading.0*(180.0/PI)); } } -// 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 { +pub 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 f4b2ce0..952291c 100644 --- a/independent_logic/src/heading_drawing.rs +++ b/independent_logic/src/heading_drawing.rs @@ -17,8 +17,7 @@ fn heading_to_line(heading: Heading, square_size: usize) -> Line { // draws a line always pointing towards heading 0 pub fn draw_constant_heading( heading: Heading, - matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>, + matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>, ) { - matrix.reset_matrix(); - draw_line::(&heading_to_line(Heading(-heading.0), X.min(Y)), matrix); + draw_line::(&heading_to_line(heading, X.min(Y)), matrix); } diff --git a/independent_logic/src/line_drawing.rs b/independent_logic/src/line_drawing.rs index 25966d0..d34c765 100644 --- a/independent_logic/src/line_drawing.rs +++ b/independent_logic/src/line_drawing.rs @@ -5,10 +5,8 @@ use core::{ #[cfg(test)] use std::dbg; -use defmt::Format; - /// a signed point in 2d space -#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct Point { pub x: isize, pub y: isize, @@ -27,7 +25,7 @@ impl Point { } /// an unsigned point in 2d space -#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct UPoint { pub x: usize, pub y: usize, @@ -47,7 +45,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, Format, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct FourQuadrantMatrix { matrix: [[T; X]; Y], max_point: Point, @@ -141,12 +139,6 @@ 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); @@ -156,7 +148,7 @@ pub struct Line(pub Point, pub Point); /// extend past its edges. pub fn draw_line( line: &Line, - matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>, + matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>, ) { let mut line = *line; #[cfg(test)] @@ -209,7 +201,7 @@ pub fn draw_line( dbg!(draw_point); if matrix.is_in_bounds(&draw_point) { - matrix[draw_point] = true; + matrix[draw_point] = 1; prev_out_of_bounds = false; } else { if !prev_out_of_bounds { @@ -257,195 +249,195 @@ mod tests { #[test] fn four_quadrant_matrix() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); - canvas[Point { x: 0, y: 0 }] = true; + canvas[Point { x: 0, y: 0 }] = 1; assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); - canvas[Point { x: -2, y: 1 }] = true; + canvas[Point { x: -2, y: 1 }] = 1; assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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] + [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] ] ); } #[test] fn diagonal_unsigned_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 0, y: 4 }); draw_line( &Line(Point { x: 0, y: 0 }, Point { x: 4, y: 4 }), &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ) } #[test] fn diagonal_signed_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); draw_line( &Line(Point { x: -2, y: -2 }, Point { x: 2, y: 2 }), &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ) } #[test] fn diagonal_signed_both_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); draw_line( &Line(Point { x: -10, y: -10 }, Point { x: 10, y: 10 }), &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn diagonal_signed_first_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); draw_line( &Line(Point { x: -10, y: -10 }, Point { x: 2, y: 2 }), &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn diagonal_signed_second_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); draw_line( &Line(Point { x: -2, y: -2 }, Point { x: 10, y: 10 }), &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn vertical_signed_both_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); draw_line( &Line(Point { x: 0, y: -10 }, Point { x: 0, y: 10 }), &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn vertical_signed_first_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); draw_line( &Line(Point { x: 0, y: -10 }, Point { x: 0, y: 0 }), &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn vertical_signed_second_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); draw_line( &Line(Point { x: 0, y: 0 }, Point { x: 0, y: 10 }), &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn cross_signed_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); draw_line( &Line(Point { x: 0, y: -2 }, Point { x: 0, y: 2 }), @@ -456,13 +448,13 @@ mod tests { &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ) }