Gabe/micro-bit-led-compass
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Compare commits

base: Gabe:26a1da2cb9afa5fc50c2951d6e049e05c0e825a2
Branches Tags
Gabe:main
Gabe:embassy
...
compare: Gabe:24c877382088fc782ac542c4a3e45b44565ebe71
Branches Tags
Gabe:embassy
Gabe:main

5 commits
26a1da2cb9 ... 24c8773820

Author SHA1 Message Date
Gabe Venberg
24c8773820 Completely ported compass to embassy. 2025-07-11 14:32:56 +02:00
Gabe Venberg
3471b71dbc ajusted line drawing from an array of u8 to array of bool. 2025-07-11 12:00:34 +02:00
Gabe Venberg
45578958a6 got embassy compass outputting a heading number. 2025-07-08 17:22:26 +02:00
Gabe Venberg
6f6660fd6a fixed rust-analyzer trying to check the top workspace without cross-compiling. 2025-07-07 12:01:26 +02:00
Gabe Venberg
b894d424aa WIP, learning embassy. 2025-06-24 23:27:07 +02:00
14 changed files with 247 additions and 535 deletions
Download patch file Download diff file Expand all files Collapse all files

4
.cargo/config.toml Normal file
View file

@ -0,0 +1,4 @@
[target.'cfg(all(target_arch = "arm", target_os = "none"))']
[build]
target = "thumbv7em-none-eabihf"

3
.vscode/settings.json vendored Normal file
View file

@ -0,0 +1,3 @@
{
"rust-analyzer.check.allTargets": false
}

9
Cargo.toml
View file

@ -3,8 +3,15 @@ members = ["hardware_main", "independent_logic"]
resolver = "2"
[profile.release]
strip = true # Automatically strip symbols from the binary.
opt-level = "z" # Optimize for size.
lto = true
codegen-units = 1
panic = "abort"
[patch.crates-io]
embassy-executor = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
embassy-futures = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
embassy-nrf = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
embassy-sync = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
embassy-time = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }
embassy-time-driver = { git = "https://github.com/embassy-rs/embassy.git", rev = "f35aa4005a63e8d478b2b95aaa2bfb316b72dece" }

4
hardware_main/.cargo/config.toml
View file

@ -1,7 +1,11 @@
[target.'cfg(all(target_arch = "arm", target_os = "none"))']
rustflags = [
"-C", "link-arg=-Tlink.x",
"-C", "link-arg=-Tdefmt.x",
]
[build]
target = "thumbv7em-none-eabihf"
[env]
DEFMT_LOG = "debug"

39
hardware_main/Cargo.toml
View file

@ -1,30 +1,23 @@
[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
authors = ["Gabriel Venberg"]
edition = "2024"
[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"] }
panic-halt = "0.2.0"
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"]
cortex-m = { version = "0.7", features = ["inline-asm", "critical-section-single-core"] }
cortex-m-rt = "0.7"
defmt = "1.0"
defmt-rtt = "1.0"
panic-probe = { version = "1.0", features = ["print-defmt"] }
embassy-executor = { version = "0.7", features = ["arch-cortex-m", "executor-thread", "defmt"] }
embassy-time = { version = "0.4", features = ["defmt", "defmt-timestamp-uptime"] }
embassy-futures = { version = "0.1", features = ["defmt"] }
embassy-sync = { version = "0.7", features = ["defmt"] }
microbit-bsp = { git = "https://github.com/lulf/microbit-bsp.git", rev = "19d555bfbbcfa39db6aac467673386662c39e299" }
libm = "0.2.15"
embedded-hal-async = "1.0.0"

4
hardware_main/Embed.toml
View file

@ -1,9 +1,5 @@
[default.general]
chip = "nrf52833_xxAA" # uncomment this line for micro:bit V2
# chip = "nrf51822_xxAA" # uncomment this line for micro:bit V1
[default.reset]
halt_afterwards = false
[default.rtt]
enabled = true

2
hardware_main/memory.x
View file

@ -1,6 +1,6 @@
MEMORY
{
/* NOTE K = KiBi = 1024 bytes */
FLASH : ORIGIN = 0x00000000, LENGTH = 256K
FLASH : ORIGIN = 0x00000000, LENGTH = 512K
RAM : ORIGIN = 0x20000000, LENGTH = 16K
}

272
hardware_main/src/calibration.rs
View file

@ -1,272 +0,0 @@
#![allow(unused)]
//! Translated from <https://github.com/lancaster-university/codal-microbit-v2/blob/006abf5566774fbcf674c0c7df27e8a9d20013de/source/MicroBitCompassCalibrator.cpp>
use core::fmt::Debug;
use embedded_hal::blocking::delay::DelayUs;
use embedded_hal::blocking::i2c::{Write, WriteRead};
use libm::{fabsf, sqrtf};
use lsm303agr::interface::I2cInterface;
use lsm303agr::mode::MagContinuous;
use lsm303agr::Lsm303agr;
use lsm303agr::Measurement;
use microbit::display::blocking::Display;
const PERIMETER_POINTS: usize = 25;
const PIXEL1_THRESHOLD: i32 = 200;
const PIXEL2_THRESHOLD: i32 = 600;
const CALIBRATION_INCREMENT: i32 = 200;
#[derive(Debug)]
pub struct Calibration {
center: Measurement,
scale: Measurement,
radius: u32,
}
impl Default for Calibration {
fn default() -> Calibration {
Calibration {
// center: Measurement { x: 0, y: 0, z: 0 },
// scale: Measurement {
// x: 1024,
// y: 1024,
// z: 1024,
// },
// radius: 0,
center: Measurement {
x: 2434,
y: 5528,
z: -40156,
},
scale: Measurement {
x: 1044,
y: 1042,
z: 1049,
},
radius: 61751,
}
}
}
pub fn calc_calibration<I, T, E>(
sensor: &mut Lsm303agr<I2cInterface<I>, MagContinuous>,
display: &mut Display,
timer: &mut T,
) -> Calibration
where
T: DelayUs<u32>,
I: Write<Error = E> + WriteRead<Error = E>,
E: Debug,
{
let data = get_data(sensor, display, timer);
calibrate(&data)
}
fn get_data<I, T, E>(
sensor: &mut Lsm303agr<I2cInterface<I>, MagContinuous>,
display: &mut Display,
timer: &mut T,
) -> [Measurement; 25]
where
T: DelayUs<u32>,
I: Write<Error = E> + WriteRead<Error = E>,
E: Debug,
{
let mut leds = [
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
];
let mut cursor = (2, 2);
let mut data = [Measurement { x: 0, y: 0, z: 0 }; PERIMETER_POINTS];
let mut samples = 0;
while samples < PERIMETER_POINTS {
while !sensor.accel_status().unwrap().xyz_new_data {}
let accel_data = sensor.accel_data().unwrap();
let x = accel_data.x;
let y = accel_data.y;
if x < -PIXEL2_THRESHOLD {
cursor.1 = 0;
} else if x < -PIXEL1_THRESHOLD {
cursor.1 = 1;
} else if x > PIXEL2_THRESHOLD {
cursor.1 = 4;
} else if x > PIXEL1_THRESHOLD {
cursor.1 = 3;
} else {
cursor.1 = 2;
}
if y < -PIXEL2_THRESHOLD {
cursor.0 = 0;
} else if y < -PIXEL1_THRESHOLD {
cursor.0 = 1;
} else if y > PIXEL2_THRESHOLD {
cursor.0 = 4;
} else if y > PIXEL1_THRESHOLD {
cursor.0 = 3;
} else {
cursor.0 = 2;
}
// Turn the y axis properly
cursor.0 = 4 - cursor.0;
if leds[cursor.0][cursor.1] != 1 {
leds[cursor.0][cursor.1] = 1;
while !sensor.mag_status().unwrap().xyz_new_data {}
let mag_data = measurement_to_enu(sensor.mag_data().unwrap());
data[samples] = mag_data;
samples += 1;
}
display.show(timer, leds, 200);
}
data
}
fn difference_square(a: Measurement, b: Measurement) -> f32 {
let dx = (a.x - b.x) as f32;
let dy = (a.y - b.y) as f32;
let dz = (a.z - b.z) as f32;
(dx * dx) + (dy * dy) + (dz * dz)
}
fn measure_score(center: Measurement, data: &[Measurement]) -> f32 {
let mut min_d = difference_square(center, data[0]);
let mut max_d = min_d;
for point in data[1..].iter() {
let d = difference_square(center, *point);
if d < min_d {
min_d = d;
}
if d > max_d {
max_d = d;
}
}
max_d - min_d
}
fn calibrate(data: &[Measurement]) -> Calibration {
// Approximate a center for the data
let mut center = Measurement { x: 0, y: 0, z: 0 };
let mut best = center;
for point in data {
center.x += point.x;
center.y += point.y;
center.z += point.z;
}
center.x /= data.len() as i32;
center.y /= data.len() as i32;
center.z /= data.len() as i32;
let mut current = center;
let mut score = measure_score(current, data);
// Calculate a fixpoint position
loop {
for x in [-CALIBRATION_INCREMENT, 0, CALIBRATION_INCREMENT] {
for y in [-CALIBRATION_INCREMENT, 0, CALIBRATION_INCREMENT] {
for z in [-CALIBRATION_INCREMENT, 0, CALIBRATION_INCREMENT] {
let mut attempt = current;
attempt.x += x;
attempt.y += y;
attempt.z += z;
let attempt_score = measure_score(attempt, data);
if attempt_score < score {
score = attempt_score;
best = attempt;
}
}
}
}
if best == current {
break;
}
current = best;
}
spherify(current, data)
}
fn spherify(center: Measurement, data: &[Measurement]) -> Calibration {
let mut radius = 0;
for point in data {
let d = sqrtf(difference_square(center, *point)) as u32;
if d > radius {
radius = d;
}
}
let mut scale: f32 = 0.0;
let mut weight_x = 0.0;
let mut weight_y = 0.0;
let mut weight_z = 0.0;
for point in data {
let d = sqrtf(difference_square(center, *point));
let s = (radius as f32 / d) - 1.0;
scale = scale.max(s);
let dx = point.x - center.x;
let dy = point.y - center.y;
let dz = point.z - center.z;
weight_x += s * fabsf(dx as f32 / d);
weight_y += s * fabsf(dy as f32 / d);
weight_z += s * fabsf(dz as f32 / d);
}
let wmag = sqrtf((weight_x * weight_x) + (weight_y * weight_y) + (weight_z * weight_z));
let scale_x = 1.0 + scale * (weight_x / wmag);
let scale_y = 1.0 + scale * (weight_y / wmag);
let scale_z = 1.0 + scale * (weight_z / wmag);
Calibration {
center,
radius,
scale: Measurement {
x: (1024.0 * scale_x) as i32,
y: (1024.0 * scale_y) as i32,
z: (1024.0 * scale_z) as i32,
},
}
}
pub fn calibrated_measurement(measurement: Measurement, calibration: &Calibration) -> Measurement {
let mut out = measurement_to_enu(measurement);
out = Measurement {
x: ((out.x - calibration.center.x) * calibration.scale.x) >> 10,
y: ((out.y - calibration.center.y) * calibration.scale.y) >> 10,
z: ((out.z - calibration.center.z) * calibration.scale.z) >> 10,
};
//to convert it back to the board-native SWU cordinates
measurement_to_enu(out)
}
fn measurement_to_enu(measurement: Measurement) -> Measurement {
Measurement {
x: -measurement.y,
y: -measurement.x,
z: measurement.z,
}
}
fn enu_to_cartesian(measurement: Measurement) -> Measurement {
Measurement {
x: -measurement.y,
y: measurement.x,
z: measurement.z,
}
}

255
hardware_main/src/main.rs
View file

@ -2,169 +2,132 @@
#![no_main]
#![no_std]
#[cfg(debug_assertions)]
use core::f32::consts::PI;
use calibration::Calibration;
use cortex_m_rt::entry;
use independent_logic::line_drawing::{FourQuadrantMatrix, UPoint};
use lsm303agr::interface::I2cInterface;
use lsm303agr::mode::MagContinuous;
use lsm303agr::{AccelOutputDataRate, Lsm303agr, MagOutputDataRate, Measurement};
use microbit::hal::{gpiote::Gpiote, Twim};
use microbit::pac::TWIM0;
#[cfg(not(debug_assertions))]
use panic_halt as _;
#[cfg(debug_assertions)]
use panic_rtt_target as _;
#[cfg(debug_assertions)]
use rtt_target::{rprintln, rtt_init_print};
mod calibration;
use microbit::{display::blocking::Display, hal::Timer};
#[cfg(feature = "v1")]
use microbit::{hal::twi, pac::twi0::frequency::FREQUENCY_A};
#[cfg(feature = "v2")]
use microbit::{hal::twim, pac::twim0::frequency::FREQUENCY_A};
use crate::calibration::calc_calibration;
use defmt::{debug, info};
use embassy_executor::Spawner;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
use embassy_time::{Duration, Ticker};
use microbit_bsp::{
LedMatrix, Microbit,
display::{Bitmap, Brightness, Frame},
embassy_nrf::{
bind_interrupts,
peripherals::TWISPI0,
twim::{InterruptHandler, Twim},
},
lsm303agr::{self, Lsm303agr, interface::I2cInterface, mode::MagContinuous},
motion::new_lsm303agr,
};
use {defmt_rtt as _, panic_probe as _};
use independent_logic::{
heading_drawing::draw_constant_heading,
line_drawing::{FourQuadrantMatrix, UPoint},
tilt_compensation::{
calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, Heading,
NedMeasurement,
Heading, NedMeasurement, calc_attitude, calc_tilt_calibrated_measurement,
heading_from_measurement,
},
};
const DELAY: u32 = 100;
static HEADING: Signal<CriticalSectionRawMutex, Heading> = Signal::new();
#[entry]
fn main() -> ! {
#[cfg(debug_assertions)]
rtt_init_print!();
let board = microbit::Board::take().unwrap();
#[embassy_executor::main]
async fn main(s: Spawner) {
let board = Microbit::default();
defmt::info!("Application started!");
#[cfg(feature = "v1")]
let i2c = { twi::Twi::new(board.TWI0, board.i2c.into(), FREQUENCY_A::K100) };
let mut display = board.display;
display.set_brightness(Brightness::MAX);
#[cfg(feature = "v2")]
let i2c = { twim::Twim::new(board.TWIM0, board.i2c_internal.into(), FREQUENCY_A::K100) };
let mut timer = Timer::new(board.TIMER0);
let mut display = Display::new(board.display_pins);
let gpiote = Gpiote::new(board.GPIOTE);
let channel_button_a = gpiote.channel0();
channel_button_a
.input_pin(&board.buttons.button_a.degrade())
.hi_to_lo();
channel_button_a.reset_events();
let channel_button_b = gpiote.channel1();
channel_button_b
.input_pin(&board.buttons.button_b.degrade())
.hi_to_lo();
channel_button_b.reset_events();
let mut sensor = Lsm303agr::new_with_i2c(i2c);
sensor.init().unwrap();
sensor.set_mag_odr(MagOutputDataRate::Hz10).unwrap();
sensor.set_accel_odr(AccelOutputDataRate::Hz10).unwrap();
let mut sensor = sensor.into_mag_continuous().ok().unwrap();
#[cfg(feature = "calibration")]
let mut calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
#[cfg(not(feature = "calibration"))]
let mut calibration = calibration::Calibration::default();
let mut current_display: FourQuadrantMatrix<5, 5, u8> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
#[cfg(debug_assertions)]
rprintln!("Calibration: {:?}", calibration);
let mut tilt_correction_enabled: bool = true;
// let mut heading = Heading(0.0);
loop {
if channel_button_b.is_event_triggered() {
calibration = calc_calibration(&mut sensor, &mut display, &mut timer);
channel_button_b.reset_events();
#[cfg(debug_assertions)]
rprintln!("Calibration: {:?}", calibration);
// Bind interrupt to the TWI/SPI peripheral.
bind_interrupts!(
struct InterruptRequests {
TWISPI0 => InterruptHandler<TWISPI0>;
}
// if channel_button_a.is_event_triggered() {
// //toggles the bool.
// tilt_correction_enabled ^= true;
// channel_button_a.reset_events()
// }
);
current_display.reset_matrix();
let irqs = InterruptRequests {};
let mut sensor = new_lsm303agr(board.twispi0, irqs, board.i2c_int_sda, board.i2c_int_scl);
sensor.init().await.unwrap();
sensor.enable_mag_offset_cancellation().await.unwrap();
sensor
.set_mag_mode_and_odr(
&mut embassy_time::Delay,
lsm303agr::MagMode::HighResolution,
lsm303agr::MagOutputDataRate::Hz50,
)
.await
.unwrap();
let Ok(mut sensor) = sensor.into_mag_continuous().await else {
panic!("Failed to set sensor to continuous mode");
};
sensor
.set_accel_mode_and_odr(
&mut embassy_time::Delay,
lsm303agr::AccelMode::Normal,
lsm303agr::AccelOutputDataRate::Hz50,
)
.await
.unwrap();
s.must_spawn(get_data(sensor));
s.must_spawn(display_data(display));
}
let heading = calc_heading(&mut sensor, &calibration, &tilt_correction_enabled);
draw_constant_heading::<5, 5>(heading, &mut current_display);
display.show(&mut timer, current_display.into(), DELAY)
#[embassy_executor::task]
async fn display_data(mut display: LedMatrix) {
let mut display_matrix: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
loop {
let heading = HEADING.wait().await;
info!("Heading: {}", heading.0 * (180.0 / PI));
draw_constant_heading(heading, &mut display_matrix);
display
.display(to_frame(&display_matrix), Duration::from_hz(25))
.await;
}
}
/// 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 enu_to_ned(measurement: Measurement) -> NedMeasurement {
#[embassy_executor::task]
async fn get_data(mut sensor: Lsm303agr<I2cInterface<Twim<'static, TWISPI0>>, MagContinuous>) {
let mut ticker = Ticker::every(Duration::from_hz(25));
loop {
let (x, y, z) = sensor
.magnetic_field()
.await
.expect("didnt get mag data")
.xyz_nt();
let mag_measurement = to_ned(x, y, z);
let (x, y, z) = sensor
.acceleration()
.await
.expect("didnt get accel data")
.xyz_mg();
let accel_measurement = to_ned(x, y, z);
debug!("Mag: {}, Accel: {}", mag_measurement, accel_measurement);
let attitude = calc_attitude(&accel_measurement);
let mag_measurement = calc_tilt_calibrated_measurement(mag_measurement, &attitude);
HEADING.signal(heading_from_measurement(&mag_measurement));
ticker.next().await;
}
}
// TODO: make the line drawing lib produce a slice of bitmaps directly.
fn to_frame(matrix: &FourQuadrantMatrix<5, 5, bool>) -> Frame<5, 5> {
Frame::new(
core::convert::Into::<&[[bool; 5]; 5]>::into(matrix).map(|bools| {
let mut bit: u8 = 0;
for (i, bool) in bools.into_iter().enumerate() {
bit |= (bool as u8) << i;
}
Bitmap::new(bit, 5)
}),
)
}
fn to_ned(x: i32, y: i32, z: i32) -> NedMeasurement {
NedMeasurement {
x: -measurement.y as f32,
y: measurement.x as f32,
z: -measurement.z as f32,
x: -y as f32,
y: x as f32,
z: -z as f32,
}
}
fn calc_heading(
sensor: &mut Lsm303agr<I2cInterface<Twim<TWIM0>>, MagContinuous>,
mag_calibration: &Calibration,
tilt_correction_enabled: &bool,
) -> Heading {
while !(sensor.mag_status().unwrap().xyz_new_data
&& sensor.accel_status().unwrap().xyz_new_data)
{}
let mag_data = sensor.mag_data().unwrap();
let mag_data = calibration::calibrated_measurement(mag_data, mag_calibration);
let acel_data = sensor.accel_data().unwrap();
let mut ned_mag_data = enu_to_ned(mag_data);
let ned_acel_data = enu_to_ned(acel_data);
let attitude = calc_attitude(&ned_acel_data);
if *tilt_correction_enabled {
ned_mag_data = calc_tilt_calibrated_measurement(ned_mag_data, &attitude);
}
//theta=0 at north, pi/-pi at south, pi/2 at east, and -pi/2 at west
let heading = heading_from_measurement(&ned_mag_data);
#[cfg(all(not(feature = "calibration"), debug_assertions))]
rprintln!(
"pitch: {:<+5.0}, roll: {:<+5.0}, heading: {:<+5.0}",
attitude.pitch * (180.0 / PI),
attitude.roll * (180.0 / PI),
heading.0 * (180.0 / PI),
);
rprintln!(
"mag: x: {:<+16}, y: {:<+16}, z: {:<+16}",
ned_mag_data.x,
ned_mag_data.y,
ned_mag_data.z
);
#[cfg(all(not(feature = "calibration"), debug_assertions))]
rprintln!(
"acell: x: {:<+16}, y: {:<+16}, z: {:<+16}",
ned_acel_data.x,
ned_acel_data.y,
ned_acel_data.z
);
heading
}

2
independent_logic/.cargo/config.toml Normal file
View file

@ -0,0 +1,2 @@
[build]
target = "x86_64-unknown-linux-gnu"

3
independent_logic/Cargo.toml
View file

@ -1,9 +1,10 @@
[package]
name = "independent_logic"
version = "0.1.0"
edition = "2021"
edition = "2024"
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
[dependencies]
defmt = "1.0.1"
libm = "0.2.1"

5
independent_logic/src/heading_drawing.rs
View file

@ -17,7 +17,8 @@ fn heading_to_line(heading: Heading, square_size: usize) -> Line {
// draws a line always pointing towards heading 0
pub fn draw_constant_heading<const X: usize, const Y: usize>(
heading: Heading,
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>,
) {
draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), matrix);
matrix.reset_matrix();
draw_line::<X, Y>(&heading_to_line(Heading(-heading.0), X.min(Y)), matrix);
}

174
independent_logic/src/line_drawing.rs
View file

@ -5,8 +5,10 @@ use core::{
#[cfg(test)]
use std::dbg;
use defmt::Format;
/// a signed point in 2d space
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)]
pub struct Point {
pub x: isize,
pub y: isize,
@ -25,7 +27,7 @@ impl Point {
}
/// an unsigned point in 2d space
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)]
pub struct UPoint {
pub x: usize,
pub y: usize,
@ -45,7 +47,7 @@ impl UPoint {
/// A matrix that allows negative co-oordinates. Will panic if referencing out of bounds, just like
/// a normal 2d array.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)]
pub struct FourQuadrantMatrix<const X: usize, const Y: usize, T> {
matrix: [[T; X]; Y],
max_point: Point,
@ -139,6 +141,12 @@ impl<T, const X: usize, const Y: usize> From<FourQuadrantMatrix<{ X }, { Y }, T>
}
}
impl<'a, T, const X: usize, const Y: usize> From<&'a FourQuadrantMatrix<{ X }, { Y }, T>> for &'a [[T; X]; Y] {
fn from(value:&'a FourQuadrantMatrix<{ X }, { Y }, T>) -> Self {
&value.matrix
}
}
/// a line segment in 2d space, described by its two endpoints
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Line(pub Point, pub Point);
@ -148,7 +156,7 @@ pub struct Line(pub Point, pub Point);
/// extend past its edges.
pub fn draw_line<const X: usize, const Y: usize>(
line: &Line,
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>,
) {
let mut line = *line;
#[cfg(test)]
@ -201,7 +209,7 @@ pub fn draw_line<const X: usize, const Y: usize>(
dbg!(draw_point);
if matrix.is_in_bounds(&draw_point) {
matrix[draw_point] = 1;
matrix[draw_point] = true;
prev_out_of_bounds = false;
} else {
if !prev_out_of_bounds {
@ -249,195 +257,195 @@ mod tests {
#[test]
fn four_quadrant_matrix() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
canvas[Point { x: 0, y: 0 }] = 1;
canvas[Point { x: 0, y: 0 }] = true;
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 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],
[false, false, false, false, false],
[false, false, false, false, false],
[false, false, true, false, false],
[false, false, false, false, false],
[false, false, false, false, false],
]
);
canvas[Point { x: -2, y: 1 }] = 1;
canvas[Point { x: -2, y: 1 }] = true;
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 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]
[false, false, false, false, false],
[true, false, false, false, false],
[false, false, true, false, false],
[false, false, false, false, false],
[false, false, false, false, false]
]
);
}
#[test]
fn diagonal_unsigned_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
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),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 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],
[false, false, false, false, true],
[false, false, false, true, false],
[false, false, true, false, false],
[false, true, false, false, false],
[true, false, false, false, false],
]
)
}
#[test]
fn diagonal_signed_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
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),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 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],
[false, false, false, false, true],
[false, false, false, true, false],
[false, false, true, false, false],
[false, true, false, false, false],
[true, false, false, false, false],
]
)
}
#[test]
fn diagonal_signed_both_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: -10, y: -10 }, Point { x: 10, y: 10 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 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],
[false, false, false, false, true],
[false, false, false, true, false],
[false, false, true, false, false],
[false, true, false, false, false],
[true, false, false, false, false],
]
);
}
#[test]
fn diagonal_signed_first_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: -10, y: -10 }, Point { x: 2, y: 2 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 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],
[false, false, false, false, true],
[false, false, false, true, false],
[false, false, true, false, false],
[false, true, false, false, false],
[true, false, false, false, false],
]
);
}
#[test]
fn diagonal_signed_second_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: -2, y: -2 }, Point { x: 10, y: 10 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 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],
[false, false, false, false, true],
[false, false, false, true, false],
[false, false, true, false, false],
[false, true, false, false, false],
[true, false, false, false, false],
]
);
}
#[test]
fn vertical_signed_both_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: 0, y: -10 }, Point { x: 0, y: 10 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
[
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[false, false, true, false, false],
[false, false, true, false, false],
[false, false, true, false, false],
[false, false, true, false, false],
[false, false, true, false, false],
]
);
}
#[test]
fn vertical_signed_first_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: 0, y: -10 }, Point { x: 0, y: 0 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
[
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[false, false, false, false, false],
[false, false, false, false, false],
[false, false, true, false, false],
[false, false, true, false, false],
[false, false, true, false, false],
]
);
}
#[test]
fn vertical_signed_second_oob_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: 0, y: 0 }, Point { x: 0, y: 10 }),
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 5]; 5]>>::into(canvas),
[
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 1, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[false, false, true, false, false],
[false, false, true, false, false],
[false, false, true, false, false],
[false, false, false, false, false],
[false, false, false, false, false],
]
);
}
#[test]
fn cross_signed_line() {
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
let mut canvas: FourQuadrantMatrix<5, 5, bool> =
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
draw_line(
&Line(Point { x: 0, y: -2 }, Point { x: 0, y: 2 }),
@ -448,13 +456,13 @@ mod tests {
&mut canvas,
);
assert_eq!(
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
<FourQuadrantMatrix<5, 5, bool> as Into<[[bool; 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],
[false, false, true, false, false],
[false, false, true, false, false],
[true, true, true, true, true],
[false, false, true, false, false],
[false, false, true, false, false],
]
)
}

6
independent_logic/src/tilt_compensation.rs
View file

@ -1,12 +1,13 @@
use defmt::Format;
use libm::{atan2f, atanf, cosf, sinf};
#[derive(Debug)]
#[derive(Debug, Format)]
pub struct Attitude {
pub pitch: f32,
pub roll: f32,
}
#[derive(Debug)]
#[derive(Debug, Format)]
pub struct NedMeasurement {
pub x: f32,
pub y: f32,
@ -14,6 +15,7 @@ pub struct NedMeasurement {
}
///theta=0 at north, pi/-pi at south, pi/2 at east, and -pi/2 at west
#[derive(Debug, Format)]
pub struct Heading(pub f32);
pub fn calc_attitude(measurement: &NedMeasurement) -> Attitude {