diff --git a/.cargo/config.toml b/.cargo/config.toml deleted file mode 100644 index 6dc9cfd..0000000 --- a/.cargo/config.toml +++ /dev/null @@ -1,4 +0,0 @@ -[target.'cfg(all(target_arch = "arm", target_os = "none"))'] - -[build] -target = "thumbv7em-none-eabihf" diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index bb17919..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "rust-analyzer.check.allTargets": false -} diff --git a/Cargo.toml b/Cargo.toml index 4ab865d..780fe94 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3,15 +3,8 @@ 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 ff8b44d..563f6ee 100644 --- a/hardware_main/.cargo/config.toml +++ b/hardware_main/.cargo/config.toml @@ -1,11 +1,7 @@ [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 ba9ad06..57e153a 100644 --- a/hardware_main/Cargo.toml +++ b/hardware_main/Cargo.toml @@ -1,23 +1,30 @@ [package] name = "led-compass" version = "0.1.0" -authors = ["Gabriel Venberg"] -edition = "2024" +authors = ["Henrik Böving "] +edition = "2018" + +[dependencies.microbit-v2] +version = "0.12.0" +optional = true + +[dependencies.microbit] +version = "0.12.0" +optional = true [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"} -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" +[features] +v2 = ["microbit-v2"] +v1 = ["microbit"] +calibration=[] +default = ["v2"] diff --git a/hardware_main/Embed.toml b/hardware_main/Embed.toml index 2f0badf..b2535bd 100644 --- a/hardware_main/Embed.toml +++ b/hardware_main/Embed.toml @@ -1,5 +1,9 @@ [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 b488bbf..9e2ab65 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 = 512K + FLASH : ORIGIN = 0x00000000, LENGTH = 256K RAM : ORIGIN = 0x20000000, LENGTH = 16K } diff --git a/hardware_main/src/calibration.rs b/hardware_main/src/calibration.rs new file mode 100644 index 0000000..0b0a6e7 --- /dev/null +++ b/hardware_main/src/calibration.rs @@ -0,0 +1,272 @@ +#![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 be277d9..dc3053a 100644 --- a/hardware_main/src/main.rs +++ b/hardware_main/src/main.rs @@ -2,132 +2,169 @@ #![no_main] #![no_std] +#[cfg(debug_assertions)] use core::f32::consts::PI; -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 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 independent_logic::{ heading_drawing::draw_constant_heading, - line_drawing::{FourQuadrantMatrix, UPoint}, tilt_compensation::{ - Heading, NedMeasurement, calc_attitude, calc_tilt_calibrated_measurement, - heading_from_measurement, + calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, Heading, + NedMeasurement, }, }; -static HEADING: Signal = Signal::new(); +const DELAY: u32 = 100; -#[embassy_executor::main] -async fn main(s: Spawner) { - let board = Microbit::default(); - defmt::info!("Application started!"); +#[entry] +fn main() -> ! { + #[cfg(debug_assertions)] + rtt_init_print!(); + let board = microbit::Board::take().unwrap(); - let mut display = board.display; - display.set_brightness(Brightness::MAX); + #[cfg(feature = "v1")] + let i2c = { twi::Twi::new(board.TWI0, board.i2c.into(), FREQUENCY_A::K100) }; - // Bind interrupt to the TWI/SPI peripheral. - bind_interrupts!( - struct InterruptRequests { - TWISPI0 => InterruptHandler; - } - ); + #[cfg(feature = "v2")] + let i2c = { twim::Twim::new(board.TWIM0, board.i2c_internal.into(), FREQUENCY_A::K100) }; - 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 mut timer = Timer::new(board.TIMER0); + let mut display = Display::new(board.display_pins); -#[embassy_executor::task] -async fn display_data(mut display: LedMatrix) { - let mut display_matrix: FourQuadrantMatrix<5, 5, bool> = + 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 { - 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; + 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); + } + // if channel_button_a.is_event_triggered() { + // //toggles the bool. + // tilt_correction_enabled ^= true; + // channel_button_a.reset_events() + // } + + current_display.reset_matrix(); + + 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 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 { +/// 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 { NedMeasurement { - x: -y as f32, - y: x as f32, - z: -z as f32, + x: -measurement.y as f32, + y: measurement.x as f32, + z: -measurement.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 deleted file mode 100644 index 2f05654..0000000 --- a/independent_logic/.cargo/config.toml +++ /dev/null @@ -1,2 +0,0 @@ -[build] -target = "x86_64-unknown-linux-gnu" diff --git a/independent_logic/Cargo.toml b/independent_logic/Cargo.toml index 27183e3..e956257 100644 --- a/independent_logic/Cargo.toml +++ b/independent_logic/Cargo.toml @@ -1,10 +1,9 @@ [package] name = "independent_logic" version = "0.1.0" -edition = "2024" +edition = "2021" # 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 f4b2ce0..952291c 100644 --- a/independent_logic/src/heading_drawing.rs +++ b/independent_logic/src/heading_drawing.rs @@ -17,8 +17,7 @@ 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 }, bool>, + matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>, ) { - matrix.reset_matrix(); - draw_line::(&heading_to_line(Heading(-heading.0), X.min(Y)), matrix); + draw_line::(&heading_to_line(heading, X.min(Y)), matrix); } diff --git a/independent_logic/src/line_drawing.rs b/independent_logic/src/line_drawing.rs index 25966d0..d34c765 100644 --- a/independent_logic/src/line_drawing.rs +++ b/independent_logic/src/line_drawing.rs @@ -5,10 +5,8 @@ use core::{ #[cfg(test)] use std::dbg; -use defmt::Format; - /// a signed point in 2d space -#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct Point { pub x: isize, pub y: isize, @@ -27,7 +25,7 @@ impl Point { } /// an unsigned point in 2d space -#[derive(Debug, Format, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct UPoint { pub x: usize, pub y: usize, @@ -47,7 +45,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, Format, Clone, Copy, PartialEq, Eq)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] pub struct FourQuadrantMatrix { matrix: [[T; X]; Y], max_point: Point, @@ -141,12 +139,6 @@ 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); @@ -156,7 +148,7 @@ pub struct Line(pub Point, pub Point); /// extend past its edges. pub fn draw_line( line: &Line, - matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>, + matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>, ) { let mut line = *line; #[cfg(test)] @@ -209,7 +201,7 @@ pub fn draw_line( dbg!(draw_point); if matrix.is_in_bounds(&draw_point) { - matrix[draw_point] = true; + matrix[draw_point] = 1; prev_out_of_bounds = false; } else { if !prev_out_of_bounds { @@ -257,195 +249,195 @@ mod tests { #[test] fn four_quadrant_matrix() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = FourQuadrantMatrix::new(UPoint { x: 2, y: 2 }); - canvas[Point { x: 0, y: 0 }] = true; + canvas[Point { x: 0, y: 0 }] = 1; assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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 }] = true; + canvas[Point { x: -2, y: 1 }] = 1; assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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] + [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, bool> = + 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!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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, bool> = + 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!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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_both_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = 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<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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_first_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = 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<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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_second_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = 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<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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 vertical_signed_both_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = 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<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn vertical_signed_first_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = 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<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn vertical_signed_second_oob_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + let mut canvas: FourQuadrantMatrix<5, 5, u8> = 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<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ); } #[test] fn cross_signed_line() { - let mut canvas: FourQuadrantMatrix<5, 5, bool> = + 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 }), @@ -456,13 +448,13 @@ mod tests { &mut canvas, ); assert_eq!( - as Into<[[bool; 5]; 5]>>::into(canvas), + as Into<[[u8; 5]; 5]>>::into(canvas), [ - [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], + [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], ] ) } diff --git a/independent_logic/src/tilt_compensation.rs b/independent_logic/src/tilt_compensation.rs index d59648b..f0dc8d3 100644 --- a/independent_logic/src/tilt_compensation.rs +++ b/independent_logic/src/tilt_compensation.rs @@ -1,13 +1,12 @@ -use defmt::Format; use libm::{atan2f, atanf, cosf, sinf}; -#[derive(Debug, Format)] +#[derive(Debug)] pub struct Attitude { pub pitch: f32, pub roll: f32, } -#[derive(Debug, Format)] +#[derive(Debug)] pub struct NedMeasurement { pub x: f32, pub y: f32, @@ -15,7 +14,6 @@ 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 {