Fixed some math bugs in the compass calcs, needle is now more accurate.
This commit is contained in:
parent
a77b3845e0
commit
26a1da2cb9
4 changed files with 38 additions and 22 deletions
|
@ -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
|
||||
|
|
|
@ -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<const X: usize, const Y: usize>(
|
||||
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<const X: usize, const Y: usize>(
|
||||
heading: Heading,
|
||||
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
|
||||
) {
|
||||
draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), matrix);
|
||||
|
|
|
@ -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))
|
||||
}
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue