1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95

//! Device position tracker which uses a sliding widow of acceleration data
//! samples to help filter the signal from the noise.

use crate::{
    accelerometer::Accelerometer,
    orientation::Orientation,
    vector::{Component, Vector, VectorExt},
};
use core::marker::PhantomData;
use micromath::generic_array::typenum::U3;

// Spuriously triggers unused import warnings in cases std is linked
#[allow(unused_imports)]
use micromath::F32Ext;

/// Device position tracker which which filters noisy accelerometer data
/// using statistical methods
pub struct Tracker<A, V>
where
    A: Accelerometer<V>,
    V: Vector<Axes = U3> + VectorExt,
{
    /// The underlying accelerometer device
    accelerometer: A,

    /// Threshold at which acceleration due to gravity is registered
    threshold: f32,

    /// Last orientation type read from the accelerometer
    last_orientation: Orientation,

    /// Vector type
    vector: PhantomData<V>,
}

impl<A, V, C> Tracker<A, V>
where
    A: Accelerometer<V>,
    V: Vector<Axes = U3, Component = C> + VectorExt,
    C: Component + Into<f32>,
{
    /// Create a new orientation tracker
    pub fn new(accelerometer: A, threshold: V::Component) -> Self {
        Self {
            accelerometer,
            threshold: threshold.into(),
            last_orientation: Orientation::Unknown,
            vector: PhantomData,
        }
    }

    /// Get an orientation reading
    pub fn orientation(&mut self) -> Result<Orientation, A::Error> {
        let accel = self.accelerometer.acceleration()?.to_array();
        let x: f32 = accel[0].into();
        let y: f32 = accel[1].into();
        let z: f32 = accel[2].into();

        let result = if x.abs() > self.threshold {
            // Landscape
            if x >= 0.0 {
                Orientation::LandscapeUp
            } else {
                Orientation::LandscapeDown
            }
        } else if y.abs() > self.threshold {
            // Portrait
            if y >= 0.0 {
                Orientation::PortraitUp
            } else {
                Orientation::PortraitDown
            }
        } else if z.abs() > self.threshold {
            // Flat
            if z >= 0.0 {
                Orientation::FaceUp
            } else {
                Orientation::FaceDown
            }
        } else {
            Orientation::Unknown
        };

        if result != Orientation::Unknown {
            self.last_orientation = result;
        }

        Ok(result)
    }

    /// Get the last known orientation reading for the device
    pub fn last_orientation(&self) -> Orientation {
        self.last_orientation
    }
}