made heading_to_sector.
This commit is contained in:
parent
1afb386f04
commit
501230e121
|
@ -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);
|
||||||
§or_to_line(heading_to_sector(sectors, heading), X.max(Y)),
|
|
||||||
&mut ret,
|
|
||||||
);
|
|
||||||
ret
|
ret
|
||||||
}
|
}
|
||||||
|
|
43
src/led.rs
43
src/led.rs
|
@ -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
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -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,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in a new issue