oxstack_drivers/
axes.rs

1//! Mapping a sensor's axes onto a board's.
2//!
3//! An IMU reports in its own frame, fixed by how the package is soldered.
4//! [`AxisMap`] turns that into the frame a program wants (say, +Z out of the
5//! screen), and [`AxisMap::from_gravity`] works the mapping out from three
6//! readings taken with the board held in known poses, so it never has to be
7//! guessed from a layout drawing.
8
9/// One signed axis of the sensor's own frame.
10#[derive(Clone, Copy, Debug, PartialEq, Eq)]
11#[cfg_attr(feature = "defmt", derive(defmt::Format))]
12pub enum Axis {
13    /// The sensor's +X.
14    PosX,
15    /// The sensor's −X.
16    NegX,
17    /// The sensor's +Y.
18    PosY,
19    /// The sensor's −Y.
20    NegY,
21    /// The sensor's +Z.
22    PosZ,
23    /// The sensor's −Z.
24    NegZ,
25}
26
27impl Axis {
28    /// Index into a `[x, y, z]` reading.
29    pub const fn index(self) -> usize {
30        match self {
31            Axis::PosX | Axis::NegX => 0,
32            Axis::PosY | Axis::NegY => 1,
33            Axis::PosZ | Axis::NegZ => 2,
34        }
35    }
36
37    /// Whether the axis points the opposite way from the sensor's.
38    pub const fn is_negative(self) -> bool {
39        matches!(self, Axis::NegX | Axis::NegY | Axis::NegZ)
40    }
41
42    /// The same axis, pointing the other way.
43    pub const fn flipped(self) -> Self {
44        match self {
45            Axis::PosX => Axis::NegX,
46            Axis::NegX => Axis::PosX,
47            Axis::PosY => Axis::NegY,
48            Axis::NegY => Axis::PosY,
49            Axis::PosZ => Axis::NegZ,
50            Axis::NegZ => Axis::PosZ,
51        }
52    }
53
54    /// The component of `v` along this axis.
55    pub const fn pick(self, v: [f32; 3]) -> f32 {
56        let c = v[self.index()];
57        if self.is_negative() { -c } else { c }
58    }
59
60    /// The axis along which `v` mostly points, if it clearly points along
61    /// one: that component must be at least `min` in magnitude and at least
62    /// twice the next largest. For a resting accelerometer in g, `min = 0.7`
63    /// accepts up to about 25° of tilt.
64    pub fn dominant(v: [f32; 3], min: f32) -> Option<Self> {
65        let mag = [v[0].abs(), v[1].abs(), v[2].abs()];
66        let (i, &largest) = mag.iter().enumerate().max_by(|a, b| a.1.total_cmp(b.1))?;
67        let second = mag
68            .iter()
69            .enumerate()
70            .filter(|&(j, _)| j != i)
71            .map(|(_, m)| *m)
72            .fold(0.0f32, f32::max);
73        if largest < min || largest < 2.0 * second {
74            return None;
75        }
76        let positive = [Axis::PosX, Axis::PosY, Axis::PosZ][i];
77        Some(if v[i] < 0.0 {
78            positive.flipped()
79        } else {
80            positive
81        })
82    }
83
84    /// Rust source for this value, e.g. `Axis::NegY`.
85    pub const fn source(self) -> &'static str {
86        match self {
87            Axis::PosX => "Axis::PosX",
88            Axis::NegX => "Axis::NegX",
89            Axis::PosY => "Axis::PosY",
90            Axis::NegY => "Axis::NegY",
91            Axis::PosZ => "Axis::PosZ",
92            Axis::NegZ => "Axis::NegZ",
93        }
94    }
95}
96
97/// Which sensor axis each board axis reads.
98///
99/// ```
100/// use oxstack_drivers::axes::{Axis, AxisMap};
101///
102/// // Sensor rotated 90° clockwise on the board: board +X is sensor −Y.
103/// let map = AxisMap { x: Axis::NegY, y: Axis::PosX, z: Axis::PosZ };
104/// assert_eq!(map.apply([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0]);
105/// assert!(map.is_right_handed());
106/// ```
107#[derive(Clone, Copy, Debug, PartialEq, Eq)]
108#[cfg_attr(feature = "defmt", derive(defmt::Format))]
109pub struct AxisMap {
110    /// The sensor axis the board's +X reads.
111    pub x: Axis,
112    /// The sensor axis the board's +Y reads.
113    pub y: Axis,
114    /// The sensor axis the board's +Z reads.
115    pub z: Axis,
116}
117
118/// Why [`AxisMap::from_gravity`] could not decide.
119#[derive(Clone, Copy, Debug, PartialEq, Eq)]
120#[cfg_attr(feature = "defmt", derive(defmt::Format))]
121pub enum GravityError {
122    /// This pose's reading does not point clearly along one sensor axis
123    /// (0 = `x_up`, 1 = `y_up`, 2 = `z_up`). Hold the board straighter.
124    Unclear(u8),
125    /// Two poses picked the same sensor axis: the board was held the same
126    /// way twice.
127    Repeated,
128}
129
130impl AxisMap {
131    /// Board frame equals sensor frame.
132    pub const IDENTITY: Self = Self {
133        x: Axis::PosX,
134        y: Axis::PosY,
135        z: Axis::PosZ,
136    };
137
138    /// Translates a sensor-frame vector into the board frame.
139    pub const fn apply(&self, v: [f32; 3]) -> [f32; 3] {
140        [self.x.pick(v), self.y.pick(v), self.z.pick(v)]
141    }
142
143    /// Whether every sensor axis is used exactly once.
144    pub const fn is_permutation(&self) -> bool {
145        let (x, y, z) = (self.x.index(), self.y.index(), self.z.index());
146        x != y && y != z && x != z
147    }
148
149    /// Whether the map is a rotation (determinant +1) rather than a
150    /// reflection. A sensor soldered flat on a board always gives a
151    /// rotation; a reflection means one pose was misread.
152    pub const fn is_right_handed(&self) -> bool {
153        if !self.is_permutation() {
154            return false;
155        }
156        // Parity of the permutation: even for xyz, yzx, zxy.
157        let even = matches!(
158            (self.x.index(), self.y.index(), self.z.index()),
159            (0, 1, 2) | (1, 2, 0) | (2, 0, 1)
160        );
161        let flips =
162            self.x.is_negative() as u8 + self.y.is_negative() as u8 + self.z.is_negative() as u8;
163        even == flips.is_multiple_of(2)
164    }
165
166    /// Works the map out from gravity. Each argument is one resting
167    /// accelerometer reading (in g, sensor frame) with the named **board**
168    /// axis pointing straight up, so that it reads about +1 g:
169    ///
170    /// - `x_up`: board stood on its left edge (+X up),
171    /// - `y_up`: board stood on its bottom edge (+Y up),
172    /// - `z_up`: board lying flat, face up (+Z up).
173    ///
174    /// ```
175    /// use oxstack_drivers::axes::{Axis, AxisMap};
176    ///
177    /// let map = AxisMap::from_gravity(
178    ///     [0.02, -0.98, 0.05],   // +X up: sensor −Y reads gravity
179    ///     [0.99, 0.01, -0.03],   // +Y up: sensor +X
180    ///     [-0.04, 0.0, 1.01],    // +Z up: sensor +Z
181    /// )
182    /// .unwrap();
183    /// assert_eq!(map, AxisMap { x: Axis::NegY, y: Axis::PosX, z: Axis::PosZ });
184    /// ```
185    pub fn from_gravity(
186        x_up: [f32; 3],
187        y_up: [f32; 3],
188        z_up: [f32; 3],
189    ) -> Result<Self, GravityError> {
190        const MIN: f32 = 0.7;
191        let x = Axis::dominant(x_up, MIN).ok_or(GravityError::Unclear(0))?;
192        let y = Axis::dominant(y_up, MIN).ok_or(GravityError::Unclear(1))?;
193        let z = Axis::dominant(z_up, MIN).ok_or(GravityError::Unclear(2))?;
194        let map = Self { x, y, z };
195        if !map.is_permutation() {
196            return Err(GravityError::Repeated);
197        }
198        Ok(map)
199    }
200}
201
202#[cfg(test)]
203mod tests {
204    use super::*;
205
206    #[test]
207    fn identity_is_a_rotation() {
208        assert!(AxisMap::IDENTITY.is_right_handed());
209        assert_eq!(AxisMap::IDENTITY.apply([1.0, 2.0, 3.0]), [1.0, 2.0, 3.0]);
210    }
211
212    #[test]
213    fn all_four_flat_rotations_are_right_handed() {
214        for (x, y) in [
215            (Axis::PosX, Axis::PosY),
216            (Axis::NegY, Axis::PosX),
217            (Axis::NegX, Axis::NegY),
218            (Axis::PosY, Axis::NegX),
219        ] {
220            let m = AxisMap {
221                x,
222                y,
223                z: Axis::PosZ,
224            };
225            assert!(m.is_right_handed(), "{m:?}");
226        }
227        // Mirror: swap X and Y without a sign change.
228        let mirror = AxisMap {
229            x: Axis::PosY,
230            y: Axis::PosX,
231            z: Axis::PosZ,
232        };
233        assert!(!mirror.is_right_handed());
234        // Flipping one axis of a rotation makes a reflection.
235        let flipped = AxisMap {
236            x: Axis::NegX,
237            y: Axis::PosY,
238            z: Axis::PosZ,
239        };
240        assert!(!flipped.is_right_handed());
241    }
242
243    #[test]
244    fn dominant_axis_tolerates_tilt_but_not_ambiguity() {
245        assert_eq!(Axis::dominant([0.0, 0.0, 0.98], 0.7), Some(Axis::PosZ));
246        assert_eq!(Axis::dominant([-0.9, 0.4, 0.1], 0.7), Some(Axis::NegX));
247        assert_eq!(Axis::dominant([0.7, 0.7, 0.0], 0.7), None); // 45°
248        assert_eq!(Axis::dominant([0.3, 0.1, 0.1], 0.7), None); // free fall?
249    }
250
251    #[test]
252    fn gravity_calibration_rejects_repeated_poses() {
253        let flat = [0.0, 0.0, 1.0];
254        assert_eq!(
255            AxisMap::from_gravity(flat, [0.0, 1.0, 0.0], flat),
256            Err(GravityError::Repeated)
257        );
258        assert_eq!(
259            AxisMap::from_gravity([0.6, 0.6, 0.0], [0.0, 1.0, 0.0], flat),
260            Err(GravityError::Unclear(0))
261        );
262    }
263
264    #[test]
265    fn gravity_calibration_round_trips() {
266        let truth = AxisMap {
267            x: Axis::NegX,
268            y: Axis::NegY,
269            z: Axis::PosZ,
270        };
271        // Sensor reading when board axis `i` points up: the sensor vector
272        // whose board-frame image is e_i. Build it by inverting the map.
273        let sensor_for = |board_axis: Axis| {
274            let mut v = [0.0f32; 3];
275            v[board_axis.index()] = if board_axis.is_negative() { -1.0 } else { 1.0 };
276            v
277        };
278        let map = AxisMap::from_gravity(
279            sensor_for(truth.x),
280            sensor_for(truth.y),
281            sensor_for(truth.z),
282        )
283        .unwrap();
284        assert_eq!(map, truth);
285    }
286}
oxstack_drivers/axes.rs

oxstack_drivers/
axes.rs
