joycon_driver/
imu.rs

1use std::f64::consts::PI;
2
3use nalgebra as na;
4use nom::{combinator::eof, sequence::tuple};
5
6use crate::{
7    error::{JoyConDriverError, JoyConDriverResult},
8    joycon::{JoyConDevice, SUB_COMMAND_READ_HEADER_BYTES, USER_CALIBRATION_DATA_MAGIC},
9    parser::parse_i16_vec3,
10};
11
12const G: f64 = 9.80665;
13
14#[derive(Debug, Clone)]
15pub struct IMUDataRaw {
16    pub accel: na::Vector3<i16>,
17    pub gyro: na::Vector3<i16>,
18}
19
20impl IMUDataRaw {
21    pub fn to_data(&self) -> IMUData {
22        IMUData::from_raw(self)
23    }
24
25    pub fn to_data_with_calib(&self, calib: &IMUCalibration) -> IMUData {
26        IMUData::from_raw_with_calib(self, calib)
27    }
28}
29
30impl TryFrom<&[u8]> for IMUDataRaw {
31    type Error = JoyConDriverError;
32
33    fn try_from(buf: &[u8]) -> JoyConDriverResult<Self> {
34        let (_, (accel, gyro, _)) = tuple((parse_i16_vec3, parse_i16_vec3, eof))(buf)
35            .map_err(|_| JoyConDriverError::InvalidResultBuffer)?;
36
37        Ok(Self { accel, gyro })
38    }
39}
40
41type F64Vec3 = na::Vector3<f64>;
42
43#[derive(Debug, Clone)]
44pub struct IMUData {
45    pub accel: F64Vec3,
46    pub gyro: F64Vec3,
47}
48
49impl IMUData {
50    pub fn from_raw(raw: &IMUDataRaw) -> Self {
51        let mut accel_raw_f64: F64Vec3 = na::convert(raw.accel);
52        accel_raw_f64[0] -= 350.;
53        let gyro_raw_f64: F64Vec3 = na::convert(raw.gyro);
54
55        Self {
56            accel: accel_raw_f64 * (16. / 65535. * G),
57            gyro: gyro_raw_f64 * (4588. / 65535. * PI / 180.),
58        }
59    }
60
61    pub fn from_raw_with_calib(raw: &IMUDataRaw, calib: &IMUCalibration) -> Self {
62        let accel_raw_f64: F64Vec3 = na::convert(raw.accel);
63        let acell_coeff_inv: F64Vec3 = na::convert(calib.accel_sensitivity - calib.accel_origin);
64
65        let gyro_minus_offset: F64Vec3 = na::convert(raw.gyro - calib.gyro_origin);
66        let gyro_coeff_inv: F64Vec3 = na::convert(calib.gyro_sensitivity - calib.gyro_origin);
67
68        Self {
69            accel: (accel_raw_f64 * 4.0).component_div(&acell_coeff_inv) * G,
70            gyro: (gyro_minus_offset * 936.).component_div(&gyro_coeff_inv) * (PI / 180.),
71        }
72    }
73}
74
75// https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering/blob/master/spi_flash_notes.md#6-axis-sensor-factory-and-user-calibration
76#[derive(Debug, Clone)]
77pub struct IMUCalibration {
78    pub accel_origin: na::Vector3<i16>,
79    pub accel_sensitivity: na::Vector3<i16>,
80    pub gyro_origin: na::Vector3<i16>,
81    pub gyro_sensitivity: na::Vector3<i16>,
82}
83
84impl IMUCalibration {
85    const DATA_BYTES: usize = 24;
86
87    pub fn read_factory_data(device: &JoyConDevice) -> JoyConDriverResult<Self> {
88        let mut buf = [0u8; SUB_COMMAND_READ_HEADER_BYTES + Self::DATA_BYTES];
89        device.read(&mut buf, 0x6020)?;
90        Self::try_from(&buf[SUB_COMMAND_READ_HEADER_BYTES..])
91    }
92
93    pub fn read_user_data(device: &JoyConDevice) -> JoyConDriverResult<Option<Self>> {
94        let mut buf = [0u8; SUB_COMMAND_READ_HEADER_BYTES
95            + USER_CALIBRATION_DATA_MAGIC.len()
96            + Self::DATA_BYTES];
97        device.read(&mut buf, 0x8026)?;
98        const DATA_START: usize = SUB_COMMAND_READ_HEADER_BYTES + USER_CALIBRATION_DATA_MAGIC.len();
99        if buf[SUB_COMMAND_READ_HEADER_BYTES..DATA_START] == USER_CALIBRATION_DATA_MAGIC {
100            Self::try_from(&buf[DATA_START..]).map(Some)
101        } else {
102            Ok(None)
103        }
104    }
105}
106
107impl TryFrom<&[u8]> for IMUCalibration {
108    type Error = JoyConDriverError;
109
110    fn try_from(buf: &[u8]) -> JoyConDriverResult<Self> {
111        let (_, (accel_bias, accel_sensitivity, gyro_bias, gyro_sensitivity, _)) = tuple((
112            parse_i16_vec3,
113            parse_i16_vec3,
114            parse_i16_vec3,
115            parse_i16_vec3,
116            eof,
117        ))(buf)
118        .map_err(|_| JoyConDriverError::InvalidResultBuffer)?;
119
120        Ok(Self {
121            accel_origin: accel_bias,
122            accel_sensitivity,
123            gyro_origin: gyro_bias,
124            gyro_sensitivity,
125        })
126    }
127}
128
129#[derive(Debug, Clone)]
130pub struct IMUOffset(pub na::Vector3<i16>);
131
132impl IMUOffset {
133    const DATA_BYTES: usize = 6;
134
135    pub fn read_data(device: &JoyConDevice) -> JoyConDriverResult<Self> {
136        let mut buf = [0u8; SUB_COMMAND_READ_HEADER_BYTES + Self::DATA_BYTES];
137        device.read(&mut buf, 0x6080)?;
138        Self::try_from(&buf[SUB_COMMAND_READ_HEADER_BYTES..])
139    }
140}
141
142impl TryFrom<&[u8]> for IMUOffset {
143    type Error = JoyConDriverError;
144
145    fn try_from(buf: &[u8]) -> JoyConDriverResult<Self> {
146        let (_, (offset, _)) = tuple((parse_i16_vec3, eof))(buf)
147            .map_err(|_| JoyConDriverError::InvalidResultBuffer)?;
148
149        Ok(IMUOffset(offset))
150    }
151}
joycon_driver/imu.rs

joycon_driver/
imu.rs
