Completely ported compass to embassy.

.cargo/config.toml

@@ -0,0 +1,4 @@
+[target.'cfg(all(target_arch = "arm", target_os = "none"))']
+
+[build]
+target = "thumbv7em-none-eabihf"

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "rust-analyzer.check.allTargets": false
+}

Cargo.toml

@@ -3,8 +3,15 @@ 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
 panic = "abort"
+
+[patch.crates-io]
+embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
+embassy-futures = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
+embassy-nrf = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
+embassy-sync = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
+embassy-time = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
+embassy-time-driver = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }

hardware_main/.cargo/config.toml

@@ -1,7 +1,11 @@
 [target.'cfg(all(target_arch = "arm", target_os = "none"))']
 rustflags = [
   "-C", "link-arg=-Tlink.x",
+  "-C", "link-arg=-Tdefmt.x",
 ]
 
 [build]
 target = "thumbv7em-none-eabihf"
+
+[env]
+DEFMT_LOG = "debug"

hardware_main/Cargo.toml

@@ -1,30 +1,23 @@
 [package]
 name = "led-compass"
 version = "0.1.0"
-authors = ["Henrik Böving <hargonix@gmail.com>"]
+authors = ["Gabriel Venberg"]
-edition = "2018"
+edition = "2024"
-
-[dependencies.microbit-v2]
-version = "0.12.0"
-optional = true
-
-[dependencies.microbit]
-version = "0.12.0"
-optional = true
 
 [dependencies]
-cortex-m = "0.7.3"
-cortex-m-rt = "0.7.0"
-rtt-target = { version = "0.3.1", features = ["cortex-m"] }
-panic-rtt-target = { version = "0.1.2", features = ["cortex-m"] }
-panic-halt = "0.2.0"
-lsm303agr = "0.2.2"
-libm = "0.2.1"
-embedded-hal = "0.2.6"
 independent_logic = {path="../independent_logic"}
 
-[features]
+cortex-m = { version = "0.7", features = ["inline-asm", "critical-section-single-core"] }
-v2 = ["microbit-v2"]
+cortex-m-rt = "0.7"
-v1 = ["microbit"]
+defmt = "1.0"
-calibration=[]
+defmt-rtt = "1.0"
-default = ["v2"]
+panic-probe = { version = "1.0", features = ["print-defmt"] }
+
+embassy-executor = { version = "0.7", features = ["arch-cortex-m", "executor-thread", "defmt"] }
+embassy-time = { version = "0.4", features = ["defmt", "defmt-timestamp-uptime"] }
+embassy-futures = { version = "0.1", features = ["defmt"] }
+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/Embed.toml

@@ -1,9 +1,5 @@
 [default.general]
 chip = "nrf52833_xxAA" # uncomment this line for micro:bit V2
-# chip = "nrf51822_xxAA" # uncomment this line for micro:bit V1
-
-[default.reset]
-halt_afterwards = false
 
 [default.rtt]
 enabled = true

hardware_main/memory.x

@@ -1,6 +1,6 @@
 MEMORY
 {
   /* NOTE K = KiBi = 1024 bytes */
-  FLASH : ORIGIN = 0x00000000, LENGTH = 256K
+  FLASH : ORIGIN = 0x00000000, LENGTH = 512K
   RAM : ORIGIN = 0x20000000, LENGTH = 16K
 }

hardware_main/src/calibration.rs

@@ -1,272 +0,0 @@
-#![allow(unused)]
-//! Translated from <https://github.com/lancaster-university/codal-microbit-v2/blob/006abf5566774fbcf674c0c7df27e8a9d20013de/source/MicroBitCompassCalibrator.cpp>
-
-use core::fmt::Debug;
-use embedded_hal::blocking::delay::DelayUs;
-use embedded_hal::blocking::i2c::{Write, WriteRead};
-use libm::{fabsf, sqrtf};
-use lsm303agr::interface::I2cInterface;
-use lsm303agr::mode::MagContinuous;
-use lsm303agr::Lsm303agr;
-use lsm303agr::Measurement;
-use microbit::display::blocking::Display;
-
-const PERIMETER_POINTS: usize = 25;
-const PIXEL1_THRESHOLD: i32 = 200;
-const PIXEL2_THRESHOLD: i32 = 600;
-const CALIBRATION_INCREMENT: i32 = 200;
-
-#[derive(Debug)]
-pub struct Calibration {
-    center: Measurement,
-    scale: Measurement,
-    radius: u32,
-}
-
-impl Default for Calibration {
-    fn default() -> Calibration {
-        Calibration {
-            // center: Measurement { x: 0, y: 0, z: 0 },
-            // scale: Measurement {
-            //     x: 1024,
-            //     y: 1024,
-            //     z: 1024,
-            // },
-            // radius: 0,
-            center: Measurement {
-                x: 2434,
-                y: 5528,
-                z: -40156,
-            },
-            scale: Measurement {
-                x: 1044,
-                y: 1042,
-                z: 1049,
-            },
-            radius: 61751,
-        }
-    }
-}
-
-pub fn calc_calibration<I, T, E>(
-    sensor: &mut Lsm303agr<I2cInterface<I>, MagContinuous>,
-    display: &mut Display,
-    timer: &mut T,
-) -> Calibration
-where
-    T: DelayUs<u32>,
-    I: Write<Error = E> + WriteRead<Error = E>,
-    E: Debug,
-{
-    let data = get_data(sensor, display, timer);
-    calibrate(&data)
-}
-
-fn get_data<I, T, E>(
-    sensor: &mut Lsm303agr<I2cInterface<I>, MagContinuous>,
-    display: &mut Display,
-    timer: &mut T,
-) -> [Measurement; 25]
-where
-    T: DelayUs<u32>,
-    I: Write<Error = E> + WriteRead<Error = E>,
-    E: Debug,
-{
-    let mut leds = [
-        [0, 0, 0, 0, 0],
-        [0, 0, 0, 0, 0],
-        [0, 0, 0, 0, 0],
-        [0, 0, 0, 0, 0],
-        [0, 0, 0, 0, 0],
-    ];
-    let mut cursor = (2, 2);
-    let mut data = [Measurement { x: 0, y: 0, z: 0 }; PERIMETER_POINTS];
-    let mut samples = 0;
-
-    while samples < PERIMETER_POINTS {
-        while !sensor.accel_status().unwrap().xyz_new_data {}
-        let accel_data = sensor.accel_data().unwrap();
-        let x = accel_data.x;
-        let y = accel_data.y;
-        if x < -PIXEL2_THRESHOLD {
-            cursor.1 = 0;
-        } else if x < -PIXEL1_THRESHOLD {
-            cursor.1 = 1;
-        } else if x > PIXEL2_THRESHOLD {
-            cursor.1 = 4;
-        } else if x > PIXEL1_THRESHOLD {
-            cursor.1 = 3;
-        } else {
-            cursor.1 = 2;
-        }
-
-        if y < -PIXEL2_THRESHOLD {
-            cursor.0 = 0;
-        } else if y < -PIXEL1_THRESHOLD {
-            cursor.0 = 1;
-        } else if y > PIXEL2_THRESHOLD {
-            cursor.0 = 4;
-        } else if y > PIXEL1_THRESHOLD {
-            cursor.0 = 3;
-        } else {
-            cursor.0 = 2;
-        }
-
-        // Turn the y axis properly
-        cursor.0 = 4 - cursor.0;
-
-        if leds[cursor.0][cursor.1] != 1 {
-            leds[cursor.0][cursor.1] = 1;
-            while !sensor.mag_status().unwrap().xyz_new_data {}
-            let mag_data = measurement_to_enu(sensor.mag_data().unwrap());
-            data[samples] = mag_data;
-            samples += 1;
-        }
-        display.show(timer, leds, 200);
-    }
-    data
-}
-
-fn difference_square(a: Measurement, b: Measurement) -> f32 {
-    let dx = (a.x - b.x) as f32;
-    let dy = (a.y - b.y) as f32;
-    let dz = (a.z - b.z) as f32;
-
-    (dx * dx) + (dy * dy) + (dz * dz)
-}
-
-fn measure_score(center: Measurement, data: &[Measurement]) -> f32 {
-    let mut min_d = difference_square(center, data[0]);
-    let mut max_d = min_d;
-
-    for point in data[1..].iter() {
-        let d = difference_square(center, *point);
-        if d < min_d {
-            min_d = d;
-        }
-
-        if d > max_d {
-            max_d = d;
-        }
-    }
-
-    max_d - min_d
-}
-
-fn calibrate(data: &[Measurement]) -> Calibration {
-    // Approximate a center for the data
-    let mut center = Measurement { x: 0, y: 0, z: 0 };
-    let mut best = center;
-
-    for point in data {
-        center.x += point.x;
-        center.y += point.y;
-        center.z += point.z;
-    }
-
-    center.x /= data.len() as i32;
-    center.y /= data.len() as i32;
-    center.z /= data.len() as i32;
-
-    let mut current = center;
-    let mut score = measure_score(current, data);
-
-    // Calculate a fixpoint position
-    loop {
-        for x in [-CALIBRATION_INCREMENT, 0, CALIBRATION_INCREMENT] {
-            for y in [-CALIBRATION_INCREMENT, 0, CALIBRATION_INCREMENT] {
-                for z in [-CALIBRATION_INCREMENT, 0, CALIBRATION_INCREMENT] {
-                    let mut attempt = current;
-                    attempt.x += x;
-                    attempt.y += y;
-                    attempt.z += z;
-
-                    let attempt_score = measure_score(attempt, data);
-                    if attempt_score < score {
-                        score = attempt_score;
-                        best = attempt;
-                    }
-                }
-            }
-        }
-
-        if best == current {
-            break;
-        }
-
-        current = best;
-    }
-
-    spherify(current, data)
-}
-
-fn spherify(center: Measurement, data: &[Measurement]) -> Calibration {
-    let mut radius = 0;
-    for point in data {
-        let d = sqrtf(difference_square(center, *point)) as u32;
-        if d > radius {
-            radius = d;
-        }
-    }
-
-    let mut scale: f32 = 0.0;
-    let mut weight_x = 0.0;
-    let mut weight_y = 0.0;
-    let mut weight_z = 0.0;
-
-    for point in data {
-        let d = sqrtf(difference_square(center, *point));
-        let s = (radius as f32 / d) - 1.0;
-        scale = scale.max(s);
-
-        let dx = point.x - center.x;
-        let dy = point.y - center.y;
-        let dz = point.z - center.z;
-
-        weight_x += s * fabsf(dx as f32 / d);
-        weight_y += s * fabsf(dy as f32 / d);
-        weight_z += s * fabsf(dz as f32 / d);
-    }
-
-    let wmag = sqrtf((weight_x * weight_x) + (weight_y * weight_y) + (weight_z * weight_z));
-    let scale_x = 1.0 + scale * (weight_x / wmag);
-    let scale_y = 1.0 + scale * (weight_y / wmag);
-    let scale_z = 1.0 + scale * (weight_z / wmag);
-
-    Calibration {
-        center,
-        radius,
-        scale: Measurement {
-            x: (1024.0 * scale_x) as i32,
-            y: (1024.0 * scale_y) as i32,
-            z: (1024.0 * scale_z) as i32,
-        },
-    }
-}
-
-pub fn calibrated_measurement(measurement: Measurement, calibration: &Calibration) -> Measurement {
-    let mut out = measurement_to_enu(measurement);
-    out = Measurement {
-        x: ((out.x - calibration.center.x) * calibration.scale.x) >> 10,
-        y: ((out.y - calibration.center.y) * calibration.scale.y) >> 10,
-        z: ((out.z - calibration.center.z) * calibration.scale.z) >> 10,
-    };
-    //to convert it back to the board-native SWU cordinates
-    measurement_to_enu(out)
-}
-
-fn measurement_to_enu(measurement: Measurement) -> Measurement {
-    Measurement {
-        x: -measurement.y,
-        y: -measurement.x,
-        z: measurement.z,
-    }
-}
-
-fn enu_to_cartesian(measurement: Measurement) -> Measurement {
-    Measurement {
-        x: -measurement.y,
-        y: measurement.x,
-        z: measurement.z,
-    }
-}

hardware_main/src/main.rs

@@ -2,169 +2,132 @@
 #![no_main]
 #![no_std]
 
-#[cfg(debug_assertions)]
 use core::f32::consts::PI;
-
+use defmt::{debug, info};
-use calibration::Calibration;
+use embassy_executor::Spawner;
-use cortex_m_rt::entry;
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
-use independent_logic::line_drawing::{FourQuadrantMatrix, UPoint};
+use embassy_time::{Duration, Ticker};
-use lsm303agr::interface::I2cInterface;
+use microbit_bsp::{
-use lsm303agr::mode::MagContinuous;
+    LedMatrix, Microbit,
-use lsm303agr::{AccelOutputDataRate, Lsm303agr, MagOutputDataRate, Measurement};
+    display::{Bitmap, Brightness, Frame},
-use microbit::hal::{gpiote::Gpiote, Twim};
+    embassy_nrf::{
-use microbit::pac::TWIM0;
+        bind_interrupts,
-#[cfg(not(debug_assertions))]
+        peripherals::TWISPI0,
-use panic_halt as _;
+        twim::{InterruptHandler, Twim},
-
+    },
-#[cfg(debug_assertions)]
+    lsm303agr::{self, Lsm303agr, interface::I2cInterface, mode::MagContinuous},
-use panic_rtt_target as _;
+    motion::new_lsm303agr,
-#[cfg(debug_assertions)]
+};
-use rtt_target::{rprintln, rtt_init_print};
+use {defmt_rtt as _, panic_probe as _};
-
-mod calibration;
-
-use microbit::{display::blocking::Display, hal::Timer};
-
-#[cfg(feature = "v1")]
-use microbit::{hal::twi, pac::twi0::frequency::FREQUENCY_A};
-
-#[cfg(feature = "v2")]
-use microbit::{hal::twim, pac::twim0::frequency::FREQUENCY_A};
-
-use crate::calibration::calc_calibration;
 
 use independent_logic::{
     heading_drawing::draw_constant_heading,
+    line_drawing::{FourQuadrantMatrix, UPoint},
     tilt_compensation::{
-        calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, Heading,
+        Heading, NedMeasurement, calc_attitude, calc_tilt_calibrated_measurement,
-        NedMeasurement,
+        heading_from_measurement,
     },
 };
 
-const DELAY: u32 = 100;
+static HEADING: Signal<CriticalSectionRawMutex, Heading> = Signal::new();
 
-#[entry]
+#[embassy_executor::main]
-fn main() -> ! {
+async fn main(s: Spawner) {
-    #[cfg(debug_assertions)]
+    let board = Microbit::default();
-    rtt_init_print!();
+    defmt::info!("Application started!");
-    let board = microbit::Board::take().unwrap();
 
-    #[cfg(feature = "v1")]
+    let mut display = board.display;
-    let i2c = { twi::Twi::new(board.TWI0, board.i2c.into(), FREQUENCY_A::K100) };
+    display.set_brightness(Brightness::MAX);
 
-    #[cfg(feature = "v2")]
+    // Bind interrupt to the TWI/SPI peripheral.
-    let i2c = { twim::Twim::new(board.TWIM0, board.i2c_internal.into(), FREQUENCY_A::K100) };
+    bind_interrupts!(
+        struct InterruptRequests {
+            TWISPI0 => InterruptHandler<TWISPI0>;
+        }
+    );
 
-    let mut timer = Timer::new(board.TIMER0);
+    let irqs = InterruptRequests {};
-    let mut display = Display::new(board.display_pins);
+    let mut sensor = new_lsm303agr(board.twispi0, irqs, board.i2c_int_sda, board.i2c_int_scl);
+    sensor.init().await.unwrap();
+    sensor.enable_mag_offset_cancellation().await.unwrap();
+    sensor
+        .set_mag_mode_and_odr(
+            &mut embassy_time::Delay,
+            lsm303agr::MagMode::HighResolution,
+            lsm303agr::MagOutputDataRate::Hz50,
+        )
+        .await
+        .unwrap();
+    let Ok(mut sensor) = sensor.into_mag_continuous().await else {
+        panic!("Failed to set sensor to continuous mode");
+    };
+    sensor
+        .set_accel_mode_and_odr(
+            &mut embassy_time::Delay,
+            lsm303agr::AccelMode::Normal,
+            lsm303agr::AccelOutputDataRate::Hz50,
+        )
+        .await
+        .unwrap();
+    s.must_spawn(get_data(sensor));
+    s.must_spawn(display_data(display));
+}
 
-    let gpiote = Gpiote::new(board.GPIOTE);
+#[embassy_executor::task]
-    let channel_button_a = gpiote.channel0();
+async fn display_data(mut display: LedMatrix) {
-    channel_button_a
+    let mut display_matrix: FourQuadrantMatrix<5, 5, bool> =
-        .input_pin(&board.buttons.button_a.degrade())
-        .hi_to_lo();
-    channel_button_a.reset_events();
-
-    let channel_button_b = gpiote.channel1();
-    channel_button_b
-        .input_pin(&board.buttons.button_b.degrade())
-        .hi_to_lo();
-    channel_button_b.reset_events();
-
-    let mut sensor = Lsm303agr::new_with_i2c(i2c);
-    sensor.init().unwrap();
-    sensor.set_mag_odr(MagOutputDataRate::Hz10).unwrap();
-    sensor.set_accel_odr(AccelOutputDataRate::Hz10).unwrap();
-    let mut sensor = sensor.into_mag_continuous().ok().unwrap();
-
-    #[cfg(feature = "calibration")]
-    let mut calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
-    #[cfg(not(feature = "calibration"))]
-    let mut calibration = calibration::Calibration::default();
-
-    let mut current_display: FourQuadrantMatrix<5, 5, u8> =
         FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
-    #[cfg(debug_assertions)]
-    rprintln!("Calibration: {:?}", calibration);
-
-    let mut tilt_correction_enabled: bool = true;
-
-    // let mut heading = Heading(0.0);
     loop {
-        if channel_button_b.is_event_triggered() {
+        let heading = HEADING.wait().await;
-            calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
+        info!("Heading: {}", heading.0 * (180.0 / PI));
-            channel_button_b.reset_events();
+        draw_constant_heading(heading, &mut display_matrix);
-            #[cfg(debug_assertions)]
+        display
-            rprintln!("Calibration: {:?}", calibration);
+            .display(to_frame(&display_matrix), Duration::from_hz(25))
-        }
+            .await;
-        // if channel_button_a.is_event_triggered() {
-        //     //toggles the bool.
-        //     tilt_correction_enabled ^= true;
-        //     channel_button_a.reset_events()
-        // }
-
-        current_display.reset_matrix();
-
-        let heading = calc_heading(&mut sensor, &calibration, &tilt_correction_enabled);
-        draw_constant_heading::<5, 5>(heading, &mut current_display);
-        display.show(&mut timer, current_display.into(), DELAY)
     }
 }
 
-/// board has forward in the -y direction and right in the +x direction, and down in the -z. (ENU),  algs for tilt compensation
+#[embassy_executor::task]
-/// need forward in +x and right in +y (this is known as the NED (north, east, down) cordinate
+async fn get_data(mut sensor: Lsm303agr<I2cInterface<Twim<'static, TWISPI0>>, MagContinuous>) {
-/// system)
+    let mut ticker = Ticker::every(Duration::from_hz(25));
-/// also converts to f32
+    loop {
-pub fn enu_to_ned(measurement: Measurement) -> NedMeasurement {
+        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: -measurement.y as f32,
+        x: -y as f32,
-        y: measurement.x as f32,
+        y: x as f32,
-        z: -measurement.z as f32,
+        z: -z as f32,
     }
 }
-
-fn calc_heading(
-    sensor: &mut Lsm303agr<I2cInterface<Twim<TWIM0>>, MagContinuous>,
-    mag_calibration: &Calibration,
-    tilt_correction_enabled: &bool,
-) -> Heading {
-    while !(sensor.mag_status().unwrap().xyz_new_data
-        && sensor.accel_status().unwrap().xyz_new_data)
-    {}
-    let mag_data = sensor.mag_data().unwrap();
-    let mag_data = calibration::calibrated_measurement(mag_data, mag_calibration);
-    let acel_data = sensor.accel_data().unwrap();
-
-    let mut ned_mag_data = enu_to_ned(mag_data);
-    let ned_acel_data = enu_to_ned(acel_data);
-
-    let attitude = calc_attitude(&ned_acel_data);
-
-    if *tilt_correction_enabled {
-        ned_mag_data = calc_tilt_calibrated_measurement(ned_mag_data, &attitude);
-    }
-    //theta=0 at north, pi/-pi at south, pi/2 at east, and -pi/2 at west
-    let heading = heading_from_measurement(&ned_mag_data);
-
-    #[cfg(all(not(feature = "calibration"), debug_assertions))]
-    rprintln!(
-        "pitch: {:<+5.0}, roll: {:<+5.0}, heading: {:<+5.0}",
-        attitude.pitch * (180.0 / PI),
-        attitude.roll * (180.0 / PI),
-        heading.0 * (180.0 / PI),
-    );
-    rprintln!(
-        "mag: x: {:<+16}, y: {:<+16}, z: {:<+16}",
-        ned_mag_data.x,
-        ned_mag_data.y,
-        ned_mag_data.z
-    );
-    #[cfg(all(not(feature = "calibration"), debug_assertions))]
-    rprintln!(
-        "acell: x: {:<+16}, y: {:<+16}, z: {:<+16}",
-        ned_acel_data.x,
-        ned_acel_data.y,
-        ned_acel_data.z
-    );
-    heading
-}

independent_logic/.cargo/config.toml

@@ -0,0 +1,2 @@
+[build]
+target = "x86_64-unknown-linux-gnu"

independent_logic/Cargo.toml

@@ -1,9 +1,10 @@
 [package]
 name = "independent_logic"
 version = "0.1.0"
-edition = "2021"
+edition = "2024"
 
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
+defmt = "1.0.1"
 libm = "0.2.1"

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],
+                [false, false, false, false, false],
-                [0, 0, 0, 0, 0],
+                [false, false, false, false, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [0, 0, 0, 0, 0],
+                [false, false, false, false, false],
-                [0, 0, 0, 0, 0],
+                [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],
+                [false, false, false, false, false],
-                [1, 0, 0, 0, 0],
+                [true, false, false, false, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [0, 0, 0, 0, 0],
+                [false, false, false, false, false],
-                [0, 0, 0, 0, 0]
+                [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],
+                [false, false, false, false, true],
-                [0, 0, 0, 1, 0],
+                [false, false, false, true, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [0, 1, 0, 0, 0],
+                [false, true, false, false, false],
-                [1, 0, 0, 0, 0],
+                [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],
+                [false, false, false, false, true],
-                [0, 0, 0, 1, 0],
+                [false, false, false, true, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [0, 1, 0, 0, 0],
+                [false, true, false, false, false],
-                [1, 0, 0, 0, 0],
+                [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],
+                [false, false, false, false, true],
-                [0, 0, 0, 1, 0],
+                [false, false, false, true, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [0, 1, 0, 0, 0],
+                [false, true, false, false, false],
-                [1, 0, 0, 0, 0],
+                [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],
+                [false, false, false, false, true],
-                [0, 0, 0, 1, 0],
+                [false, false, false, true, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [0, 1, 0, 0, 0],
+                [false, true, false, false, false],
-                [1, 0, 0, 0, 0],
+                [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],
+                [false, false, false, false, true],
-                [0, 0, 0, 1, 0],
+                [false, false, false, true, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [0, 1, 0, 0, 0],
+                [false, true, false, false, false],
-                [1, 0, 0, 0, 0],
+                [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],
+                [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],
             ]
         );
     }
 
     #[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],
+                [false, false, false, false, false],
-                [0, 0, 0, 0, 0],
+                [false, false, false, 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],
             ]
         );
     }
 
     #[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],
+                [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, 0, 0, 0],
+                [false, false, false, false, false],
-                [0, 0, 0, 0, 0],
+                [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],
+                [false, false, true, false, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [1, 1, 1, 1, 1],
+                [true, true, true, true, true],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
-                [0, 0, 1, 0, 0],
+                [false, false, true, false, false],
             ]
         )
     }

independent_logic/src/tilt_compensation.rs

@@ -1,12 +1,13 @@
+use defmt::Format;
 use libm::{atan2f, atanf, cosf, sinf};
 
-#[derive(Debug)]
+#[derive(Debug, Format)]
 pub struct Attitude {
     pub pitch: f32,
     pub roll: f32,
 }
 
-#[derive(Debug)]
+#[derive(Debug, Format)]
 pub struct NedMeasurement {
     pub x: f32,
     pub y: f32,
@@ -14,6 +15,7 @@ pub struct NedMeasurement {
 }
 
 ///theta=0 at north, pi/-pi at south, pi/2 at east, and -pi/2 at west
+#[derive(Debug, Format)]
 pub struct Heading(pub f32);
 
 pub fn calc_attitude(measurement: &NedMeasurement) -> Attitude {