mecha10_controllers/
imu.rs

1//! IMU controller trait and types
2
3use crate::{Controller, ControllerError};
4use async_trait::async_trait;
5use mecha10_core::sensor::ImuData;
6use serde::{Deserialize, Serialize};
7
8/// IMU (Inertial Measurement Unit) controller trait
9///
10/// Provides a unified interface for different IMU types:
11/// - 6-DOF IMUs (accelerometer + gyroscope)
12/// - 9-DOF IMUs (accel + gyro + magnetometer)
13/// - Absolute orientation sensors with sensor fusion
14#[async_trait]
15pub trait ImuController: Controller {
16    /// Read current IMU data
17    ///
18    /// Returns the latest sensor readings including orientation,
19    /// angular velocity, and linear acceleration.
20    async fn read_imu(&mut self) -> Result<ImuData, Self::Error>;
21
22    /// Get calibration status
23    ///
24    /// Returns the current calibration state of each sensor component.
25    fn calibration_status(&self) -> CalibrationStatus;
26
27    /// Perform sensor calibration
28    ///
29    /// Initiate the calibration process. This may take several seconds
30    /// and require specific device movements (e.g., figure-8 for magnetometer).
31    async fn calibrate(&mut self) -> Result<CalibrationData, Self::Error>;
32
33    /// Save calibration data to persistent storage
34    async fn save_calibration(&self, path: &str) -> Result<(), Self::Error>;
35
36    /// Load calibration data from persistent storage
37    async fn load_calibration(&mut self, path: &str) -> Result<(), Self::Error>;
38
39    /// Reset sensor fusion (if supported)
40    ///
41    /// Reset the orientation estimate to a known state.
42    async fn reset_fusion(&mut self) -> Result<(), Self::Error>
43    where
44        Self::Error: From<ControllerError>,
45    {
46        Err(Self::Error::from(ControllerError::NotSupported(
47            "Sensor fusion reset not supported".to_string(),
48        )))
49    }
50
51    /// Get sensor temperature if available
52    async fn temperature(&self) -> Option<f32> {
53        None
54    }
55
56    /// Check if the IMU has a magnetometer
57    fn has_magnetometer(&self) -> bool {
58        self.capabilities()
59            .features
60            .get("magnetometer")
61            .copied()
62            .unwrap_or(false)
63    }
64
65    /// Check if the IMU provides absolute orientation
66    fn has_absolute_orientation(&self) -> bool {
67        self.capabilities()
68            .features
69            .get("absolute_orientation")
70            .copied()
71            .unwrap_or(false)
72    }
73}
74
75// ============================================================================
76// Calibration Types
77// ============================================================================
78
79/// Calibration status for each sensor component
80#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)]
81pub struct CalibrationStatus {
82    /// System calibration (overall)
83    pub system: CalibrationLevel,
84
85    /// Accelerometer calibration
86    pub accelerometer: CalibrationLevel,
87
88    /// Gyroscope calibration
89    pub gyroscope: CalibrationLevel,
90
91    /// Magnetometer calibration (if present)
92    pub magnetometer: Option<CalibrationLevel>,
93}
94
95impl Default for CalibrationStatus {
96    fn default() -> Self {
97        Self {
98            system: CalibrationLevel::Uncalibrated,
99            accelerometer: CalibrationLevel::Uncalibrated,
100            gyroscope: CalibrationLevel::Uncalibrated,
101            magnetometer: None,
102        }
103    }
104}
105
106impl CalibrationStatus {
107    /// Check if all sensors are fully calibrated
108    pub fn is_fully_calibrated(&self) -> bool {
109        self.system == CalibrationLevel::FullyCalibrated
110            && self.accelerometer == CalibrationLevel::FullyCalibrated
111            && self.gyroscope == CalibrationLevel::FullyCalibrated
112            && self
113                .magnetometer
114                .as_ref()
115                .map(|m| *m == CalibrationLevel::FullyCalibrated)
116                .unwrap_or(true)
117    }
118
119    /// Check if minimally calibrated (ready to use)
120    pub fn is_minimally_calibrated(&self) -> bool {
121        self.system != CalibrationLevel::Uncalibrated
122            && self.accelerometer != CalibrationLevel::Uncalibrated
123            && self.gyroscope != CalibrationLevel::Uncalibrated
124    }
125}
126
127/// Calibration level (0-3)
128#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Serialize, Deserialize)]
129pub enum CalibrationLevel {
130    /// Not calibrated
131    Uncalibrated = 0,
132
133    /// Minimally calibrated
134    Minimal = 1,
135
136    /// Partially calibrated
137    Partial = 2,
138
139    /// Fully calibrated
140    FullyCalibrated = 3,
141}
142
143impl From<u8> for CalibrationLevel {
144    fn from(value: u8) -> Self {
145        match value {
146            0 => Self::Uncalibrated,
147            1 => Self::Minimal,
148            2 => Self::Partial,
149            _ => Self::FullyCalibrated,
150        }
151    }
152}
153
154impl From<CalibrationLevel> for u8 {
155    fn from(level: CalibrationLevel) -> Self {
156        level as u8
157    }
158}
159
160/// Calibration data that can be saved/loaded
161#[derive(Debug, Clone, Serialize, Deserialize)]
162pub struct CalibrationData {
163    /// Timestamp when calibration was performed
164    pub timestamp: u64,
165
166    /// IMU model/type
167    pub model: String,
168
169    /// Accelerometer offsets [x, y, z]
170    pub accel_offset: [f32; 3],
171
172    /// Accelerometer radius
173    pub accel_radius: Option<f32>,
174
175    /// Gyroscope offsets [x, y, z]
176    pub gyro_offset: [f32; 3],
177
178    /// Magnetometer offsets [x, y, z] (if present)
179    pub mag_offset: Option<[f32; 3]>,
180
181    /// Magnetometer radius (if present)
182    pub mag_radius: Option<f32>,
183
184    /// Raw calibration data (device-specific)
185    pub raw_data: Option<Vec<u8>>,
186}
187
188impl Default for CalibrationData {
189    fn default() -> Self {
190        Self {
191            timestamp: 0,
192            model: String::new(),
193            accel_offset: [0.0; 3],
194            accel_radius: None,
195            gyro_offset: [0.0; 3],
196            mag_offset: None,
197            mag_radius: None,
198            raw_data: None,
199        }
200    }
201}
202
203// ============================================================================
204// IMU Configuration
205// ============================================================================
206
207/// Common IMU configuration options
208#[derive(Debug, Clone, Serialize, Deserialize)]
209pub struct ImuConfig {
210    /// Update rate in Hz
211    pub update_rate_hz: f32,
212
213    /// Enable accelerometer
214    pub enable_accel: bool,
215
216    /// Enable gyroscope
217    pub enable_gyro: bool,
218
219    /// Enable magnetometer (if available)
220    pub enable_mag: bool,
221
222    /// Accelerometer range in g (e.g., 2, 4, 8, 16)
223    pub accel_range_g: Option<u8>,
224
225    /// Gyroscope range in degrees/sec (e.g., 250, 500, 1000, 2000)
226    pub gyro_range_dps: Option<u16>,
227
228    /// Low-pass filter cutoff frequency (Hz)
229    pub filter_cutoff_hz: Option<f32>,
230
231    /// Path to calibration file
232    pub calibration_file: Option<String>,
233}
234
235impl Default for ImuConfig {
236    fn default() -> Self {
237        Self {
238            update_rate_hz: 100.0,
239            enable_accel: true,
240            enable_gyro: true,
241            enable_mag: true,
242            accel_range_g: Some(4),
243            gyro_range_dps: Some(500),
244            filter_cutoff_hz: Some(10.0),
245            calibration_file: None,
246        }
247    }
248}
249
250// ============================================================================
251// Sensor Fusion Modes
252// ============================================================================
253
254/// Sensor fusion operation modes
255#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
256pub enum FusionMode {
257    /// Accelerometer only (tilt sensing)
258    AccelOnly,
259
260    /// Gyroscope only (rotation sensing)
261    GyroOnly,
262
263    /// Magnetometer only (compass heading)
264    MagOnly,
265
266    /// Accel + Gyro (6-DOF orientation without heading)
267    AccelGyro,
268
269    /// Accel + Mag (gravity + compass)
270    AccelMag,
271
272    /// Gyro + Mag (rotation + compass)
273    GyroMag,
274
275    /// Accel + Gyro + Mag (9-DOF absolute orientation)
276    FullFusion,
277
278    /// IMU mode (accel + gyro, no mag)
279    ImuMode,
280
281    /// Compass mode (accel + mag, no gyro)
282    CompassMode,
283
284    /// M4G mode (accel + gyro + mag with gyro for fast rotations)
285    M4G,
286
287    /// NDOF (Nine Degrees of Freedom - full sensor fusion)
288    NDOF,
289
290    /// NDOF with Fast Magnetometer Calibration
291    NDOFFastMagCal,
292}
293
294// ============================================================================
295// Orientation Representations
296// ============================================================================
297
298/// Quaternion orientation (w, x, y, z)
299#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
300pub struct Quaternion {
301    pub w: f32,
302    pub x: f32,
303    pub y: f32,
304    pub z: f32,
305}
306
307impl Default for Quaternion {
308    fn default() -> Self {
309        Self {
310            w: 1.0,
311            x: 0.0,
312            y: 0.0,
313            z: 0.0,
314        }
315    }
316}
317
318impl Quaternion {
319    /// Create identity quaternion
320    pub fn identity() -> Self {
321        Self::default()
322    }
323
324    /// Convert to Euler angles (roll, pitch, yaw) in radians
325    pub fn to_euler(&self) -> (f32, f32, f32) {
326        // Roll (x-axis rotation)
327        let sinr_cosp = 2.0 * (self.w * self.x + self.y * self.z);
328        let cosr_cosp = 1.0 - 2.0 * (self.x * self.x + self.y * self.y);
329        let roll = sinr_cosp.atan2(cosr_cosp);
330
331        // Pitch (y-axis rotation)
332        let sinp = 2.0 * (self.w * self.y - self.z * self.x);
333        let pitch = if sinp.abs() >= 1.0 {
334            std::f32::consts::FRAC_PI_2.copysign(sinp)
335        } else {
336            sinp.asin()
337        };
338
339        // Yaw (z-axis rotation)
340        let siny_cosp = 2.0 * (self.w * self.z + self.x * self.y);
341        let cosy_cosp = 1.0 - 2.0 * (self.y * self.y + self.z * self.z);
342        let yaw = siny_cosp.atan2(cosy_cosp);
343
344        (roll, pitch, yaw)
345    }
346}
347
348/// Euler angles (roll, pitch, yaw) in radians
349#[derive(Debug, Clone, Copy, Serialize, Deserialize)]
350pub struct EulerAngles {
351    pub roll: f32,
352    pub pitch: f32,
353    pub yaw: f32,
354}
355
356impl Default for EulerAngles {
357    fn default() -> Self {
358        Self {
359            roll: 0.0,
360            pitch: 0.0,
361            yaw: 0.0,
362        }
363    }
364}
365
366impl From<Quaternion> for EulerAngles {
367    fn from(q: Quaternion) -> Self {
368        let (roll, pitch, yaw) = q.to_euler();
369        Self { roll, pitch, yaw }
370    }
371}
mecha10_controllers/imu.rs

mecha10_controllers/
imu.rs
