pamoja_kit/
imu.rs

1//! Tilt from a three-axis accelerometer.
2
3use core::f64::consts::PI;
4
5use libm::{atan2, sqrt};
6
7fn to_degrees(radians: f64) -> f64 {
8    radians * (180.0 / PI)
9}
10
11/// Roll and pitch angles, in degrees.
12#[derive(Clone, Copy, Debug, PartialEq)]
13pub struct Tilt {
14    /// Rotation about the forward (x) axis, in degrees, in `[-180.0, 180.0]`.
15    pub roll: f64,
16    /// Rotation about the right (y) axis, in degrees, in `[-90.0, 90.0]`.
17    pub pitch: f64,
18}
19
20/// Computes roll and pitch from a three-axis accelerometer reading.
21///
22/// At rest, gravity tells a three-axis accelerometer which way is down, which fixes the
23/// board's tilt. This uses the standard formula (Freescale AN3461): roll is `atan2(ay, az)`
24/// and pitch is `atan2(-ax, sqrt(ay^2 + az^2))`, with `atan2` placing each angle in the
25/// correct quadrant. The reading's units do not matter (raw counts or g), because only the
26/// ratios between axes set the angle and any common scale cancels. It holds while the board
27/// is still or moving gently, since it assumes the only acceleration is gravity. Yaw
28/// (heading) cannot be found from an accelerometer alone; that needs a magnetometer.
29///
30/// # Arguments
31///
32/// * `ax` - acceleration along the x (forward) axis.
33/// * `ay` - acceleration along the y (right) axis.
34/// * `az` - acceleration along the z (up) axis.
35///
36/// # Returns
37///
38/// The [`Tilt`] in degrees.
39///
40/// # Examples
41///
42/// ```
43/// use pamoja_kit::imu::tilt_from_accel;
44///
45/// // Board level, 1 g straight down on z: no tilt.
46/// let level = tilt_from_accel(0.0, 0.0, 1.0);
47/// assert!(level.roll.abs() < 1e-6 && level.pitch.abs() < 1e-6);
48///
49/// // Tipped fully onto its y axis: 90 degrees of roll.
50/// let rolled = tilt_from_accel(0.0, 1.0, 0.0);
51/// assert!((rolled.roll - 90.0).abs() < 1e-6);
52/// ```
53pub fn tilt_from_accel(ax: f64, ay: f64, az: f64) -> Tilt {
54    let roll = to_degrees(atan2(ay, az));
55    let pitch = to_degrees(atan2(-ax, sqrt(ay * ay + az * az)));
56    Tilt { roll, pitch }
57}
58
59#[cfg(test)]
60mod tests {
61    use super::*;
62
63    #[test]
64    fn a_level_board_has_no_tilt() {
65        let tilt = tilt_from_accel(0.0, 0.0, 1.0);
66        assert!(tilt.roll.abs() < 1e-9);
67        assert!(tilt.pitch.abs() < 1e-9);
68    }
69
70    #[test]
71    fn rolled_onto_the_y_axis_is_ninety_degrees_of_roll() {
72        let tilt = tilt_from_accel(0.0, 1.0, 0.0);
73        assert!((tilt.roll - 90.0).abs() < 1e-9);
74        assert!(tilt.pitch.abs() < 1e-9);
75    }
76
77    #[test]
78    fn pitched_onto_the_x_axis_is_minus_ninety_pitch() {
79        let tilt = tilt_from_accel(1.0, 0.0, 0.0);
80        assert!((tilt.pitch + 90.0).abs() < 1e-9);
81    }
82
83    #[test]
84    fn equal_y_and_z_is_forty_five_degrees_of_roll() {
85        let tilt = tilt_from_accel(0.0, 1.0, 1.0);
86        assert!((tilt.roll - 45.0).abs() < 1e-9);
87    }
88
89    #[test]
90    fn the_scale_of_the_reading_does_not_change_the_angle() {
91        let g = tilt_from_accel(0.0, 1.0, 1.0);
92        let counts = tilt_from_accel(0.0, 1000.0, 1000.0);
93        assert!((g.roll - counts.roll).abs() < 1e-9);
94    }
95}
pamoja_kit/imu.rs

pamoja_kit/
imu.rs
