made heading_to_sector.

This commit is contained in:
Gabe Venberg 2023-10-27 14:28:10 -05:00
parent 1afb386f04
commit 501230e121
3 changed files with 52 additions and 26 deletions

View file

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

View file

@ -92,23 +92,42 @@ pub fn direction_to_led(direction: Direction) -> [[u8; 5]; 5] {
} }
pub fn theta_to_direction(heading: Heading) -> Direction { 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.) { 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 Direction::South
} else if heading.0 < (3. * PI / 8.) { } else if heading.0 < (-5. * PI / 8.) {
Direction::SouthEast Direction::SouthEast
} else if heading.0 < (5. * PI / 8.) { } else if heading.0 < (-3. * PI / 8.) {
Direction::East Direction::East
} else if heading.0 < (7. * PI / 8.) { } else if heading.0 < (-PI / 8.) {
Direction::NorthEast Direction::NorthEast
} else { } else if heading.0 < (PI / 8.) {
Direction::North 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
} }
} }

View file

@ -18,14 +18,14 @@ 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. (SWU), algs for tilt compensation /// 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 /// need forward in +x and right in +y (this is known as the NED (north, east, down) cordinate
/// system) /// system)
/// also converts to f32 /// also converts to f32
pub fn swd_to_ned(measurement: Measurement) -> NedMeasurement { pub fn swd_to_ned(measurement: Measurement) -> NedMeasurement {
NedMeasurement { NedMeasurement {
x: measurement.y as f32, x: -measurement.y as f32,
y: measurement.x as f32, y: -measurement.x as f32,
z: -measurement.z as f32, z: -measurement.z as f32,
} }
} }