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
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165

use crate::common::*;
use cgmath::{Array, ElementWise, Vector3};
use std::fmt;

pub const IMU_SAMPLE_DURATION: f64 = 0.005;
pub const IMU_SAMPLES_PER_SECOND: u32 = 200;

#[repr(u8)]
#[derive(Copy, Clone, Debug, FromPrimitive, ToPrimitive, PartialEq, Eq)]
pub enum IMUMode {
    Disabled = 0,
    GyroAccel = 1,
    _Unknown0x02 = 2,
    MaybeRingcon = 3,
}

#[repr(packed)]
#[derive(Copy, Clone)]
pub struct Frame {
    raw_accel: [I16LE; 3],
    raw_gyro: [I16LE; 3],
}

impl Frame {
    pub fn raw_ringcon(&self) -> u16 {
        let raw_self = unsafe {
            std::slice::from_raw_parts(self as *const _ as *const u8, std::mem::size_of_val(self))
        };
        u16::from_le_bytes([raw_self[2], raw_self[3]])
    }
    pub fn raw_accel(&self) -> Vector3<f64> {
        vector_from_raw(self.raw_accel)
    }

    pub fn raw_gyro(&self) -> Vector3<f64> {
        vector_from_raw(self.raw_gyro)
    }

    /// Calculation from <https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/imu_sensor_notes.md#accelerometer---acceleration-in-g>
    pub fn accel_g(&self, offset: Vector3<f64>, _sens: AccSens) -> Vector3<f64> {
        // TODO: handle sens
        (self.raw_accel() * 4.).div_element_wise(Vector3::from_value(16383.) - offset)
    }

    /// The rotation described in this frame.
    /// <https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/imu_sensor_notes.md#gyroscope-calibrated---rotation-in-degreess---dps>
    pub fn rotation_dps(&self, offset: Vector3<f64>, sens: GyroSens) -> Vector3<f64> {
        (self.raw_gyro() - offset) * sens.factor()
    }
}

impl fmt::Debug for Frame {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        f.debug_struct("imu::Frame")
            .field("accel", &self.raw_accel())
            .field("gyro", &self.raw_gyro())
            .finish()
    }
}

#[repr(packed)]
#[derive(Copy, Clone, Default, Debug)]
pub struct Sensitivity {
    pub gyro_sens: RawId<GyroSens>,
    pub acc_sens: RawId<AccSens>,
    pub gyro_perf_rate: RawId<GyroPerfRate>,
    pub acc_anti_aliasing: RawId<AccAntiAliasing>,
}

/// Sensitivity range of the gyroscope.
///
/// If using DPS2000 for example, the gyroscope can measure values of
/// up to +-2000 degree per second for a total range of 4000 DPS over
/// the 16 bit raw value.
#[repr(u8)]
#[derive(Copy, Clone, Debug, FromPrimitive, ToPrimitive)]
pub enum GyroSens {
    DPS250 = 0,
    DPS500 = 1,
    DPS1000 = 2,
    DPS2000 = 3,
}

impl GyroSens {
    pub fn range_dps(self) -> u16 {
        match self {
            GyroSens::DPS250 => 500,
            GyroSens::DPS500 => 1000,
            GyroSens::DPS1000 => 2000,
            GyroSens::DPS2000 => 4000,
        }
    }

    /// factor from raw unit to dps
    pub fn factor(self) -> f64 {
        self.range_dps() as f64 * 1.147 / u16::MAX as f64
    }
}

impl Default for GyroSens {
    fn default() -> Self {
        GyroSens::DPS2000
    }
}

/// Sensitivity range of the accelerometer.
///
/// If using G4 for example, the accelerometer can measure values of
/// up to +-4G for a total range of 8G over the 16 bit raw value.
#[repr(u8)]
#[derive(Copy, Clone, Debug, FromPrimitive, ToPrimitive)]
pub enum AccSens {
    G8 = 0,
    G4 = 1,
    G2 = 2,
    G16 = 3,
}

impl AccSens {
    pub fn range_g(self) -> u16 {
        match self {
            AccSens::G8 => 16,
            AccSens::G4 => 8,
            AccSens::G2 => 4,
            AccSens::G16 => 32,
        }
    }
}

impl Default for AccSens {
    fn default() -> Self {
        AccSens::G8
    }
}

#[repr(u8)]
#[derive(Copy, Clone, Debug, FromPrimitive, ToPrimitive)]
pub enum GyroPerfRate {
    Hz833 = 0,
    Hz208 = 1,
}

impl Default for GyroPerfRate {
    fn default() -> Self {
        GyroPerfRate::Hz208
    }
}

/// Anti-aliasing setting of the accelerometer.
///
/// Accelerations frequencies above the value are ignored using a low-pass filter.
///
/// See <https://blog.endaq.com/filter-selection-for-shock-and-vibration-applications>.
#[repr(u8)]
#[derive(Copy, Clone, Debug, FromPrimitive, ToPrimitive)]
pub enum AccAntiAliasing {
    Hz200 = 0,
    Hz100 = 1,
}

impl Default for AccAntiAliasing {
    fn default() -> Self {
        AccAntiAliasing::Hz100
    }
}