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..5141e80 100644 --- a/hardware_main/Cargo.toml +++ b/hardware_main/Cargo.toml @@ -1,30 +1,20 @@ [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" +cortex-m = { version = "0.7.7", features = ["inline-asm", "critical-section-single-core"] } +cortex-m-rt = "0.7.5" +defmt = "1.0.1" +defmt-rtt = "1.0.0" +embassy-executor = { version = "0.7.0", features = ["arch-cortex-m", "executor-thread", "defmt"] } +embassy-nrf = { version = "0.3.1", features = ["defmt", "nrf52833", "gpiote", "time-driver-rtc1", "time"] } +embassy-time = { version = "0.4.0", features = ["defmt", "defmt-timestamp-uptime"] } +lsm303agr = { version = "1.1.0", features = ["async"] } +panic-probe = { version = "1.0.0", features = ["print-defmt"] } independent_logic = {path="../independent_logic"} +embassy-futures = { version = "0.1.1", features = ["defmt"] } +embassy-sync = { version = "0.7.0", features = ["defmt"] } -[features] -v2 = ["microbit-v2"] -v1 = ["microbit"] -calibration=[] -default = ["v2"] 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/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..66882d5 100644 --- a/hardware_main/src/main.rs +++ b/hardware_main/src/main.rs @@ -2,169 +2,68 @@ #![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 independent_logic::{ - heading_drawing::draw_constant_heading, - tilt_compensation::{ - calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, Heading, - NedMeasurement, - }, +use core::cmp::max; +use defmt::info; +use defmt_rtt as _; +use embassy_executor::Spawner; +use embassy_futures::join::join; +use embassy_nrf::{ + bind_interrupts, + gpio::{AnyPin, Input, Pin, Pull}, + temp::Temp, }; +use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal}; +use embassy_time::{Duration, Timer, WithTimeout}; +use panic_probe as _; -const DELAY: u32 = 100; +#[derive(Debug, Clone, Copy)] +enum Button { + A, + B, +} -#[entry] -fn main() -> ! { - #[cfg(debug_assertions)] - rtt_init_print!(); - let board = microbit::Board::take().unwrap(); +static SIGNAL: Signal = Signal::new(); - #[cfg(feature = "v1")] - let i2c = { twi::Twi::new(board.TWI0, board.i2c.into(), FREQUENCY_A::K100) }; +bind_interrupts!(struct Irqs { + TEMP => embassy_nrf::temp::InterruptHandler; +}); - #[cfg(feature = "v2")] - let i2c = { twim::Twim::new(board.TWIM0, board.i2c_internal.into(), FREQUENCY_A::K100) }; +#[embassy_executor::main] +async fn main(spawner: Spawner) { + info!("Starting"); + let p = embassy_nrf::init(Default::default()); + let temp = Temp::new(p.TEMP, Irqs); + spawner.spawn(temp_task(temp)).unwrap(); + let button_a = button(p.P0_14.degrade(), "A", Button::A); + let button_b = button(p.P0_23.degrade(), "B", Button::B); + join(button_a, button_b).await; +} - 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); +async fn button(pin: AnyPin, id: &'static str, b: Button) { + let mut button = Input::new(pin, Pull::None); 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); + button.wait_for_low().await; + info!("Button {} Pressed!", id); + SIGNAL.signal(b); + Timer::after_millis(200).await; + button.wait_for_high().await; + } +} + +#[embassy_executor::task] +async fn temp_task(mut temp: Temp<'static>) { + const INTERVAL_MS: u64 = 500; + let mut delay_ms = INTERVAL_MS; + loop { + let value: u16 = temp.read().await.to_num(); + info!("{} C", value); + let delay = Duration::from_millis(delay_ms); + if let Some(v) = SIGNAL.wait().with_timeout(delay).await.ok() { + delay_ms = match v { + Button::A => max(INTERVAL_MS, delay_ms.saturating_sub(INTERVAL_MS)), + Button::B => delay_ms + INTERVAL_MS, + }; + info!("Delay = {} ms", delay_ms); } - // 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) } } - -/// 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: -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.toml b/independent_logic/Cargo.toml index e956257..2ed93c5 100644 --- a/independent_logic/Cargo.toml +++ b/independent_logic/Cargo.toml @@ -1,7 +1,7 @@ [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