Compare commits
5 commits
26a1da2cb9
...
24c8773820
| Author | SHA1 | Date | |
|---|---|---|---|
| 24c8773820 | |||
| 3471b71dbc | |||
| 45578958a6 | |||
| 6f6660fd6a | |||
| b894d424aa |
14 changed files with 247 additions and 535 deletions
4
.cargo/config.toml
Normal file
4
.cargo/config.toml
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
[target.'cfg(all(target_arch = "arm", target_os = "none"))']
|
||||
|
||||
[build]
|
||||
target = "thumbv7em-none-eabihf"
|
||||
3
.vscode/settings.json
vendored
Normal file
3
.vscode/settings.json
vendored
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
{
|
||||
"rust-analyzer.check.allTargets": false
|
||||
}
|
||||
|
|
@ -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" }
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -1,30 +1,23 @@
|
|||
[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"
|
||||
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"
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
}
|
||||
}
|
||||
|
|
@ -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<CriticalSectionRawMutex, Heading> = 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) };
|
||||
// Bind interrupt to the TWI/SPI peripheral.
|
||||
bind_interrupts!(
|
||||
struct InterruptRequests {
|
||||
TWISPI0 => InterruptHandler<TWISPI0>;
|
||||
}
|
||||
);
|
||||
|
||||
let mut timer = Timer::new(board.TIMER0);
|
||||
let mut display = Display::new(board.display_pins);
|
||||
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 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> =
|
||||
#[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 });
|
||||
#[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);
|
||||
}
|
||||
// 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)
|
||||
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<I2cInterface<Twim<'static, TWISPI0>>, 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<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/config.toml
Normal file
2
independent_logic/.cargo/config.toml
Normal file
|
|
@ -0,0 +1,2 @@
|
|||
[build]
|
||||
target = "x86_64-unknown-linux-gnu"
|
||||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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<const X: usize, const Y: usize>(
|
||||
heading: Heading,
|
||||
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
|
||||
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, bool>,
|
||||
) {
|
||||
draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), matrix);
|
||||
matrix.reset_matrix();
|
||||
draw_line::<X, Y>(&heading_to_line(Heading(-heading.0), X.min(Y)), matrix);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<const X: usize, const Y: usize, T> {
|
||||
matrix: [[T; X]; Y],
|
||||
max_point: Point,
|
||||
|
|
@ -139,6 +141,12 @@ impl<T, const X: usize, const Y: usize> From<FourQuadrantMatrix<{ X }, { Y }, T>
|
|||
}
|
||||
}
|
||||
|
||||
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<const X: usize, const Y: usize>(
|
||||
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<const X: usize, const Y: usize>(
|
|||
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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
<FourQuadrantMatrix<5, 5, bool> 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],
|
||||
]
|
||||
)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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