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

WIP, learning embassy.

Browse source
This commit is contained in:
Gabe Venberg 2025-06-24 23:27:07 +02:00
parent 26a1da2cb9
commit b894d424aa
6 changed files with 73 additions and 456 deletions
Download patch file Download diff file Expand all files Collapse all files

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"

36
hardware_main/Cargo.toml
View file

@ -1,30 +1,20 @@
[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"
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"]

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

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,
}
}

211
hardware_main/src/main.rs
View file

@ -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<CriticalSectionRawMutex, Button> = 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<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.toml
View file

@ -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