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(&self, vector: Vector3) -> 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 assume unit quaternions
59        let qw = self.w;
60        let qx = self.x;
61        let qy = self.y;
62        let qz = self.z;
63        let vx = vector.x;
64        let vy = vector.y;
65        let vz = vector.z;
66
67        // Calculate rotation using quaternion multiplication
68        let x = (1.0 - 2.0 * qy * qy - 2.0 * qz * qz) * vx
69            + (2.0 * qx * qy - 2.0 * qz * qw) * vy
70            + (2.0 * qx * qz + 2.0 * qy * qw) * vz;
71        let y = (2.0 * qx * qy + 2.0 * qz * qw) * vx
72            + (1.0 - 2.0 * qx * qx - 2.0 * qz * qz) * vy
73            + (2.0 * qy * qz - 2.0 * qx * qw) * vz;
74        let z = (2.0 * qx * qz - 2.0 * qy * qw) * vx
75            + (2.0 * qy * qz + 2.0 * qx * qw) * vy
76            + (1.0 - 2.0 * qx * qx - 2.0 * qy * qy) * vz;
77
78        Vector3 { x, y, z }
79    }
80}
81
82impl fmt::Display for Quaternion {
83    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
84        write!(
85            f,
86            "Quaternion(w={}, x={}, y={}, z={})",
87            self.w, self.x, self.y, self.z
88        )
89    }
90}
91
92// --- Standard IMU Data ---
93#[derive(Debug, Clone, Copy, Default)]
94pub struct ImuData {
95    /// Acceleration including gravity (m/s²)
96    pub accelerometer: Option<Vector3>,
97    /// Angular velocity (deg/s)
98    pub gyroscope: Option<Vector3>,
99    /// Magnetic field vector (micro Tesla, µT)
100    pub magnetometer: Option<Vector3>,
101    /// Orientation as a unit quaternion (WXYZ order)
102    pub quaternion: Option<Quaternion>,
103    /// Orientation as Euler angles (deg)
104    pub euler: Option<Vector3>,
105    /// Linear acceleration (acceleration without gravity) (m/s²)
106    pub linear_acceleration: Option<Vector3>,
107    /// Estimated gravity vector (m/s²)
108    pub gravity: Option<Vector3>,
109    /// Temperature (°C)
110    pub temperature: Option<f32>,
111    /// Calibration status
112    pub calibration_status: Option<u8>,
113}
114
115// --- Standard Error Type ---
116#[derive(Debug)]
117pub enum ImuError {
118    /// Error originating from the underlying device communication (I2C, Serial, CAN)
119    DeviceError(String),
120    /// Error reading data from the device or internal state
121    ReadError(String),
122    /// Error writing commands or configuration to the device
123    WriteError(String),
124    /// Error during device configuration or setup
125    ConfigurationError(String),
126    /// Error related to multithreading locks (e.g., poisoned)
127    LockError(String),
128    /// Error sending a command to the reader thread
129    CommandSendError(String),
130    /// Functionality not supported by this specific IMU implementation
131    NotSupported(String),
132    /// Invalid packet received from the device
133    InvalidPacket(String),
134    /// Catch-all for other errors
135    Other(String),
136}
137
138impl fmt::Display for ImuError {
139    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
140        match self {
141            ImuError::DeviceError(s) => write!(f, "Device error: {}", s),
142            ImuError::ReadError(s) => write!(f, "Read error: {}", s),
143            ImuError::WriteError(s) => write!(f, "Write error: {}", s),
144            ImuError::ConfigurationError(s) => write!(f, "Configuration error: {}", s),
145            ImuError::LockError(s) => write!(f, "Lock error: {}", s),
146            ImuError::CommandSendError(s) => write!(f, "Command send error: {}", s),
147            ImuError::NotSupported(s) => write!(f, "Not supported: {}", s),
148            ImuError::InvalidPacket(s) => write!(f, "Invalid packet: {}", s),
149            ImuError::Other(s) => write!(f, "Other IMU error: {}", s),
150        }
151    }
152}
153
154impl StdError for ImuError {}
155
156// Add From implementations for common error types
157impl From<io::Error> for ImuError {
158    fn from(err: io::Error) -> Self {
159        ImuError::DeviceError(err.to_string())
160    }
161}
162
163impl From<serialport::Error> for ImuError {
164    fn from(err: serialport::Error) -> Self {
165        ImuError::DeviceError(err.to_string())
166    }
167}
168
169impl<T> From<std::sync::PoisonError<T>> for ImuError {
170    fn from(err: std::sync::PoisonError<T>) -> Self {
171        ImuError::LockError(err.to_string())
172    }
173}
174
175impl<T> From<mpsc::SendError<T>> for ImuError {
176    fn from(err: mpsc::SendError<T>) -> Self {
177        ImuError::CommandSendError(err.to_string())
178    }
179}
180
181impl From<mpsc::RecvError> for ImuError {
182    fn from(err: mpsc::RecvError) -> Self {
183        ImuError::CommandSendError(err.to_string())
184    }
185}
186
187pub trait ImuReader {
188    /// Retrieves the latest available IMU data.
189    fn get_data(&self) -> Result<ImuData, ImuError>;
190
191    fn stop(&self) -> Result<(), ImuError>;
192}
193
194#[derive(Debug, Clone, Copy)]
195pub enum ImuFrequency {
196    Hz0_2,  // 0.2 Hz
197    Hz0_5,  // 0.5 Hz
198    Hz1,    // 1 Hz
199    Hz2,    // 2 Hz
200    Hz5,    // 5 Hz
201    Hz10,   // 10 Hz
202    Hz20,   // 20 Hz
203    Hz50,   // 50 Hz
204    Hz100,  // 100 Hz
205    Hz200,  // 200 Hz
206    Single, // Single reading
207    None,   // No readings
208}
imu_traits/lib.rs

imu_traits/
lib.rs
