diff --git a/Cargo.toml b/Cargo.toml index d0dc7b4..4f32696 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,28 +1,3 @@ -[package] -name = "led-compass" -version = "0.1.0" -authors = ["Henrik Böving "] -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"] +[workspace] +members = ["hardware_main", "independent_logic"] +resolver = "2" diff --git a/.cargo/config b/hardware_main/.cargo/config similarity index 100% rename from .cargo/config rename to hardware_main/.cargo/config diff --git a/hardware_main/Cargo.toml b/hardware_main/Cargo.toml new file mode 100644 index 0000000..cac4e30 --- /dev/null +++ b/hardware_main/Cargo.toml @@ -0,0 +1,29 @@ +[package] +name = "led-compass" +version = "0.1.0" +authors = ["Henrik Böving "] +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"] diff --git a/Embed.toml b/hardware_main/Embed.toml similarity index 100% rename from Embed.toml rename to hardware_main/Embed.toml diff --git a/build.rs b/hardware_main/build.rs similarity index 100% rename from build.rs rename to hardware_main/build.rs diff --git a/memory.x b/hardware_main/memory.x similarity index 100% rename from memory.x rename to hardware_main/memory.x diff --git a/src/calibration.rs b/hardware_main/src/calibration.rs similarity index 100% rename from src/calibration.rs rename to hardware_main/src/calibration.rs diff --git a/src/main.rs b/hardware_main/src/main.rs similarity index 83% rename from src/main.rs rename to hardware_main/src/main.rs index 4d829a0..7d8fe31 100644 --- a/src/main.rs +++ b/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 crate::tilt_compensation::{ - calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, swd_to_ned, +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; @@ -87,7 +84,7 @@ fn main() -> ! { channel_button_b.reset_events(); rprintln!("Calibration: {:?}", calibration); } - if channel_button_a.is_event_triggered(){ + if channel_button_a.is_event_triggered() { //toggles the bool. tilt_correction_enabled ^= true; channel_button_a.reset_events() @@ -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>, 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) diff --git a/independent_logic/Cargo.toml b/independent_logic/Cargo.toml new file mode 100644 index 0000000..e956257 --- /dev/null +++ b/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" diff --git a/src/heading_drawing.rs b/independent_logic/src/heading_drawing.rs similarity index 70% rename from src/heading_drawing.rs rename to independent_logic/src/heading_drawing.rs index 85a0a4c..d54cbe0 100644 --- a/src/heading_drawing.rs +++ b/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( draw_line::(&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); + } +} diff --git a/src/led.rs b/independent_logic/src/led.rs similarity index 100% rename from src/led.rs rename to independent_logic/src/led.rs diff --git a/independent_logic/src/lib.rs b/independent_logic/src/lib.rs new file mode 100644 index 0000000..24d4daa --- /dev/null +++ b/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; diff --git a/independent_logic/src/line_drawing.rs b/independent_logic/src/line_drawing.rs new file mode 100644 index 0000000..1afa770 --- /dev/null +++ b/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 { + 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 { + matrix: [[T; X]; Y], + pub zero_coord: UPoint, +} + +impl 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 IndexMut 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 Index 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 From> 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( + 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!( + 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!( + 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!( + 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!( + 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!( + 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], + ] + ) + } +} diff --git a/src/tilt_compensation.rs b/independent_logic/src/tilt_compensation.rs similarity index 76% rename from src/tilt_compensation.rs rename to independent_logic/src/tilt_compensation.rs index 7c6574b..423680f 100644 --- a/src/tilt_compensation.rs +++ b/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... diff --git a/src/line_drawing.rs b/src/line_drawing.rs deleted file mode 100644 index cd4eceb..0000000 --- a/src/line_drawing.rs +++ /dev/null @@ -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 { - 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 { - matrix: [[T; X]; Y], - pub zero_coord: UPoint, -} - -impl 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 IndexMut 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 Index 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 From> 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( - 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 - } - } -}