59 lines
1.9 KiB
Rust
59 lines
1.9 KiB
Rust
use libm::{atan2f, atanf, cosf, sinf};
|
|
|
|
#[derive(Debug)]
|
|
pub struct Attitude {
|
|
pub pitch: f32,
|
|
pub roll: f32,
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub struct NedMeasurement {
|
|
pub x: f32,
|
|
pub y: f32,
|
|
pub z: f32,
|
|
}
|
|
|
|
///theta=0 at north, pi/-pi at south, pi/2 at east, and -pi/2 at west
|
|
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 }
|
|
}
|
|
|
|
pub fn calc_tilt_calibrated_measurement(
|
|
mag_measurement: NedMeasurement,
|
|
attitde: &Attitude,
|
|
) -> NedMeasurement {
|
|
//based off of: https://www.nxp.com/docs/en/application-note/AN4248.pdf
|
|
// ø=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: corrected_mag_z,
|
|
}
|
|
}
|
|
|
|
//0 is the top sector and positive is clockwise, negative is counterclockwise.
|
|
pub fn heading_from_measurement(measurement: &NedMeasurement) -> Heading {
|
|
Heading(atan2f(-measurement.y, measurement.x))
|
|
}
|
|
|
|
//I have no freaking clue how to test this...
|