diff --git a/.cargo/config.toml b/.cargo/config.toml new file mode 100644 index 0000000..6dc9cfd --- /dev/null +++ b/.cargo/config.toml @@ -0,0 +1,4 @@ +[target.'cfg(all(target_arch = "arm", target_os = "none"))'] + +[build] +target = "thumbv7em-none-eabihf" diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..bb17919 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "rust-analyzer.check.allTargets": false +} diff --git a/Cargo.toml b/Cargo.toml index 780fe94..4ab865d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -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" } diff --git a/hardware_main/.cargo/config.toml b/hardware_main/.cargo/config.toml index 563f6ee..ff8b44d 100644 --- a/hardware_main/.cargo/config.toml +++ b/hardware_main/.cargo/config.toml @@ -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" diff --git a/hardware_main/Cargo.toml b/hardware_main/Cargo.toml index 57e153a..ba9ad06 100644 --- a/hardware_main/Cargo.toml +++ b/hardware_main/Cargo.toml @@ -1,30 +1,23 @@ [package] name = "led-compass" version = "0.1.0" -authors = ["Henrik Böving "] -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" diff --git a/hardware_main/Embed.toml b/hardware_main/Embed.toml index b2535bd..2f0badf 100644 --- a/hardware_main/Embed.toml +++ b/hardware_main/Embed.toml @@ -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 diff --git a/hardware_main/memory.x b/hardware_main/memory.x index 9e2ab65..b488bbf 100644 --- a/hardware_main/memory.x +++ b/hardware_main/memory.x @@ -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 } diff --git a/hardware_main/src/calibration.rs b/hardware_main/src/calibration.rs deleted file mode 100644 index 0b0a6e7..0000000 --- a/hardware_main/src/calibration.rs +++ /dev/null @@ -1,272 +0,0 @@ -#![allow(unused)] -//! Translated from - -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( - sensor: &mut Lsm303agr, MagContinuous>, - display: &mut Display, - timer: &mut T, -) -> Calibration -where - T: DelayUs, - I: Write + WriteRead, - E: Debug, -{ - let data = get_data(sensor, display, timer); - calibrate(&data) -} - -fn get_data( - sensor: &mut Lsm303agr, MagContinuous>, - display: &mut Display, - timer: &mut T, -) -> [Measurement; 25] -where - T: DelayUs, - I: Write + WriteRead, - 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, - } -} diff --git a/hardware_main/src/main.rs b/hardware_main/src/main.rs index dc3053a..be277d9 100644 --- a/hardware_main/src/main.rs +++ b/hardware_main/src/main.rs @@ -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 = 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; } - // 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>, 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>, 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 -} diff --git a/independent_logic/.cargo/config.toml b/independent_logic/.cargo/config.toml new file mode 100644 index 0000000..2f05654 --- /dev/null +++ b/independent_logic/.cargo/config.toml @@ -0,0 +1,2 @@ +[build] +target = "x86_64-unknown-linux-gnu" diff --git a/independent_logic/Cargo.toml b/independent_logic/Cargo.toml index e956257..27183e3 100644 --- a/independent_logic/Cargo.toml +++ b/independent_logic/Cargo.toml @@ -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" diff --git a/independent_logic/src/heading_drawing.rs b/independent_logic/src/heading_drawing.rs index 952291c..f4b2ce0 100644 --- a/independent_logic/src/heading_drawing.rs +++ b/independent_logic/src/heading_drawing.rs @@ -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( heading: Heading, - matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>, + matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>, ) { - draw_line::(&heading_to_line(heading, X.min(Y)), matrix); + matrix.reset_matrix(); + draw_line::(&heading_to_line(Heading(-heading.0), X.min(Y)), matrix); } diff --git a/independent_logic/src/line_drawing.rs b/independent_logic/src/line_drawing.rs index d34c765..25966d0 100644 --- a/independent_logic/src/line_drawing.rs +++ b/independent_logic/src/line_drawing.rs @@ -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 { matrix: [[T; X]; Y], max_point: Point, @@ -139,6 +141,12 @@ impl From } } +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( 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( 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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!( - as Into<[[u8; 5]; 5]>>::into(canvas), + 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], ] ) } diff --git a/independent_logic/src/tilt_compensation.rs b/independent_logic/src/tilt_compensation.rs index f0dc8d3..d59648b 100644 --- a/independent_logic/src/tilt_compensation.rs +++ b/independent_logic/src/tilt_compensation.rs @@ -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 {