Added unit tests!!
This commit is contained in:
parent
501230e121
commit
3e29d8bc6a
15 changed files with 335 additions and 183 deletions
53
independent_logic/src/heading_drawing.rs
Normal file
53
independent_logic/src/heading_drawing.rs
Normal file
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
133
independent_logic/src/led.rs
Normal file
133
independent_logic/src/led.rs
Normal file
|
|
@ -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
|
||||
}
|
||||
}
|
||||
5
independent_logic/src/lib.rs
Normal file
5
independent_logic/src/lib.rs
Normal file
|
|
@ -0,0 +1,5 @@
|
|||
#![no_std]
|
||||
pub mod heading_drawing;
|
||||
pub mod line_drawing;
|
||||
pub mod tilt_compensation;
|
||||
pub mod led;
|
||||
249
independent_logic/src/line_drawing.rs
Normal file
249
independent_logic/src/line_drawing.rs
Normal 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],
|
||||
]
|
||||
)
|
||||
}
|
||||
}
|
||||
53
independent_logic/src/tilt_compensation.rs
Normal file
53
independent_logic/src/tilt_compensation.rs
Normal file
|
|
@ -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...
|
||||
Loading…
Add table
Add a link
Reference in a new issue