imu_traits/
lib.rs

1use std::error::Error as StdError;
2use std::fmt;
3use std::io;
4use std::sync::mpsc;
5
6// --- Basic Types ---
7#[derive(Debug, Clone, Copy, Default, PartialEq)]
8pub struct Vector3 {
9    pub x: f32,
10    pub y: f32,
11    pub z: f32,
12}
13
14#[derive(Debug, Clone, Copy, Default, PartialEq)]
15pub struct Quaternion {
16    pub w: f32,
17    pub x: f32,
18    pub y: f32,
19    pub z: f32,
20}
21
22impl Vector3 {
23    pub fn new(x: f32, y: f32, z: f32) -> Self {
24        Self { x, y, z }
25    }
26
27    pub fn euler_to_quaternion(&self) -> Quaternion {
28        // Convert Euler angles (in radians) to quaternion
29        // Using the ZYX rotation order (yaw, pitch, roll)
30        let (roll, pitch, yaw) = (self.x, self.y, self.z);
31
32        let cr = (roll * 0.5).cos();
33        let sr = (roll * 0.5).sin();
34        let cp = (pitch * 0.5).cos();
35        let sp = (pitch * 0.5).sin();
36        let cy = (yaw * 0.5).cos();
37        let sy = (yaw * 0.5).sin();
38
39        let w = cr * cp * cy + sr * sp * sy;
40        let x = sr * cp * cy - cr * sp * sy;
41        let y = cr * sp * cy + sr * cp * sy;
42        let z = cr * cp * sy - sr * sp * cy;
43
44        Quaternion { w, x, y, z }
45    }
46}
47
48impl fmt::Display for Vector3 {
49    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
50        write!(f, "Vector3(x={}, y={}, z={})", self.x, self.y, self.z)
51    }
52}
53
54impl Quaternion {
55    pub fn rotate_vector(&self, v: Vector3, inverse: bool) -> Vector3 {
56        // Rotate a vector by a quaternion using the formula:
57        // v' = q * v * q^-1
58        // Where q^-1 is the conjugate since we normalize the quaternion
59        const EPS: f32 = 1e-6;
60        let n2 = self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z;
61        let scale = 1.0 / (n2.sqrt() + EPS);
62
63        let w = self.w * scale;
64        let mut x = self.x * scale;
65        let mut y = self.y * scale;
66        let mut z = self.z * scale;
67
68        // 2. Conjugate if inverse
69        if inverse {
70            x = -x;
71            y = -y;
72            z = -z;
73        }
74
75        // 3. Apply the expanded formula
76        let vx = v.x;
77        let vy = v.y;
78        let vz = v.z;
79        let xx = w * w * vx + 2.0 * y * w * vz - 2.0 * z * w * vy
80            + x * x * vx
81            + 2.0 * y * x * vy
82            + 2.0 * z * x * vz
83            - z * z * vx
84            - y * y * vx;
85        let yy = 2.0 * x * y * vx + y * y * vy + 2.0 * z * y * vz + 2.0 * w * z * vx - z * z * vy
86            + w * w * vy
87            - 2.0 * w * x * vz
88            - x * x * vy;
89        let zz = 2.0 * x * z * vx + 2.0 * y * z * vy + z * z * vz - 2.0 * w * y * vx
90            + w * w * vz
91            + 2.0 * w * x * vy
92            - y * y * vz
93            - x * x * vz;
94
95        Vector3 {
96            x: xx,
97            y: yy,
98            z: zz,
99        }
100    }
101}
102
103impl fmt::Display for Quaternion {
104    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
105        write!(
106            f,
107            "Quaternion(w={}, x={}, y={}, z={})",
108            self.w, self.x, self.y, self.z
109        )
110    }
111}
112
113// --- Standard IMU Data ---
114#[derive(Debug, Clone, Copy, Default)]
115pub struct ImuData {
116    /// Acceleration including gravity (m/s²)
117    pub accelerometer: Option<Vector3>,
118    /// Angular velocity (deg/s)
119    pub gyroscope: Option<Vector3>,
120    /// Magnetic field vector (micro Tesla, µT)
121    pub magnetometer: Option<Vector3>,
122    /// Orientation as a unit quaternion (WXYZ order)
123    pub quaternion: Option<Quaternion>,
124    /// Orientation as Euler angles (deg)
125    pub euler: Option<Vector3>,
126    /// Linear acceleration (acceleration without gravity) (m/s²)
127    pub linear_acceleration: Option<Vector3>,
128    /// Estimated gravity vector (m/s²)
129    pub gravity: Option<Vector3>,
130    /// Temperature (°C)
131    pub temperature: Option<f32>,
132    /// Calibration status
133    pub calibration_status: Option<u8>,
134}
135
136// --- Standard Error Type ---
137#[derive(Debug)]
138pub enum ImuError {
139    /// Error originating from the underlying device communication (I2C, Serial, CAN)
140    DeviceError(String),
141    /// Error reading data from the device or internal state
142    ReadError(String),
143    /// Error writing commands or configuration to the device
144    WriteError(String),
145    /// Error during device configuration or setup
146    ConfigurationError(String),
147    /// Error related to multithreading locks (e.g., poisoned)
148    LockError(String),
149    /// Error sending a command to the reader thread
150    CommandSendError(String),
151    /// Functionality not supported by this specific IMU implementation
152    NotSupported(String),
153    /// Invalid packet received from the device
154    InvalidPacket(String),
155    /// Catch-all for other errors
156    Other(String),
157}
158
159impl fmt::Display for ImuError {
160    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
161        match self {
162            ImuError::DeviceError(s) => write!(f, "Device error: {}", s),
163            ImuError::ReadError(s) => write!(f, "Read error: {}", s),
164            ImuError::WriteError(s) => write!(f, "Write error: {}", s),
165            ImuError::ConfigurationError(s) => write!(f, "Configuration error: {}", s),
166            ImuError::LockError(s) => write!(f, "Lock error: {}", s),
167            ImuError::CommandSendError(s) => write!(f, "Command send error: {}", s),
168            ImuError::NotSupported(s) => write!(f, "Not supported: {}", s),
169            ImuError::InvalidPacket(s) => write!(f, "Invalid packet: {}", s),
170            ImuError::Other(s) => write!(f, "Other IMU error: {}", s),
171        }
172    }
173}
174
175impl StdError for ImuError {}
176
177// Add From implementations for common error types
178impl From<io::Error> for ImuError {
179    fn from(err: io::Error) -> Self {
180        ImuError::DeviceError(err.to_string())
181    }
182}
183
184impl From<serialport::Error> for ImuError {
185    fn from(err: serialport::Error) -> Self {
186        ImuError::DeviceError(err.to_string())
187    }
188}
189
190impl<T> From<std::sync::PoisonError<T>> for ImuError {
191    fn from(err: std::sync::PoisonError<T>) -> Self {
192        ImuError::LockError(err.to_string())
193    }
194}
195
196impl<T> From<mpsc::SendError<T>> for ImuError {
197    fn from(err: mpsc::SendError<T>) -> Self {
198        ImuError::CommandSendError(err.to_string())
199    }
200}
201
202impl From<mpsc::RecvError> for ImuError {
203    fn from(err: mpsc::RecvError) -> Self {
204        ImuError::CommandSendError(err.to_string())
205    }
206}
207
208pub trait ImuReader {
209    /// Retrieves the latest available IMU data.
210    fn get_data(&self) -> Result<ImuData, ImuError>;
211
212    fn stop(&self) -> Result<(), ImuError>;
213}
214
215#[derive(Debug, Clone, Copy)]
216pub enum ImuFrequency {
217    Hz0_2,  // 0.2 Hz
218    Hz0_5,  // 0.5 Hz
219    Hz1,    // 1 Hz
220    Hz2,    // 2 Hz
221    Hz5,    // 5 Hz
222    Hz10,   // 10 Hz
223    Hz20,   // 20 Hz
224    Hz50,   // 50 Hz
225    Hz100,  // 100 Hz
226    Hz200,  // 200 Hz
227    Single, // Single reading
228    None,   // No readings
229}
imu_traits/lib.rs

imu_traits/
lib.rs
