From 26a1da2cb9afa5fc50c2951d6e049e05c0e825a2 Mon Sep 17 00:00:00 2001 From: Gabe Venberg Date: Mon, 23 Jun 2025 18:00:26 +0200 Subject: [PATCH] Fixed some math bugs in the compass calcs, needle is now more accurate. --- hardware_main/.cargo/{config => config.toml} | 0 hardware_main/src/main.rs | 29 ++++++++++++-------- independent_logic/src/heading_drawing.rs | 13 +++++---- independent_logic/src/tilt_compensation.rs | 18 ++++++++---- 4 files changed, 38 insertions(+), 22 deletions(-) rename hardware_main/.cargo/{config => config.toml} (100%) diff --git a/hardware_main/.cargo/config b/hardware_main/.cargo/config.toml similarity index 100% rename from hardware_main/.cargo/config rename to hardware_main/.cargo/config.toml diff --git a/hardware_main/src/main.rs b/hardware_main/src/main.rs index 685e480..dc3053a 100644 --- a/hardware_main/src/main.rs +++ b/hardware_main/src/main.rs @@ -34,7 +34,7 @@ use microbit::{hal::twim, pac::twim0::frequency::FREQUENCY_A}; use crate::calibration::calc_calibration; use independent_logic::{ - heading_drawing::draw_heading, + heading_drawing::draw_constant_heading, tilt_compensation::{ calc_attitude, calc_tilt_calibrated_measurement, heading_from_measurement, Heading, NedMeasurement, @@ -77,7 +77,6 @@ fn main() -> ! { sensor.set_accel_odr(AccelOutputDataRate::Hz10).unwrap(); let mut sensor = sensor.into_mag_continuous().ok().unwrap(); - //TODO: re-callibrate with button. #[cfg(feature = "calibration")] let mut calibration = calc_calibration(&mut sensor, &mut display, &mut timer); #[cfg(not(feature = "calibration"))] @@ -98,28 +97,28 @@ fn main() -> ! { #[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() - } + // 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_heading::<5, 5>(heading.0, &mut current_display); + 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 +/// 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, + y: measurement.x as f32, z: -measurement.z as f32, } } @@ -145,7 +144,7 @@ fn calc_heading( 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); + let heading = heading_from_measurement(&ned_mag_data); #[cfg(all(not(feature = "calibration"), debug_assertions))] rprintln!( @@ -154,9 +153,15 @@ fn calc_heading( 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!( - "x: {:<+16}, y: {:<+16}, z: {:<+16}", + "acell: x: {:<+16}, y: {:<+16}, z: {:<+16}", ned_acel_data.x, ned_acel_data.y, ned_acel_data.z diff --git a/independent_logic/src/heading_drawing.rs b/independent_logic/src/heading_drawing.rs index 731b038..952291c 100644 --- a/independent_logic/src/heading_drawing.rs +++ b/independent_logic/src/heading_drawing.rs @@ -1,19 +1,22 @@ use libm::{cosf, roundf, sinf}; use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point}; +use crate::tilt_compensation::Heading; -fn heading_to_line(heading: f32, square_size: usize) -> Line { +fn heading_to_line(heading: Heading, square_size: usize) -> Line { Line( Point { x: 0, y: 0 }, Point { - x: roundf((square_size as f32) * sinf(heading)) as isize, - y: roundf((square_size as f32) * cosf(heading)) as isize, + x: roundf((square_size as f32) * sinf(heading.0)) as isize, + y: roundf((square_size as f32) * cosf(heading.0)) as isize, }, ) } -pub fn draw_heading( - heading: f32, +// given the compass heading that the board '0' is facing, +// draws a line always pointing towards heading 0 +pub fn draw_constant_heading( + heading: Heading, matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>, ) { draw_line::(&heading_to_line(heading, X.min(Y)), matrix); diff --git a/independent_logic/src/tilt_compensation.rs b/independent_logic/src/tilt_compensation.rs index fae117b..f0dc8d3 100644 --- a/independent_logic/src/tilt_compensation.rs +++ b/independent_logic/src/tilt_compensation.rs @@ -18,6 +18,7 @@ pub struct Heading(pub f32); pub fn calc_attitude(measurement: &NedMeasurement) -> Attitude { //based off of: https://www.nxp.com/docs/en/application-note/AN4248.pdf + //Gp{xyz} is the acellerometer measurements. let roll = atan2f(measurement.y, measurement.z); let pitch = atanf(-measurement.x / (measurement.y * sinf(roll) + measurement.z * cosf(roll))); Attitude { pitch, roll } @@ -28,23 +29,30 @@ pub fn calc_tilt_calibrated_measurement( attitde: &Attitude, ) -> NedMeasurement { //based off of: https://www.nxp.com/docs/en/application-note/AN4248.pdf - - let corrected_mag_y = - mag_measurement.z * sinf(attitde.roll) - mag_measurement.y * cosf(attitde.roll); + // ø=roll, + // θ=pitch, + // Bp{xyz} is magnometer readings, + // V{xyz} is the magnometers constant hard-iron compnent, (currently taken care by micro:bits + // library) let corrected_mag_x = mag_measurement.x * cosf(attitde.pitch) + mag_measurement.y * sinf(attitde.pitch) * sinf(attitde.roll) + mag_measurement.z * sinf(attitde.pitch) * cosf(attitde.roll); + let corrected_mag_y = + mag_measurement.y * cosf(attitde.roll) - mag_measurement.z * sinf(attitde.roll); + let corrected_mag_z = -mag_measurement.x * sinf(attitde.pitch) + + mag_measurement.y * cosf(attitde.pitch) * sinf(attitde.roll) + + mag_measurement.z * cosf(attitde.pitch) * cosf(attitde.roll); NedMeasurement { x: corrected_mag_x, y: corrected_mag_y, - z: 0.0, + z: corrected_mag_z, } } //0 is the top sector and positive is clockwise, negative is counterclockwise. -pub fn heading_from_measurement(measurement: NedMeasurement) -> Heading { +pub fn heading_from_measurement(measurement: &NedMeasurement) -> Heading { Heading(atan2f(-measurement.y, measurement.x)) }