got embassy compass outputting a heading number.
This commit is contained in:
parent
6f6660fd6a
commit
45578958a6
8 changed files with 103 additions and 76 deletions
|
|
@ -1,11 +1,4 @@
|
|||
[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"
|
||||
|
|
|
|||
|
|
@ -8,3 +8,11 @@ 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" }
|
||||
|
|
|
|||
|
|
@ -5,16 +5,18 @@ authors = ["Gabriel Venberg"]
|
|||
edition = "2024"
|
||||
|
||||
[dependencies]
|
||||
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"] }
|
||||
|
||||
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"
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,68 +2,87 @@
|
|||
#![no_main]
|
||||
#![no_std]
|
||||
|
||||
use core::cmp::max;
|
||||
use defmt::info;
|
||||
use defmt_rtt as _;
|
||||
use core::f32::consts::PI;
|
||||
use defmt::{debug, info};
|
||||
use embassy_executor::Spawner;
|
||||
use embassy_futures::join::join;
|
||||
use embassy_nrf::{
|
||||
bind_interrupts,
|
||||
gpio::{AnyPin, Input, Pin, Pull},
|
||||
temp::Temp,
|
||||
use embassy_time::Timer;
|
||||
use microbit_bsp::{
|
||||
Microbit,
|
||||
display::{Brightness, Frame},
|
||||
embassy_nrf::{bind_interrupts, peripherals::TWISPI0, twim::InterruptHandler},
|
||||
lsm303agr,
|
||||
motion::new_lsm303agr,
|
||||
};
|
||||
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
|
||||
use embassy_time::{Duration, Timer, WithTimeout};
|
||||
use panic_probe as _;
|
||||
use {defmt_rtt as _, panic_probe as _};
|
||||
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
enum Button {
|
||||
A,
|
||||
B,
|
||||
}
|
||||
|
||||
static SIGNAL: Signal<CriticalSectionRawMutex, Button> = Signal::new();
|
||||
|
||||
bind_interrupts!(struct Irqs {
|
||||
TEMP => embassy_nrf::temp::InterruptHandler;
|
||||
});
|
||||
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,
|
||||
},
|
||||
};
|
||||
|
||||
#[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;
|
||||
}
|
||||
async fn main(_s: Spawner) {
|
||||
let board = Microbit::default();
|
||||
defmt::info!("Application started!");
|
||||
|
||||
async fn button(pin: AnyPin, id: &'static str, b: Button) {
|
||||
let mut button = Input::new(pin, Pull::None);
|
||||
loop {
|
||||
button.wait_for_low().await;
|
||||
info!("Button {} Pressed!", id);
|
||||
SIGNAL.signal(b);
|
||||
Timer::after_millis(200).await;
|
||||
button.wait_for_high().await;
|
||||
}
|
||||
}
|
||||
let mut display = board.display;
|
||||
display.set_brightness(Brightness::MAX);
|
||||
|
||||
#[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 Ok(v) = SIGNAL.wait().with_timeout(delay).await {
|
||||
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);
|
||||
// Bind interrupt to the TWI/SPI peripheral.
|
||||
bind_interrupts!(
|
||||
struct InterruptRequests {
|
||||
TWISPI0 => InterruptHandler<TWISPI0>;
|
||||
}
|
||||
);
|
||||
|
||||
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();
|
||||
|
||||
Timer::after_secs(2).await;
|
||||
|
||||
loop {
|
||||
let (x, y, z) = sensor.magnetic_field().await.unwrap().xyz_nt();
|
||||
let mag_measurement = to_ned(x, y, z);
|
||||
let (x, y, z) = sensor.acceleration().await.unwrap().xyz_mg();
|
||||
let accel_measurement = to_ned(x, y, z);
|
||||
debug!("Mag: {}, Accel: {}", mag_measurement, accel_measurement);
|
||||
Timer::after_millis(250).await;
|
||||
let attitude = calc_attitude(&accel_measurement);
|
||||
let mag_measurement = calc_tilt_calibrated_measurement(mag_measurement, &attitude);
|
||||
let heading = heading_from_measurement(&mag_measurement);
|
||||
debug!("Attitude: {}, Heading: {}", attitude, heading.0*(180.0/PI));
|
||||
}
|
||||
}
|
||||
|
||||
pub fn to_ned(x: i32, y: i32, z: i32) -> NedMeasurement {
|
||||
NedMeasurement {
|
||||
x: -y as f32,
|
||||
y: x as f32,
|
||||
z: -z as f32,
|
||||
}
|
||||
}
|
||||
|
|
|
|||
2
independent_logic/.cargo/config.toml
Normal file
2
independent_logic/.cargo/config.toml
Normal file
|
|
@ -0,0 +1,2 @@
|
|||
[build]
|
||||
target = "x86_64-unknown-linux-gnu"
|
||||
|
|
@ -6,4 +6,5 @@ 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"
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue