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

//! V5 Inertial Sensor

use core::ffi::c_double;

use crate::{device::V5_DeviceT, map_jump_table};

#[repr(C)]
#[repr(packed)]
#[derive(Default, Debug, Copy, Clone, PartialEq)]
pub struct V5_DeviceImuRaw {
    pub x: c_double,
    pub y: c_double,
    pub z: c_double,
    pub w: c_double,
}

#[repr(C)]
#[repr(packed)]
#[derive(Default, Debug, Copy, Clone, PartialEq)]
pub struct V5_DeviceImuQuaternion {
    pub a: c_double,
    pub b: c_double,
    pub c: c_double,
    pub d: c_double,
}

#[derive(Default, Debug, Copy, Clone, Eq, PartialEq)]
pub struct V5ImuOrientationMode(core::ffi::c_uint);

impl V5ImuOrientationMode {
    pub const kImuOrientationZUp: core::ffi::c_uint = 0x00;
    pub const kImuOrientationZDown: core::ffi::c_uint = 0x10;
    pub const kImuOrientationXUp: core::ffi::c_uint = 0x20;
    pub const kImuOrientationXDown: core::ffi::c_uint = 0x30;
    pub const kImuOrientationYUp: core::ffi::c_uint = 0x40;
    pub const kImuOrientationYDown: core::ffi::c_uint = 0x50;
    pub const kImuOrientationAuto: core::ffi::c_uint = 0x80;
}

#[repr(C)]
#[repr(packed)]
#[derive(Default, Debug, Copy, Clone, PartialEq)]
pub struct V5_DeviceImuAttitude {
    pub pitch: c_double,
    pub roll: c_double,
    pub yaw: c_double,
}

map_jump_table! {
    0x410 =>
        /// Calibrates the IMU. This function is non-blocking.
        pub fn vexDeviceImuReset(device: V5_DeviceT),
    0x414 =>
        /// Returns the yaw-axis rotation of the IMU as an unbounded angle in degrees.
        pub fn vexDeviceImuHeadingGet(device: V5_DeviceT) -> c_double,
    0x418 =>
        /// Returns the yaw-axis rotation of the IMU as an angle in degrees bounded 0-360.
        pub fn vexDeviceImuDegreesGet(device: V5_DeviceT) -> c_double,
    0x41c =>
        /// Returns quaternion defined by the IMU's rotation.
        pub fn vexDeviceImuQuaternionGet(device: V5_DeviceT, data: *mut V5_DeviceImuQuaternion),
    0x420 =>
        /// Returns the 3-axis euler angles of the IMU bounded from -180 to 180 degrees.
        pub fn vexDeviceImuAttitudeGet(device: V5_DeviceT, data: *mut V5_DeviceImuAttitude),
    0x424 =>
        /// Returns the raw gyroscope readings of the IMU in degrees per second,
        pub fn vexDeviceImuRawGyroGet(device: V5_DeviceT, data: *mut V5_DeviceImuRaw),
    0x428 =>
        /// Returns the raw accelerometer readings of the IMU in G.
        pub fn vexDeviceImuRawAccelGet(device: V5_DeviceT, data: *mut V5_DeviceImuRaw),
    0x42c =>
        /// Returns the IMU's status bits.
        pub fn vexDeviceImuStatusGet(device: V5_DeviceT) -> u32,
    0x430 => pub fn vexDeviceImuTemperatureGet(device: V5_DeviceT) -> c_double,
    0x438 => pub fn vexDeviceImuModeSet(device: V5_DeviceT, mode: u32),
    0x43c => pub fn vexDeviceImuModeGet(device: V5_DeviceT) -> u32,
    0x444 => pub fn vexDeviceImuDataRateSet(device: V5_DeviceT, rate: u32),
}