m5unified/
imu.rs

1//! IMU data, calibration, and axis-order helpers.
2//!
3//! This module wraps M5Unified's IMU class with typed vectors, sensor masks,
4//! sensor kind detection, and safe accessors for combined IMU data.
5
6use core::ffi::c_int;
7
8use crate::system::Board;
9
10#[derive(Debug, Copy, Clone, Default, PartialEq)]
11pub struct Vec3 {
12    pub x: f32,
13    pub y: f32,
14    pub z: f32,
15}
16
17#[derive(Debug, Copy, Clone, PartialEq, Eq)]
18pub enum ImuAxis {
19    XPos,
20    XNeg,
21    YPos,
22    YNeg,
23    ZPos,
24    ZNeg,
25    Raw(i32),
26}
27
28impl ImuAxis {
29    pub const fn raw(self) -> i32 {
30        match self {
31            Self::XPos => 0,
32            Self::XNeg => 1,
33            Self::YPos => 2,
34            Self::YNeg => 3,
35            Self::ZPos => 4,
36            Self::ZNeg => 5,
37            Self::Raw(raw) => raw,
38        }
39    }
40}
41
42#[derive(Debug, Copy, Clone, Default, PartialEq, Eq)]
43pub struct ImuSensorMask(u8);
44
45impl ImuSensorMask {
46    pub const NONE: Self = Self(0);
47    pub const ACCEL: Self = Self(1 << 0);
48    pub const GYRO: Self = Self(1 << 1);
49    pub const MAG: Self = Self(1 << 2);
50
51    pub const fn from_raw(raw: u8) -> Self {
52        Self(raw)
53    }
54
55    pub const fn raw(self) -> u8 {
56        self.0
57    }
58
59    pub const fn is_empty(self) -> bool {
60        self.0 == 0
61    }
62
63    pub const fn contains(self, other: Self) -> bool {
64        self.0 & other.0 == other.0
65    }
66}
67
68#[derive(Debug)]
69pub struct Imu;
70
71impl Imu {
72    pub fn begin(&mut self) -> bool {
73        unsafe { m5unified_sys::m5u_imu_begin() }
74    }
75
76    pub fn begin_for_board(&mut self, board: Board) -> bool {
77        unsafe { m5unified_sys::m5u_imu_begin_for_board(board.raw()) }
78    }
79
80    pub fn init(&mut self) -> bool {
81        self.begin()
82    }
83
84    pub fn init_for_board(&mut self, board: Board) -> bool {
85        self.begin_for_board(board)
86    }
87
88    pub fn ak8963(&self) -> ImuDevice {
89        ImuDevice::new(ImuDeviceKind::Ak8963)
90    }
91
92    pub fn bmm150(&self) -> ImuDevice {
93        ImuDevice::new(ImuDeviceKind::Bmm150)
94    }
95
96    pub fn bmi270(&self) -> ImuDevice {
97        ImuDevice::new(ImuDeviceKind::Bmi270)
98    }
99
100    pub fn mpu6886(&self) -> ImuDevice {
101        ImuDevice::new(ImuDeviceKind::Mpu6886)
102    }
103
104    pub fn sh200q(&self) -> ImuDevice {
105        ImuDevice::new(ImuDeviceKind::Sh200q)
106    }
107
108    pub fn accel(&self) -> Option<Vec3> {
109        let (mut x, mut y, mut z) = (0.0, 0.0, 0.0);
110        let ok = unsafe { m5unified_sys::m5u_imu_get_accel(&mut x, &mut y, &mut z) };
111        ok.then_some(Vec3 { x, y, z })
112    }
113
114    pub fn accel_data(&self) -> Option<Vec3> {
115        self.accel()
116    }
117
118    pub fn gyro(&self) -> Option<Vec3> {
119        let (mut x, mut y, mut z) = (0.0, 0.0, 0.0);
120        let ok = unsafe { m5unified_sys::m5u_imu_get_gyro(&mut x, &mut y, &mut z) };
121        ok.then_some(Vec3 { x, y, z })
122    }
123
124    pub fn gyro_data(&self) -> Option<Vec3> {
125        self.gyro()
126    }
127
128    pub fn mag(&self) -> Option<Vec3> {
129        let (mut x, mut y, mut z) = (0.0, 0.0, 0.0);
130        let ok = unsafe { m5unified_sys::m5u_imu_get_mag(&mut x, &mut y, &mut z) };
131        ok.then_some(Vec3 { x, y, z })
132    }
133
134    pub fn gyro_mag(&self) -> Option<Vec3> {
135        self.mag()
136    }
137
138    pub fn temperature_c(&self) -> Option<f32> {
139        let mut temp = 0.0;
140        let ok = unsafe { m5unified_sys::m5u_imu_get_temp_c(&mut temp) };
141        ok.then_some(temp)
142    }
143
144    pub fn is_enabled(&self) -> bool {
145        unsafe { m5unified_sys::m5u_imu_is_enabled() }
146    }
147
148    pub fn kind(&self) -> ImuKind {
149        ImuKind::from_raw(unsafe { m5unified_sys::m5u_imu_get_type() as i32 })
150    }
151
152    pub fn update(&mut self) -> bool {
153        unsafe { m5unified_sys::m5u_imu_update() }
154    }
155
156    pub fn update_mask(&mut self) -> ImuSensorMask {
157        let raw = unsafe { m5unified_sys::m5u_imu_update_mask() };
158        ImuSensorMask::from_raw(raw.max(0) as u8)
159    }
160
161    pub fn data(&self) -> Option<ImuData> {
162        let mut raw = m5unified_sys::m5u_imu_data_t::default();
163        let ok = unsafe { m5unified_sys::m5u_imu_get_data(&mut raw) };
164        ok.then(|| ImuData {
165            usec: raw.usec,
166            accel: Vec3 {
167                x: raw.accel_x,
168                y: raw.accel_y,
169                z: raw.accel_z,
170            },
171            gyro: Vec3 {
172                x: raw.gyro_x,
173                y: raw.gyro_y,
174                z: raw.gyro_z,
175            },
176            mag: Vec3 {
177                x: raw.mag_x,
178                y: raw.mag_y,
179                z: raw.mag_z,
180            },
181            temperature_c: self.temperature_c(),
182        })
183    }
184
185    pub fn load_offset_from_nvs(&mut self) -> bool {
186        unsafe { m5unified_sys::m5u_imu_load_offset_from_nvs() }
187    }
188
189    pub fn save_offset_to_nvs(&mut self) -> bool {
190        unsafe { m5unified_sys::m5u_imu_save_offset_to_nvs() }
191    }
192
193    pub fn offset_data(&self, index: i32) -> f32 {
194        unsafe { m5unified_sys::m5u_imu_get_offset_data(index) }
195    }
196
197    pub fn set_calibration(&mut self, x: f32, y: f32, z: f32) {
198        unsafe { m5unified_sys::m5u_imu_set_calibration(x, y, z) }
199    }
200
201    pub fn sleep(&mut self) -> bool {
202        unsafe { m5unified_sys::m5u_imu_sleep() }
203    }
204
205    pub fn set_clock_hz(&mut self, freq: u32) {
206        unsafe { m5unified_sys::m5u_imu_set_clock(freq) }
207    }
208
209    pub fn set_axis_order(&mut self, axis0: ImuAxis, axis1: ImuAxis, axis2: ImuAxis) -> bool {
210        unsafe { m5unified_sys::m5u_imu_set_axis_order(axis0.raw(), axis1.raw(), axis2.raw()) }
211    }
212
213    pub fn set_axis_order_right_handed(&mut self, axis0: ImuAxis, axis1: ImuAxis) -> bool {
214        unsafe { m5unified_sys::m5u_imu_set_axis_order_right_handed(axis0.raw(), axis1.raw()) }
215    }
216
217    pub fn set_axis_order_left_handed(&mut self, axis0: ImuAxis, axis1: ImuAxis) -> bool {
218        unsafe { m5unified_sys::m5u_imu_set_axis_order_left_handed(axis0.raw(), axis1.raw()) }
219    }
220
221    pub fn set_int_pin_active_logic(&mut self, level: bool) -> bool {
222        unsafe { m5unified_sys::m5u_imu_set_int_pin_active_logic(level) }
223    }
224
225    pub fn set_calibration_strength(&mut self, accel: u8, gyro: u8, mag: u8) {
226        unsafe { m5unified_sys::m5u_imu_set_calibration_strength(accel, gyro, mag) }
227    }
228
229    pub fn clear_offset_data(&mut self) {
230        unsafe { m5unified_sys::m5u_imu_clear_offset_data() }
231    }
232
233    pub fn set_offset_data(&mut self, index: usize, value: i32) {
234        unsafe { m5unified_sys::m5u_imu_set_offset_data(index, value) }
235    }
236
237    pub fn offset_data_i32(&self, index: usize) -> i32 {
238        unsafe { m5unified_sys::m5u_imu_get_offset_data_i32(index) }
239    }
240
241    pub fn raw_data(&self, index: usize) -> i16 {
242        unsafe { m5unified_sys::m5u_imu_get_raw_data(index) }
243    }
244}
245
246#[derive(Debug, Copy, Clone, PartialEq, Eq)]
247pub enum ImuKind {
248    None,
249    Sh200q,
250    Mpu6050,
251    Mpu6886,
252    Mpu9250,
253    Bmi270,
254    Unknown(i32),
255}
256
257impl ImuKind {
258    pub const fn from_raw(raw: i32) -> Self {
259        match raw {
260            0 => Self::None,
261            2 => Self::Sh200q,
262            3 => Self::Mpu6050,
263            4 => Self::Mpu6886,
264            5 => Self::Mpu9250,
265            6 => Self::Bmi270,
266            raw => Self::Unknown(raw),
267        }
268    }
269
270    pub const fn raw(self) -> i32 {
271        match self {
272            Self::None => 0,
273            Self::Unknown(raw) => raw,
274            Self::Sh200q => 2,
275            Self::Mpu6050 => 3,
276            Self::Mpu6886 => 4,
277            Self::Mpu9250 => 5,
278            Self::Bmi270 => 6,
279        }
280    }
281}
282
283#[derive(Debug, Copy, Clone, Default, PartialEq)]
284pub struct ImuData {
285    pub usec: u32,
286    pub accel: Vec3,
287    pub gyro: Vec3,
288    pub mag: Vec3,
289    pub temperature_c: Option<f32>,
290}
291
292#[derive(Debug, Copy, Clone, PartialEq, Eq)]
293pub enum ImuDeviceKind {
294    Ak8963,
295    Bmm150,
296    Bmi270,
297    Mpu6886,
298    Sh200q,
299}
300
301impl ImuDeviceKind {
302    const fn raw(self) -> c_int {
303        match self {
304            Self::Ak8963 => 0,
305            Self::Bmm150 => 1,
306            Self::Bmi270 => 2,
307            Self::Mpu6886 => 3,
308            Self::Sh200q => 4,
309        }
310    }
311}
312
313#[derive(Debug, Copy, Clone, Default, PartialEq, Eq)]
314pub struct RawVec3 {
315    pub x: i16,
316    pub y: i16,
317    pub z: i16,
318}
319
320impl RawVec3 {
321    pub const fn new(x: i16, y: i16, z: i16) -> Self {
322        Self { x, y, z }
323    }
324
325    pub fn scaled(self, resolution: f32) -> Vec3 {
326        Vec3 {
327            x: self.x as f32 * resolution,
328            y: self.y as f32 * resolution,
329            z: self.z as f32 * resolution,
330        }
331    }
332}
333
334#[derive(Debug, Copy, Clone, Default, PartialEq, Eq)]
335pub struct ImuRawData {
336    pub sensor_mask: ImuSensorMask,
337    pub accel: RawVec3,
338    pub gyro: RawVec3,
339    pub mag: RawVec3,
340    pub temp_adc: i16,
341}
342
343impl ImuRawData {
344    fn from_raw(raw: m5unified_sys::m5u_imu_raw_data_t) -> Self {
345        Self {
346            sensor_mask: ImuSensorMask::from_raw(raw.sensor_mask),
347            accel: RawVec3::new(raw.accel_x, raw.accel_y, raw.accel_z),
348            gyro: RawVec3::new(raw.gyro_x, raw.gyro_y, raw.gyro_z),
349            mag: RawVec3::new(raw.mag_x, raw.mag_y, raw.mag_z),
350            temp_adc: raw.temp,
351        }
352    }
353
354    pub const fn has_accel(self) -> bool {
355        self.sensor_mask.contains(ImuSensorMask::ACCEL)
356    }
357
358    pub const fn has_gyro(self) -> bool {
359        self.sensor_mask.contains(ImuSensorMask::GYRO)
360    }
361
362    pub const fn has_mag(self) -> bool {
363        self.sensor_mask.contains(ImuSensorMask::MAG)
364    }
365}
366
367#[derive(Debug, Copy, Clone, PartialEq)]
368pub struct ImuConvertParams {
369    pub accel_res: f32,
370    pub gyro_res: f32,
371    pub mag_res: f32,
372    pub temp_res: f32,
373    pub temp_offset: f32,
374}
375
376impl ImuConvertParams {
377    fn from_raw(raw: m5unified_sys::m5u_imu_convert_param_t) -> Self {
378        Self {
379            accel_res: raw.accel_res,
380            gyro_res: raw.gyro_res,
381            mag_res: raw.mag_res,
382            temp_res: raw.temp_res,
383            temp_offset: raw.temp_offset,
384        }
385    }
386
387    pub fn temperature_c(self, adc: i16) -> f32 {
388        adc as f32 * self.temp_res + self.temp_offset
389    }
390}
391
392impl Default for ImuConvertParams {
393    fn default() -> Self {
394        Self::from_raw(m5unified_sys::m5u_imu_convert_param_t::default())
395    }
396}
397
398#[derive(Debug, Copy, Clone, PartialEq, Eq)]
399pub struct ImuDevice {
400    kind: ImuDeviceKind,
401}
402
403impl ImuDevice {
404    const fn new(kind: ImuDeviceKind) -> Self {
405        Self { kind }
406    }
407
408    pub const fn kind(&self) -> ImuDeviceKind {
409        self.kind
410    }
411
412    pub fn begin(&mut self) -> ImuSensorMask {
413        let raw = unsafe { m5unified_sys::m5u_imu_device_begin(self.kind.raw()) };
414        ImuSensorMask::from_raw(raw.max(0) as u8)
415    }
416
417    pub fn init(&mut self) -> ImuSensorMask {
418        self.begin()
419    }
420
421    pub fn raw_data(&self) -> Option<ImuRawData> {
422        let mut raw = m5unified_sys::m5u_imu_raw_data_t::default();
423        unsafe { m5unified_sys::m5u_imu_device_get_raw_data(self.kind.raw(), &mut raw) }
424            .then_some(ImuRawData::from_raw(raw))
425    }
426
427    pub fn convert_params(&self) -> Option<ImuConvertParams> {
428        let mut raw = m5unified_sys::m5u_imu_convert_param_t::default();
429        unsafe { m5unified_sys::m5u_imu_device_get_convert_param(self.kind.raw(), &mut raw) }
430            .then_some(ImuConvertParams::from_raw(raw))
431    }
432
433    pub fn data(&self) -> Option<ImuData> {
434        let raw = self.raw_data()?;
435        let params = self.convert_params().unwrap_or_default();
436        Some(ImuData {
437            usec: 0,
438            accel: raw.accel.scaled(params.accel_res),
439            gyro: raw.gyro.scaled(params.gyro_res),
440            mag: raw.mag.scaled(params.mag_res),
441            temperature_c: self.temperature_adc().map(|adc| params.temperature_c(adc)),
442        })
443    }
444
445    pub fn temperature_adc(&self) -> Option<i16> {
446        let mut adc = 0;
447        unsafe { m5unified_sys::m5u_imu_device_get_temp_adc(self.kind.raw(), &mut adc) }
448            .then_some(adc)
449    }
450
451    pub fn temperature_c(&self) -> Option<f32> {
452        let adc = self.temperature_adc()?;
453        self.convert_params()
454            .map(|params| params.temperature_c(adc))
455    }
456
457    pub fn sleep(&mut self) -> bool {
458        unsafe { m5unified_sys::m5u_imu_device_sleep(self.kind.raw()) }
459    }
460
461    pub fn set_int_pin_active_logic(&mut self, level: bool) -> bool {
462        unsafe { m5unified_sys::m5u_imu_device_set_int_pin_active_logic(self.kind.raw(), level) }
463    }
464
465    pub fn who_am_i(&self) -> Option<u8> {
466        let raw = unsafe { m5unified_sys::m5u_imu_device_who_am_i(self.kind.raw()) };
467        (raw >= 0).then_some(raw as u8)
468    }
469}
m5unified/imu.rs

m5unified/
imu.rs
