Completely ported compass to embassy.

This commit is contained in:
Gabe Venberg 2025-07-11 14:32:56 +02:00
parent 3471b71dbc
commit 24c8773820
5 changed files with 78 additions and 24 deletions

View file

@ -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

View file

@ -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"

View file

@ -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,

View file

@ -19,5 +19,6 @@ pub fn draw_constant_heading<const X: usize, const Y: usize>(
heading: Heading,
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);
}

View file

@ -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);