Added unit tests!!

hardware_main/.cargo/config

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

hardware_main/Cargo.toml

@@ -0,0 +1,29 @@
+[package]
+name = "led-compass"
+version = "0.1.0"
+authors = ["Henrik Böving <hargonix@gmail.com>"]
+edition = "2018"
+
+[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"] }
+lsm303agr = "0.2.2"
+libm = "0.2.1"
+embedded-hal = "0.2.6"
+independent_logic = {path="../independent_logic"}
+
+[features]
+v2 = ["microbit-v2"]
+v1 = ["microbit"]
+calibration=[]
+default = ["v2"]

hardware_main/Embed.toml

@@ -0,0 +1,12 @@
+[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
+
+[default.gdb]
+enabled = false

hardware_main/build.rs

@@ -0,0 +1,30 @@
+//! This build script copies the `memory.x` file from the crate root into
+//! a directory where the linker can always find it at build time.
+//! For many projects this is optional, as the linker always searches the
+//! project root directory (wherever `Cargo.toml` is). However, if you
+//! are using a workspace or have a more complicated build setup, this
+//! build script becomes required. Additionally, by requesting that
+//! Cargo re-run the build script whenever `memory.x` is changed,
+//! a rebuild of the application with new memory settings is ensured after updating `memory.x`.
+
+use std::env;
+use std::fs::File;
+use std::io::Write;
+use std::path::PathBuf;
+
+fn main() {
+    // Put `memory.x` in our output directory and ensure it's
+    // on the linker search path.
+    let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap());
+    File::create(out.join("memory.x"))
+        .unwrap()
+        .write_all(include_bytes!("memory.x"))
+        .unwrap();
+    println!("cargo:rustc-link-search={}", out.display());
+
+    // By default, Cargo will re-run a build script whenever
+    // any file in the project changes. By specifying `memory.x`
+    // here, we ensure the build script is only re-run when
+    // `memory.x` is changed.
+    println!("cargo:rerun-if-changed=memory.x");
+}

hardware_main/memory.x

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

hardware_main/src/calibration.rs

@@ -0,0 +1,272 @@
+#![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

@@ -0,0 +1,152 @@
+#![deny(unsafe_code)]
+#![no_main]
+#![no_std]
+
+use core::f32::consts::PI;
+
+use calibration::Calibration;
+use cortex_m_rt::entry;
+use lsm303agr::interface::I2cInterface;
+use lsm303agr::mode::MagContinuous;
+use lsm303agr::{AccelOutputDataRate, Lsm303agr, MagOutputDataRate, Measurement};
+use microbit::hal::{gpiote::Gpiote, Twim};
+use microbit::pac::TWIM0;
+use panic_rtt_target as _;
+use rtt_target::{rprintln, rtt_init_print};
+
+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::{
+    led::{direction_to_led, theta_to_direction},
+    tilt_compensation::{
+        calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, Heading,
+        NedMeasurement,
+    },
+};
+
+const DELAY: u32 = 100;
+
+#[entry]
+fn main() -> ! {
+    rtt_init_print!();
+    let board = microbit::Board::take().unwrap();
+
+    #[cfg(feature = "v1")]
+    let i2c = { twi::Twi::new(board.TWI0, board.i2c.into(), FREQUENCY_A::K100) };
+
+    #[cfg(feature = "v2")]
+    let i2c = { twim::Twim::new(board.TWIM0, board.i2c_internal.into(), FREQUENCY_A::K100) };
+
+    let mut timer = Timer::new(board.TIMER0);
+    let mut display = Display::new(board.display_pins);
+
+    let gpiote = Gpiote::new(board.GPIOTE);
+    let channel_button_a = gpiote.channel0();
+    channel_button_a
+        .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();
+
+    //TODO: re-callibrate with button.
+    #[cfg(feature = "calibration")]
+    let mut calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
+    #[cfg(not(feature = "calibration"))]
+    let mut calibration = calibration::Calibration::default();
+    rprintln!("Calibration: {:?}", calibration);
+
+    let mut tilt_correction_enabled: bool = true;
+
+    loop {
+        if channel_button_b.is_event_triggered() {
+            calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
+            channel_button_b.reset_events();
+            rprintln!("Calibration: {:?}", calibration);
+        }
+        if channel_button_a.is_event_triggered() {
+            //toggles the bool.
+            tilt_correction_enabled ^= true;
+            channel_button_a.reset_events()
+        }
+
+        let heading = calc_heading(&mut sensor, &calibration, &tilt_correction_enabled);
+        display.show(
+            &mut timer,
+            direction_to_led(theta_to_direction(heading)),
+            DELAY,
+        )
+    }
+}
+
+/// board has forward in the y direction and right in the -x direction, and down in the -z. (ENU),  algs for tilt compensation
+/// need forward in +x and right in +y (this is known as the NED (north, east, down) cordinate
+/// system)
+/// also converts to f32
+pub fn swd_to_ned(measurement: Measurement) -> NedMeasurement {
+    NedMeasurement {
+        x: -measurement.y as f32,
+        y: -measurement.x as f32,
+        z: -measurement.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 = swd_to_ned(mag_data);
+    let ned_acel_data = swd_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(not(feature = "calibration"))]
+    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),
+    );
+    #[cfg(not(feature = "calibration"))]
+    rprintln!(
+        "x: {:<+16}, y: {:<+16}, z: {:<+16}",
+        ned_acel_data.x,
+        ned_acel_data.y,
+        ned_acel_data.z
+    );
+    heading
+}