Gabe/micro-bit-led-compass
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Compare commits

base: Gabe:24c877382088fc782ac542c4a3e45b44565ebe71
Branches Tags
Gabe:main
Gabe:embassy
..
compare: Gabe:45578958a673710f32a8326a998fb37cde125809
Branches Tags
Gabe:embassy
Gabe:main

No commits in common. "24c877382088fc782ac542c4a3e45b44565ebe71" and "45578958a673710f32a8326a998fb37cde125809" have entirely different histories.
24c8773820 ... 45578958a6

5 changed files with 99 additions and 153 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Cargo.toml
View file

@ -3,6 +3,7 @@ members = ["hardware_main", "independent_logic"]
resolver = "2" resolver = "2"
[profile.release] [profile.release]
strip = true # Automatically strip symbols from the binary.
opt-level = "z" # Optimize for size. opt-level = "z" # Optimize for size.
lto = true lto = true
codegen-units = 1 codegen-units = 1

1
hardware_main/Cargo.toml
View file

@ -20,4 +20,3 @@ embassy-sync = { version = "0.7", features = ["defmt"] }
microbit-bsp = { git = "https://github.com/lulf/microbit-bsp.git", rev = "19d555bfbbcfa39db6aac467673386662c39e299" } microbit-bsp = { git = "https://github.com/lulf/microbit-bsp.git", rev = "19d555bfbbcfa39db6aac467673386662c39e299" }
libm = "0.2.15" libm = "0.2.15"
embedded-hal-async = "1.0.0"

71
hardware_main/src/main.rs
View file

@ -5,17 +5,12 @@
use core::f32::consts::PI; use core::f32::consts::PI;
use defmt::{debug, info}; use defmt::{debug, info};
use embassy_executor::Spawner; use embassy_executor::Spawner;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal}; use embassy_time::Timer;
use embassy_time::{Duration, Ticker};
use microbit_bsp::{ use microbit_bsp::{
LedMatrix, Microbit, Microbit,
display::{Bitmap, Brightness, Frame}, display::{Brightness, Frame},
embassy_nrf::{ embassy_nrf::{bind_interrupts, peripherals::TWISPI0, twim::InterruptHandler},
bind_interrupts, lsm303agr,
peripherals::TWISPI0,
twim::{InterruptHandler, Twim},
},
lsm303agr::{self, Lsm303agr, interface::I2cInterface, mode::MagContinuous},
motion::new_lsm303agr, motion::new_lsm303agr,
}; };
use {defmt_rtt as _, panic_probe as _}; use {defmt_rtt as _, panic_probe as _};
@ -29,10 +24,8 @@ use independent_logic::{
}, },
}; };
static HEADING: Signal<CriticalSectionRawMutex, Heading> = Signal::new();
#[embassy_executor::main] #[embassy_executor::main]
async fn main(s: Spawner) { async fn main(_s: Spawner) {
let board = Microbit::default(); let board = Microbit::default();
defmt::info!("Application started!"); defmt::info!("Application started!");
@ -69,62 +62,24 @@ async fn main(s: Spawner) {
) )
.await .await
.unwrap(); .unwrap();
s.must_spawn(get_data(sensor));
s.must_spawn(display_data(display));
}
#[embassy_executor::task] Timer::after_secs(2).await;
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;
}
}
#[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 { loop {
let (x, y, z) = sensor let (x, y, z) = sensor.magnetic_field().await.unwrap().xyz_nt();
.magnetic_field()
.await
.expect("didnt get mag data")
.xyz_nt();
let mag_measurement = to_ned(x, y, z); let mag_measurement = to_ned(x, y, z);
let (x, y, z) = sensor let (x, y, z) = sensor.acceleration().await.unwrap().xyz_mg();
.acceleration()
.await
.expect("didnt get accel data")
.xyz_mg();
let accel_measurement = to_ned(x, y, z); let accel_measurement = to_ned(x, y, z);
debug!("Mag: {}, Accel: {}", mag_measurement, accel_measurement); debug!("Mag: {}, Accel: {}", mag_measurement, accel_measurement);
Timer::after_millis(250).await;
let attitude = calc_attitude(&accel_measurement); let attitude = calc_attitude(&accel_measurement);
let mag_measurement = calc_tilt_calibrated_measurement(mag_measurement, &attitude); let mag_measurement = calc_tilt_calibrated_measurement(mag_measurement, &attitude);
HEADING.signal(heading_from_measurement(&mag_measurement)); let heading = heading_from_measurement(&mag_measurement);
ticker.next().await; debug!("Attitude: {}, Heading: {}", attitude, heading.0*(180.0/PI));
} }
} }
// TODO: make the line drawing lib produce a slice of bitmaps directly. pub fn to_ned(x: i32, y: i32, z: i32) -> NedMeasurement {
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 { NedMeasurement {
x: -y as f32, x: -y as f32,
y: x as f32, y: x as f32,

5
independent_logic/src/heading_drawing.rs
View file

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

174
independent_logic/src/line_drawing.rs
View file

@ -5,10 +5,8 @@ use core::{
#[cfg(test)] #[cfg(test)]
use std::dbg; use std::dbg;
use defmt::Format;
/// a signed point in 2d space /// a signed point in 2d space
#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] #[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Point { pub struct Point {
pub x: isize, pub x: isize,
pub y: isize, pub y: isize,
@ -27,7 +25,7 @@ impl Point {
} }
/// an unsigned point in 2d space /// an unsigned point in 2d space
#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] #[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct UPoint { pub struct UPoint {
pub x: usize, pub x: usize,
pub y: 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 matrix that allows negative co-oordinates. Will panic if referencing out of bounds, just like
/// a normal 2d array. /// a normal 2d array.
#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] #[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct FourQuadrantMatrix<const X: usize, const Y: usize, T> { pub struct FourQuadrantMatrix<const X: usize, const Y: usize, T> {
matrix: [[T; X]; Y], matrix: [[T; X]; Y],
max_point: Point, max_point: Point,
@ -141,12 +139,6 @@ 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 /// a line segment in 2d space, described by its two endpoints
#[derive(Debug, Clone, Copy, PartialEq, Eq)] #[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Line(pub Point, pub Point); pub struct Line(pub Point, pub Point);
@ -156,7 +148,7 @@ pub struct Line(pub Point, pub Point);
/// extend past its edges. /// extend past its edges.
pub fn draw_line<const X: usize, const Y: usize>( pub fn draw_line<const X: usize, const Y: usize>(
line: &Line, line: &Line,
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>, matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
) { ) {
let mut line = *line; let mut line = *line;
#[cfg(test)] #[cfg(test)]
@ -209,7 +201,7 @@ pub fn draw_line<const X: usize, const Y: usize>(
dbg!(draw_point); dbg!(draw_point);
if matrix.is_in_bounds(&draw_point) { if matrix.is_in_bounds(&draw_point) {
matrix[draw_point] = true; matrix[draw_point] = 1;
prev_out_of_bounds = false; prev_out_of_bounds = false;
} else { } else {
if !prev_out_of_bounds { if !prev_out_of_bounds {
@ -257,195 +249,195 @@ mod tests {
#[test] #[test]
fn four_quadrant_matrix() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
canvas[Point { x: 0, y: 0 }] = true; canvas[Point { x: 0, y: 0 }] = 1;
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, false, false, false], [0, 0, 0, 0, 0],
[false, false, false, false, false], [0, 0, 0, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, false, false, false], [0, 0, 0, 0, 0],
[false, false, false, false, false], [0, 0, 0, 0, 0],
] ]
); );
canvas[Point { x: -2, y: 1 }] = true; canvas[Point { x: -2, y: 1 }] = 1;
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, false, false, false], [0, 0, 0, 0, 0],
[true, false, false, false, false], [1, 0, 0, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, false, false, false], [0, 0, 0, 0, 0],
[false, false, false, false, false] [0, 0, 0, 0, 0]
] ]
); );
} }
#[test] #[test]
fn diagonal_unsigned_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 0, y: 4 });
draw_line( draw_line(
&Line(Point { x: 0, y: 0 }, Point { x: 4, y: 4 }), &Line(Point { x: 0, y: 0 }, Point { x: 4, y: 4 }),
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, false, false, true], [0, 0, 0, 0, 1],
[false, false, false, true, false], [0, 0, 0, 1, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, true, false, false, false], [0, 1, 0, 0, 0],
[true, false, false, false, false], [1, 0, 0, 0, 0],
] ]
) )
} }
#[test] #[test]
fn diagonal_signed_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line( draw_line(
&Line(Point { x: -2, y: -2 }, Point { x: 2, y: 2 }), &Line(Point { x: -2, y: -2 }, Point { x: 2, y: 2 }),
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, false, false, true], [0, 0, 0, 0, 1],
[false, false, false, true, false], [0, 0, 0, 1, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, true, false, false, false], [0, 1, 0, 0, 0],
[true, false, false, false, false], [1, 0, 0, 0, 0],
] ]
) )
} }
#[test] #[test]
fn diagonal_signed_both_oob_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line( draw_line(
&Line(Point { x: -10, y: -10 }, Point { x: 10, y: 10 }), &Line(Point { x: -10, y: -10 }, Point { x: 10, y: 10 }),
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, false, false, true], [0, 0, 0, 0, 1],
[false, false, false, true, false], [0, 0, 0, 1, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, true, false, false, false], [0, 1, 0, 0, 0],
[true, false, false, false, false], [1, 0, 0, 0, 0],
] ]
); );
} }
#[test] #[test]
fn diagonal_signed_first_oob_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line( draw_line(
&Line(Point { x: -10, y: -10 }, Point { x: 2, y: 2 }), &Line(Point { x: -10, y: -10 }, Point { x: 2, y: 2 }),
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, false, false, true], [0, 0, 0, 0, 1],
[false, false, false, true, false], [0, 0, 0, 1, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, true, false, false, false], [0, 1, 0, 0, 0],
[true, false, false, false, false], [1, 0, 0, 0, 0],
] ]
); );
} }
#[test] #[test]
fn diagonal_signed_second_oob_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line( draw_line(
&Line(Point { x: -2, y: -2 }, Point { x: 10, y: 10 }), &Line(Point { x: -2, y: -2 }, Point { x: 10, y: 10 }),
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, false, false, true], [0, 0, 0, 0, 1],
[false, false, false, true, false], [0, 0, 0, 1, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, true, false, false, false], [0, 1, 0, 0, 0],
[true, false, false, false, false], [1, 0, 0, 0, 0],
] ]
); );
} }
#[test] #[test]
fn vertical_signed_both_oob_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line( draw_line(
&Line(Point { x: 0, y: -10 }, Point { x: 0, y: 10 }), &Line(Point { x: 0, y: -10 }, Point { x: 0, y: 10 }),
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
] ]
); );
} }
#[test] #[test]
fn vertical_signed_first_oob_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line( draw_line(
&Line(Point { x: 0, y: -10 }, Point { x: 0, y: 0 }), &Line(Point { x: 0, y: -10 }, Point { x: 0, y: 0 }),
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, false, false, false], [0, 0, 0, 0, 0],
[false, false, false, false, false], [0, 0, 0, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
] ]
); );
} }
#[test] #[test]
fn vertical_signed_second_oob_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line( draw_line(
&Line(Point { x: 0, y: 0 }, Point { x: 0, y: 10 }), &Line(Point { x: 0, y: 0 }, Point { x: 0, y: 10 }),
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, false, false, false], [0, 0, 0, 0, 0],
[false, false, false, false, false], [0, 0, 0, 0, 0],
] ]
); );
} }
#[test] #[test]
fn cross_signed_line() { 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 }); FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line( draw_line(
&Line(Point { x: 0, y: -2 }, Point { x: 0, y: 2 }), &Line(Point { x: 0, y: -2 }, Point { x: 0, y: 2 }),
@ -456,13 +448,13 @@ mod tests {
&mut canvas, &mut canvas,
); );
assert_eq!( assert_eq!(
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas), <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
[ [
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[true, true, true, true, true], [1, 1, 1, 1, 1],
[false, false, true, false, false], [0, 0, 1, 0, 0],
[false, false, true, false, false], [0, 0, 1, 0, 0],
] ]
) )
} }