Added unit tests!!

Cargo.toml

@@ -1,28 +1,3 @@
-[package]
+[workspace]
-name = "led-compass"
+members = ["hardware_main", "independent_logic"]
-version = "0.1.0"
+resolver = "2"
-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"
-
-[features]
-v2 = ["microbit-v2"]
-v1 = ["microbit"]
-calibration=[]
-default = ["v2"]

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/src/main.rs

@@ -8,16 +8,13 @@ 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;
-mod led;
-mod tilt_compensation;
-mod line_drawing;
-mod heading_drawing;
 
 use microbit::{display::blocking::Display, hal::Timer};
 
@@ -27,14 +24,14 @@ use microbit::{hal::twi, pac::twi0::frequency::FREQUENCY_A};
 #[cfg(feature = "v2")]
 use microbit::{hal::twim, pac::twim0::frequency::FREQUENCY_A};
 
-use lsm303agr::{AccelOutputDataRate, Lsm303agr, MagOutputDataRate};
-use tilt_compensation::Heading;
-
 use crate::calibration::calc_calibration;
 
-use crate::led::{direction_to_led, theta_to_direction};
+use independent_logic::{
-use crate::tilt_compensation::{
+    led::{direction_to_led, theta_to_direction},
-    calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, swd_to_ned,
+    tilt_compensation::{
+        calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, Heading,
+        NedMeasurement,
+    },
 };
 
 const DELAY: u32 = 100;
@@ -102,9 +99,22 @@ fn main() -> ! {
     }
 }
 
+/// 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
+    mag_calibration: &Calibration,
+    tilt_correction_enabled: &bool,
 ) -> Heading {
     while !(sensor.mag_status().unwrap().xyz_new_data
         && sensor.accel_status().unwrap().xyz_new_data)

independent_logic/Cargo.toml

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

independent_logic/src/heading_drawing.rs

@@ -1,8 +1,9 @@
+#![allow(unused)]
 use core::f32::consts::PI;
 
 use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point, UPoint};
 
-#[derive(Debug, Clone, Copy)]
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
 struct Sector {
     total_sectors: usize,
     sector: usize,
@@ -36,3 +37,17 @@ pub fn draw_heading<const X: usize, const Y: usize>(
     draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), &mut ret);
     ret
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn sectors() {
+        assert_eq!(heading_to_sector(4,0.0).sector, 0);
+        assert_eq!(heading_to_sector(4,PI/2.0).sector, 1);
+        assert_eq!(heading_to_sector(4,-PI/2.0).sector, 3);
+        assert_eq!(heading_to_sector(4,PI).sector, 2);
+        assert_eq!(heading_to_sector(4,-PI).sector, 2);
+    }
+}

independent_logic/src/lib.rs

@@ -0,0 +1,5 @@
+#![no_std]
+pub mod heading_drawing;
+pub mod line_drawing;
+pub mod tilt_compensation;
+pub mod led;

independent_logic/src/line_drawing.rs

@@ -0,0 +1,249 @@
+use core::{
+    mem::swap,
+    ops::{Index, IndexMut},
+};
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct Point {
+    pub x: isize,
+    pub y: isize,
+}
+
+impl Point {
+    /// converts a point (representing a point on a 4 quadrant grid with positive xy in the
+    /// top-right) into a upoint (representing a point on a 1 quadrant grid with the origin in the
+    /// top-left corner). Returns none if the resulting point would have either number negative.
+    pub fn to_upoint(self, zero_coord: &UPoint) -> Option<UPoint> {
+        Some(UPoint {
+            x: zero_coord.x.checked_add_signed(self.x)?,
+            y: zero_coord.y.checked_add_signed(-self.y)?,
+        })
+    }
+}
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct UPoint {
+    pub x: usize,
+    pub y: usize,
+}
+
+impl UPoint {
+    /// converts a upoint (representing a point on a 1 quadrant grid with the origin in the
+    /// top-left corner) into a point( representing a point on a 4 quadrant grid with positive xy
+    /// in the top-right)
+    pub fn to_point(self, zero_coord: &UPoint) -> Point {
+        Point {
+            x: -(zero_coord.x as isize - self.x as isize),
+            y: zero_coord.y as isize - self.y as isize,
+        }
+    }
+}
+
+/// A matrix that allows negative co-oordinates. Will panic if referencing out of bounds, just like
+/// a nomral matrix.
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct FourQuadrantMatrix<const X: usize, const Y: usize, T> {
+    matrix: [[T; X]; Y],
+    pub zero_coord: UPoint,
+}
+
+impl<const X: usize, const Y: usize, T> FourQuadrantMatrix<{ X }, { Y }, T>
+where
+    T: Copy,
+    T: Default,
+{
+    pub fn new(zero_coord: UPoint) -> FourQuadrantMatrix<{ X }, { Y }, T> {
+        FourQuadrantMatrix {
+            matrix: [[T::default(); X]; Y],
+            zero_coord,
+        }
+    }
+}
+
+impl<T, const X: usize, const Y: usize> IndexMut<Point> for FourQuadrantMatrix<{ X }, { Y }, T> {
+    fn index_mut(&mut self, index: Point) -> &mut Self::Output {
+        let upoint = index
+            .to_upoint(&self.zero_coord)
+            .expect("would result in negative unsigned coordinate!");
+        &mut self.matrix[upoint.y][upoint.x]
+    }
+}
+
+impl<T, const X: usize, const Y: usize> Index<Point> for FourQuadrantMatrix<{ X }, { Y }, T> {
+    type Output = T;
+
+    fn index(&self, index: Point) -> &Self::Output {
+        let upoint = index
+            .to_upoint(&self.zero_coord)
+            .expect("would result in negative unsigned coordinate!");
+        &self.matrix[upoint.y][upoint.x]
+    }
+}
+
+impl<T, const X: usize, const Y: usize> From<FourQuadrantMatrix<{ X }, { Y }, T>> for [[T; X]; Y] {
+    fn from(value: FourQuadrantMatrix<{ X }, { Y }, T>) -> Self {
+        value.matrix
+    }
+}
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct Line(pub Point, pub Point);
+
+//no boxes here!
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub struct ULine(pub UPoint, pub UPoint);
+
+/// Renders a line into a matrix of pixels.
+pub fn draw_line<const X: usize, const Y: usize>(
+    line: &Line,
+    matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
+) {
+    let mut line = *line;
+    let steep = (line.0.x - line.1.x).abs() < (line.0.y - line.1.x).abs();
+
+    if steep {
+        swap(&mut line.0.x, &mut line.0.y);
+        swap(&mut line.1.x, &mut line.1.y);
+    }
+
+    if line.0.x > line.1.x {
+        swap(&mut line.0.x, &mut line.1.x);
+        swap(&mut line.0.y, &mut line.1.y)
+    }
+
+    let dx = line.1.x - line.0.x;
+    let dy = line.1.y - line.0.y;
+    let derror2 = dy.abs() * 2;
+    let mut error2 = 0;
+    let mut y = line.0.y;
+
+    for x in line.0.x..=line.1.x {
+        if steep {
+            matrix[Point { x: y, y: x }] = 1;
+        } else {
+            matrix[Point { x, y }] = 1;
+        }
+
+        error2 += derror2;
+
+        if error2 > dx {
+            y += if line.1.y > line.0.y { 1 } else { -1 };
+            error2 -= dx * 2
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    #[test]
+    fn point_upoint_conv() {
+        let zero_coord = UPoint { x: 2, y: 2 };
+        let point = Point { x: -1, y: -1 };
+        let upoint = point.to_upoint(&zero_coord).unwrap();
+        assert_eq!(upoint, UPoint { x: 1, y: 3 });
+        assert_eq!(upoint.to_point(&zero_coord), point);
+
+        let point = Point { x: -2, y: 1 };
+        let upoint = point.to_upoint(&zero_coord).unwrap();
+        assert_eq!(upoint, UPoint { x: 0, y: 1 });
+        assert_eq!(upoint.to_point(&zero_coord), point);
+
+        let point = Point { x: 2, y: 2 };
+        let upoint = point.to_upoint(&zero_coord).unwrap();
+        assert_eq!(upoint, UPoint { x: 4, y: 0 });
+        assert_eq!(upoint.to_point(&zero_coord), point);
+    }
+
+    #[test]
+    fn four_quadrant_matrix() {
+        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+            FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
+        canvas[Point { x: 0, y: 0 }] = 1;
+        assert_eq!(
+            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            [
+                [0, 0, 0, 0, 0],
+                [0, 0, 0, 0, 0],
+                [0, 0, 1, 0, 0],
+                [0, 0, 0, 0, 0],
+                [0, 0, 0, 0, 0],
+            ]
+        );
+        canvas[Point { x: -2, y: 1 }] = 1;
+        assert_eq!(
+            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            [
+                [0, 0, 0, 0, 0],
+                [1, 0, 0, 0, 0],
+                [0, 0, 1, 0, 0],
+                [0, 0, 0, 0, 0],
+                [0, 0, 0, 0, 0]
+            ]
+        );
+    }
+
+    #[test]
+    fn diagonal_unsigned_line() {
+        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+            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),
+            [
+                [0, 0, 0, 0, 1],
+                [0, 0, 0, 1, 0],
+                [0, 0, 1, 0, 0],
+                [0, 1, 0, 0, 0],
+                [1, 0, 0, 0, 0],
+            ]
+        )
+    }
+
+    #[test]
+    fn diagonal_signed_line() {
+        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+            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),
+            [
+                [0, 0, 0, 0, 1],
+                [0, 0, 0, 1, 0],
+                [0, 0, 1, 0, 0],
+                [0, 1, 0, 0, 0],
+                [1, 0, 0, 0, 0],
+            ]
+        )
+    }
+
+    #[test]
+    fn cross_signed_line() {
+        let mut canvas: FourQuadrantMatrix<5, 5, u8> =
+            FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
+        draw_line(
+            &Line(Point { x: 0, y: -2 }, Point { x: 0, y: 2 }),
+            &mut canvas,
+        );
+        draw_line(
+            &Line(Point { x: -2, y: 0 }, Point { x: 2, y: 0 }),
+            &mut canvas,
+        );
+        assert_eq!(
+            <FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
+            [
+                [0, 0, 1, 0, 0],
+                [0, 0, 1, 0, 0],
+                [1, 1, 1, 1, 1],
+                [0, 0, 1, 0, 0],
+                [0, 0, 1, 0, 0],
+            ]
+        )
+    }
+}

independent_logic/src/tilt_compensation.rs

@@ -1,5 +1,4 @@
 use libm::{atan2f, atanf, cosf, sinf};
-use lsm303agr::Measurement;
 
 #[derive(Debug)]
 pub struct Attitude {
@@ -18,18 +17,6 @@ pub struct NedMeasurement {
 //theta=0 at south, pi/-pi at north, pi/2 at east, and -pi/2 at west (current)
 pub struct Heading(pub f32);
 
-/// 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,
-    }
-}
-
 pub fn calc_attitude(measurement: &NedMeasurement) -> Attitude {
     //based off of: https://www.nxp.com/docs/en/application-note/AN4248.pdf
     let roll = atan2f(measurement.y, measurement.z);
@@ -62,3 +49,5 @@ pub fn calc_tilt_calibrated_measurement(
 pub fn heading_from_measurement(measurement: NedMeasurement) -> Heading {
     Heading(atan2f(-measurement.y, measurement.x))
 }
+
+//I have no freaking clue how to test this...

src/line_drawing.rs

@@ -1,129 +0,0 @@
-use core::{mem::swap, ops::Index, ops::IndexMut};
-
-#[derive(Debug, Clone, Copy)]
-pub struct Point {
-    pub x: isize,
-    pub y: isize,
-}
-
-impl Point {
-    /// converts a point (representing a point on a 4 quadrant grid) into a upoint (representing a
-    /// point on a 1 quadrant grid with the origin in the bottom-left corner). Returns none if
-    /// the resulting point would have either number negative.
-    pub fn to_upoint(self, zero_coord: &UPoint) -> Option<UPoint> {
-        Some(UPoint {
-            x: zero_coord.x.checked_add_signed(self.x)?,
-            y: zero_coord.y.checked_add_signed(self.y)?,
-        })
-    }
-}
-
-#[derive(Debug, Clone, Copy)]
-pub struct UPoint {
-    pub x: usize,
-    pub y: usize,
-}
-
-impl UPoint {
-    /// converts a upoint (representing a point on a 1 quadrant grid with the origin in the
-    /// bottom-left corner) into a point( representing a point on a 4 quadrant grid)
-    pub fn to_point(self, zero_coord: &UPoint) -> Point {
-        Point {
-            x: zero_coord.x as isize - self.x as isize,
-            y: zero_coord.y as isize - self.y as isize,
-        }
-    }
-}
-
-/// A matrix that allows negative co-oordinates. Will panic if referencing out of bounds, just like
-/// a nomral matrix.
-pub struct FourQuadrantMatrix<const X: usize, const Y: usize, T> {
-    matrix: [[T; X]; Y],
-    pub zero_coord: UPoint,
-}
-
-impl<const X: usize, const Y: usize, T> FourQuadrantMatrix<{ X }, { Y }, T>
-where
-    T: Copy,
-    T: Default,
-{
-    pub fn new(zero_coord: UPoint) -> FourQuadrantMatrix<{ X }, { Y }, T> {
-        FourQuadrantMatrix {
-            matrix: [[T::default(); X]; Y],
-            zero_coord,
-        }
-    }
-}
-
-impl<T, const X: usize, const Y: usize> IndexMut<Point> for FourQuadrantMatrix<{ X }, { Y }, T> {
-    fn index_mut(&mut self, index: Point) -> &mut Self::Output {
-        let upoint = index
-            .to_upoint(&self.zero_coord)
-            .expect("would result in negative unsigned coordinate!");
-        &mut self.matrix[upoint.x][upoint.y]
-    }
-}
-
-impl<T, const X: usize, const Y: usize> Index<Point> for FourQuadrantMatrix<{ X }, { Y }, T> {
-    type Output = T;
-
-    fn index(&self, index: Point) -> &Self::Output {
-        let upoint = index
-            .to_upoint(&self.zero_coord)
-            .expect("would result in negative unsigned coordinate!");
-        &self.matrix[upoint.x][upoint.y]
-    }
-}
-
-impl<T, const X: usize, const Y: usize> From<FourQuadrantMatrix<{ X }, { Y }, T>> for [[T; X]; Y] {
-    fn from(value: FourQuadrantMatrix<{ X }, { Y }, T>) -> Self {
-        value.matrix
-    }
-}
-
-#[derive(Debug, Clone, Copy)]
-pub struct Line(pub Point, pub Point);
-
-//no boxes here!
-#[derive(Debug, Clone, Copy)]
-pub struct ULine(pub UPoint, pub UPoint);
-
-/// Renders a line into a matrix of pixels.
-pub fn draw_line<const X: usize, const Y: usize>(
-    line: &Line,
-    matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
-) {
-    let mut line = *line;
-    let steep = (line.0.x - line.1.x).abs() < (line.0.y - line.1.x).abs();
-
-    if steep {
-        swap(&mut line.0.x, &mut line.0.y);
-        swap(&mut line.1.x, &mut line.1.y);
-    }
-
-    if line.0.x > line.1.x {
-        swap(&mut line.0.x, &mut line.1.x);
-        swap(&mut line.0.y, &mut line.1.y)
-    }
-
-    let dx = line.1.x - line.0.x;
-    let dy = line.1.y - line.0.y;
-    let derror2 = dy.abs() * 2;
-    let mut error2 = 0;
-    let mut y = line.0.y;
-
-    for x in line.0.x..=line.1.x {
-        if steep {
-            matrix[Point { x: y, y: x }] = 1;
-        } else {
-            matrix[Point { x, y }] = 1;
-        }
-
-        error2 += derror2;
-
-        if error2 > dx {
-            y += if line.1.y > line.0.y { 1 } else { -1 };
-            error2 -= dx * 2
-        }
-    }
-}