Added unit tests!!

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

@@ -0,0 +1,53 @@
+#![allow(unused)]
+use core::f32::consts::PI;
+
+use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point, UPoint};
+
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+struct Sector {
+    total_sectors: usize,
+    sector: usize,
+}
+
+//heading starts at north, with the positive direction being clockwise.
+//Heading ranges from -pi to pi.
+//
+//sectors have 0 at north an proceed clockwise, always being positive.
+fn heading_to_sector(sectors: usize, heading: f32) -> Sector {
+    let half_sector = PI / sectors as f32;
+    let sector_size = 2.0 * half_sector;
+    Sector {
+        total_sectors: sectors,
+        sector: (modulo(heading + half_sector, 2.0 * PI) / (sector_size)) as usize,
+    }
+}
+
+fn modulo(a: f32, b: f32) -> f32 {
+    ((a % b) + b) % b
+}
+
+fn heading_to_line(heading: f32, square_size: usize) -> Line {
+    todo!()
+}
+
+pub fn draw_heading<const X: usize, const Y: usize>(
+    heading: f32,
+) -> FourQuadrantMatrix<{ X }, { Y }, u8> {
+    let mut ret = FourQuadrantMatrix::new(UPoint { x: X / 2, y: Y / 2 });
+    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/led.rs

@@ -0,0 +1,133 @@
+use core::f32::consts::PI;
+use crate::tilt_compensation::Heading;
+
+#[derive(Debug)]
+pub enum Direction {
+    North,
+    NorthEast,
+    East,
+    SouthEast,
+    South,
+    SouthWest,
+    West,
+    NorthWest,
+}
+
+//forward is towards usb port
+const NORTH: [[u8; 5]; 5] = [
+    [0, 0, 1, 0, 0],
+    [0, 1, 1, 1, 0],
+    [1, 0, 1, 0, 1],
+    [0, 0, 1, 0, 0],
+    [0, 0, 1, 0, 0],
+];
+
+const NORTH_EAST: [[u8; 5]; 5] = [
+    [1, 1, 1, 0, 0],
+    [1, 1, 0, 0, 0],
+    [1, 0, 1, 0, 0],
+    [0, 0, 0, 1, 0],
+    [0, 0, 0, 0, 1],
+];
+
+const EAST: [[u8; 5]; 5] = [
+    [0, 0, 1, 0, 0],
+    [0, 1, 0, 0, 0],
+    [1, 1, 1, 1, 1],
+    [0, 1, 0, 0, 0],
+    [0, 0, 1, 0, 0],
+];
+
+const SOUTH_EAST: [[u8; 5]; 5] = [
+    [0, 0, 0, 0, 1],
+    [0, 0, 0, 1, 0],
+    [1, 0, 1, 0, 0],
+    [1, 1, 0, 0, 0],
+    [1, 1, 1, 0, 0],
+];
+
+const SOUTH: [[u8; 5]; 5] = [
+    [0, 0, 1, 0, 0],
+    [0, 0, 1, 0, 0],
+    [1, 0, 1, 0, 1],
+    [0, 1, 1, 1, 0],
+    [0, 0, 1, 0, 0],
+];
+
+const SOUTH_WEST: [[u8; 5]; 5] = [
+    [1, 0, 0, 0, 0],
+    [0, 1, 0, 0, 0],
+    [0, 0, 1, 0, 1],
+    [0, 0, 0, 1, 1],
+    [0, 0, 1, 1, 1],
+];
+
+const WEST: [[u8; 5]; 5] = [
+    [0, 0, 1, 0, 0],
+    [0, 0, 0, 1, 0],
+    [1, 1, 1, 1, 1],
+    [0, 0, 0, 1, 0],
+    [0, 0, 1, 0, 0],
+];
+
+const NORTH_WEST: [[u8; 5]; 5] = [
+    [0, 0, 1, 1, 1],
+    [0, 0, 0, 1, 1],
+    [0, 0, 1, 0, 1],
+    [0, 1, 0, 0, 0],
+    [1, 0, 0, 0, 0],
+];
+
+pub fn direction_to_led(direction: Direction) -> [[u8; 5]; 5] {
+    match direction {
+        Direction::North => NORTH,
+        Direction::NorthEast => NORTH_EAST,
+        Direction::East => EAST,
+        Direction::SouthEast => SOUTH_EAST,
+        Direction::South => SOUTH,
+        Direction::SouthWest => SOUTH_WEST,
+        Direction::West => WEST,
+        Direction::NorthWest => NORTH_WEST,
+    }
+}
+
+pub fn theta_to_direction(heading: Heading) -> Direction {
+    // if heading.0 < (-7. * PI / 8.) {
+    //     Direction::North
+    // } else if heading.0 < (-5. * PI / 8.) {
+    //     Direction::NorthWest
+    // } else if heading.0 < (-3. * PI / 8.) {
+    //     Direction::West
+    // } else if heading.0 < (-PI / 8.) {
+    //     Direction::SouthWest
+    // } else if heading.0 < (PI / 8.) {
+    //     Direction::South
+    // } else if heading.0 < (3. * PI / 8.) {
+    //     Direction::SouthEast
+    // } else if heading.0 < (5. * PI / 8.) {
+    //     Direction::East
+    // } else if heading.0 < (7. * PI / 8.) {
+    //     Direction::NorthEast
+    // } else {
+    //     Direction::North
+    // }
+    if heading.0 < (-7. * PI / 8.) {
+        Direction::South
+    } else if heading.0 < (-5. * PI / 8.) {
+        Direction::SouthEast
+    } else if heading.0 < (-3. * PI / 8.) {
+        Direction::East
+    } else if heading.0 < (-PI / 8.) {
+        Direction::NorthEast
+    } else if heading.0 < (PI / 8.) {
+        Direction::North
+    } else if heading.0 < (3. * PI / 8.) {
+        Direction::NorthWest
+    } else if heading.0 < (5. * PI / 8.) {
+        Direction::West
+    } else if heading.0 < (7. * PI / 8.) {
+        Direction::SouthWest
+    } else {
+        Direction::South
+    }
+}

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

@@ -0,0 +1,53 @@
+use libm::{atan2f, atanf, cosf, sinf};
+
+#[derive(Debug)]
+pub struct Attitude {
+    pub pitch: f32,
+    pub roll: f32,
+}
+
+#[derive(Debug)]
+pub struct NedMeasurement {
+    pub x: f32,
+    pub y: f32,
+    pub z: f32,
+}
+
+//theta=0 at north, pi/-pi at south, pi/2 at east, and -pi/2 at west (desired)
+//theta=0 at south, pi/-pi at north, pi/2 at east, and -pi/2 at west (current)
+pub struct Heading(pub 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);
+    let pitch = atanf(-measurement.x / (measurement.y * sinf(roll) + measurement.z * cosf(roll)));
+    Attitude { pitch, roll }
+    // Attitude { pitch: 0.0, roll: 0.0 }
+}
+
+pub fn calc_tilt_calibrated_measurement(
+    mag_measurement: NedMeasurement,
+    attitde: &Attitude,
+) -> NedMeasurement {
+    //based off of: https://www.nxp.com/docs/en/application-note/AN4248.pdf
+
+    let corrected_mag_y =
+        mag_measurement.z * sinf(attitde.roll) - mag_measurement.y * cosf(attitde.roll);
+
+    let corrected_mag_x = mag_measurement.x * cosf(attitde.pitch)
+        + mag_measurement.y * sinf(attitde.pitch) * sinf(attitde.roll)
+        + mag_measurement.z * sinf(attitde.pitch) * cosf(attitde.roll);
+
+    NedMeasurement {
+        x: corrected_mag_x,
+        y: corrected_mag_y,
+        z: 0.0,
+    }
+}
+
+//0 is the top sector and positive is clockwise, negative is counterclockwise.
+pub fn heading_from_measurement(measurement: NedMeasurement) -> Heading {
+    Heading(atan2f(-measurement.y, measurement.x))
+}
+
+//I have no freaking clue how to test this...