Added unit tests!!

This commit is contained in:
Gabe Venberg 2023-10-28 22:01:36 -05:00
parent 501230e121
commit 3e29d8bc6a
15 changed files with 335 additions and 183 deletions

View file

@ -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"]

29
hardware_main/Cargo.toml Normal file
View file

@ -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"]

View file

@ -8,16 +8,13 @@ use calibration::Calibration;
use cortex_m_rt::entry; use cortex_m_rt::entry;
use lsm303agr::interface::I2cInterface; use lsm303agr::interface::I2cInterface;
use lsm303agr::mode::MagContinuous; use lsm303agr::mode::MagContinuous;
use lsm303agr::{AccelOutputDataRate, Lsm303agr, MagOutputDataRate, Measurement};
use microbit::hal::{gpiote::Gpiote, Twim}; use microbit::hal::{gpiote::Gpiote, Twim};
use microbit::pac::TWIM0; use microbit::pac::TWIM0;
use panic_rtt_target as _; use panic_rtt_target as _;
use rtt_target::{rprintln, rtt_init_print}; use rtt_target::{rprintln, rtt_init_print};
mod calibration; mod calibration;
mod led;
mod tilt_compensation;
mod line_drawing;
mod heading_drawing;
use microbit::{display::blocking::Display, hal::Timer}; use microbit::{display::blocking::Display, hal::Timer};
@ -27,14 +24,14 @@ use microbit::{hal::twi, pac::twi0::frequency::FREQUENCY_A};
#[cfg(feature = "v2")] #[cfg(feature = "v2")]
use microbit::{hal::twim, pac::twim0::frequency::FREQUENCY_A}; 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::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; const DELAY: u32 = 100;
@ -87,7 +84,7 @@ fn main() -> ! {
channel_button_b.reset_events(); channel_button_b.reset_events();
rprintln!("Calibration: {:?}", calibration); rprintln!("Calibration: {:?}", calibration);
} }
if channel_button_a.is_event_triggered(){ if channel_button_a.is_event_triggered() {
//toggles the bool. //toggles the bool.
tilt_correction_enabled ^= true; tilt_correction_enabled ^= true;
channel_button_a.reset_events() 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( fn calc_heading(
sensor: &mut Lsm303agr<I2cInterface<Twim<TWIM0>>, MagContinuous>, sensor: &mut Lsm303agr<I2cInterface<Twim<TWIM0>>, MagContinuous>,
mag_calibration: &Calibration, tilt_correction_enabled: &bool mag_calibration: &Calibration,
tilt_correction_enabled: &bool,
) -> Heading { ) -> Heading {
while !(sensor.mag_status().unwrap().xyz_new_data while !(sensor.mag_status().unwrap().xyz_new_data
&& sensor.accel_status().unwrap().xyz_new_data) && sensor.accel_status().unwrap().xyz_new_data)

View file

@ -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"

View file

@ -1,8 +1,9 @@
#![allow(unused)]
use core::f32::consts::PI; use core::f32::consts::PI;
use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point, UPoint}; use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point, UPoint};
#[derive(Debug, Clone, Copy)] #[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct Sector { struct Sector {
total_sectors: usize, total_sectors: usize,
sector: 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); draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), &mut ret);
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);
}
}

View file

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

View file

@ -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],
]
)
}
}

View file

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

View file

@ -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
}
}
}