diff --git a/.cargo/config.toml b/.cargo/config.toml
index ff8b44d..6dc9cfd 100644
--- a/.cargo/config.toml
+++ b/.cargo/config.toml
@@ -1,11 +1,4 @@
 [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"
diff --git a/Cargo.toml b/Cargo.toml
index 780fe94..1a272a7 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -8,3 +8,11 @@ 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" }
diff --git a/hardware_main/Cargo.toml b/hardware_main/Cargo.toml
index 5141e80..f4a8f23 100644
--- a/hardware_main/Cargo.toml
+++ b/hardware_main/Cargo.toml
@@ -5,16 +5,18 @@ authors = ["Gabriel Venberg"]
 edition = "2024"
 
 [dependencies]
-cortex-m = { version = "0.7.7", features = ["inline-asm", "critical-section-single-core"] }
-cortex-m-rt = "0.7.5"
-defmt = "1.0.1"
-defmt-rtt = "1.0.0"
-embassy-executor = { version = "0.7.0", features = ["arch-cortex-m", "executor-thread", "defmt"] }
-embassy-nrf = { version = "0.3.1", features = ["defmt", "nrf52833", "gpiote", "time-driver-rtc1", "time"] }
-embassy-time = { version = "0.4.0", features = ["defmt", "defmt-timestamp-uptime"] }
-lsm303agr = { version = "1.1.0", features = ["async"] }
-panic-probe = { version = "1.0.0", features = ["print-defmt"] }
 independent_logic = {path="../independent_logic"}
-embassy-futures = { version = "0.1.1", features = ["defmt"] }
-embassy-sync = { version = "0.7.0", features = ["defmt"] }
 
+cortex-m = { version = "0.7", features = ["inline-asm", "critical-section-single-core"] }
+cortex-m-rt = "0.7"
+defmt = "1.0"
+defmt-rtt = "1.0"
+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"
diff --git a/hardware_main/memory.x b/hardware_main/memory.x
index 9e2ab65..b488bbf 100644
--- a/hardware_main/memory.x
+++ b/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
 }
diff --git a/hardware_main/src/main.rs b/hardware_main/src/main.rs
index 8da839d..14e6ada 100644
--- a/hardware_main/src/main.rs
+++ b/hardware_main/src/main.rs
@@ -2,68 +2,87 @@
 #![no_main]
 #![no_std]
 
-use core::cmp::max;
-use defmt::info;
-use defmt_rtt as _;
+use core::f32::consts::PI;
+use defmt::{debug, info};
 use embassy_executor::Spawner;
-use embassy_futures::join::join;
-use embassy_nrf::{
-    bind_interrupts,
-    gpio::{AnyPin, Input, Pin, Pull},
-    temp::Temp,
+use embassy_time::Timer;
+use microbit_bsp::{
+    Microbit,
+    display::{Brightness, Frame},
+    embassy_nrf::{bind_interrupts, peripherals::TWISPI0, twim::InterruptHandler},
+    lsm303agr,
+    motion::new_lsm303agr,
 };
-use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
-use embassy_time::{Duration, Timer, WithTimeout};
-use panic_probe as _;
+use {defmt_rtt as _, panic_probe as _};
 
-#[derive(Debug, Clone, Copy)]
-enum Button {
-    A,
-    B,
-}
-
-static SIGNAL: Signal<CriticalSectionRawMutex, Button> = Signal::new();
-
-bind_interrupts!(struct Irqs {
-    TEMP => embassy_nrf::temp::InterruptHandler;
-});
+use independent_logic::{
+    heading_drawing::draw_constant_heading,
+    line_drawing::{FourQuadrantMatrix, UPoint},
+    tilt_compensation::{
+        Heading, NedMeasurement, calc_attitude, calc_tilt_calibrated_measurement,
+        heading_from_measurement,
+    },
+};
 
 #[embassy_executor::main]
-async fn main(spawner: Spawner) {
-    info!("Starting");
-    let p = embassy_nrf::init(Default::default());
-    let temp = Temp::new(p.TEMP, Irqs);
-    spawner.spawn(temp_task(temp)).unwrap();
-    let button_a = button(p.P0_14.degrade(), "A", Button::A);
-    let button_b = button(p.P0_23.degrade(), "B", Button::B);
-    join(button_a, button_b).await;
-}
+async fn main(_s: Spawner) {
+    let board = Microbit::default();
+    defmt::info!("Application started!");
 
-async fn button(pin: AnyPin, id: &'static str, b: Button) {
-    let mut button = Input::new(pin, Pull::None);
-    loop {
-        button.wait_for_low().await;
-        info!("Button {} Pressed!", id);
-        SIGNAL.signal(b);
-        Timer::after_millis(200).await;
-        button.wait_for_high().await;
-    }
-}
+    let mut display = board.display;
+    display.set_brightness(Brightness::MAX);
 
-#[embassy_executor::task]
-async fn temp_task(mut temp: Temp<'static>) {
-    const INTERVAL_MS: u64 = 500;
-    let mut delay_ms = INTERVAL_MS;
-    loop {
-        let value: u16 = temp.read().await.to_num();
-        info!("{} C", value);
-        let delay = Duration::from_millis(delay_ms);
-        if let Ok(v) = SIGNAL.wait().with_timeout(delay).await {
-            delay_ms = match v {
-                Button::A => max(INTERVAL_MS, delay_ms.saturating_sub(INTERVAL_MS)),
-                Button::B => delay_ms + INTERVAL_MS,
-            };
-            info!("Delay = {} ms", delay_ms);
+    // Bind interrupt to the TWI/SPI peripheral.
+    bind_interrupts!(
+        struct InterruptRequests {
+            TWISPI0 => InterruptHandler<TWISPI0>;
         }
+    );
+
+    let irqs = InterruptRequests {};
+    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();
+
+    Timer::after_secs(2).await;
+
+    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));
+    }
+}
+
+pub fn to_ned(x: i32, y: i32, z: i32) -> NedMeasurement {
+    NedMeasurement {
+        x: -y as f32,
+        y: x as f32,
+        z: -z as f32,
     }
 }
diff --git a/independent_logic/.cargo/config.toml b/independent_logic/.cargo/config.toml
new file mode 100644
index 0000000..2f05654
--- /dev/null
+++ b/independent_logic/.cargo/config.toml
@@ -0,0 +1,2 @@
+[build]
+target = "x86_64-unknown-linux-gnu"
diff --git a/independent_logic/Cargo.toml b/independent_logic/Cargo.toml
index 2ed93c5..27183e3 100644
--- a/independent_logic/Cargo.toml
+++ b/independent_logic/Cargo.toml
@@ -6,4 +6,5 @@ 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"
diff --git a/independent_logic/src/tilt_compensation.rs b/independent_logic/src/tilt_compensation.rs
index f0dc8d3..d59648b 100644
--- a/independent_logic/src/tilt_compensation.rs
+++ b/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 {