vex_rt/
imu.rs

1//! # Inertial Sensor API.
2
3use core::fmt;
4
5use alloc::format;
6
7use crate::{
8    bindings,
9    error::{get_errno, Error},
10    rtos::DataSource,
11};
12
13/// A struct which represents a V5 smart port configured as a inertial sensor.
14pub struct InertialSensor {
15    port: u8,
16}
17
18impl InertialSensor {
19    /// Constructs a new inertial sensor.
20    ///
21    /// # Safety
22    ///
23    /// This function is unsafe because it allows the user to create multiple
24    /// mutable references to the same inertial sensor. You likely want to
25    /// implement [`Robot::new()`](crate::robot::Robot::new()) instead.
26    pub unsafe fn new(port: u8) -> InertialSensor {
27        InertialSensor { port }
28    }
29
30    /// Calibrate IMU.
31    ///
32    /// This calls the reset function from PROS.
33    /// This takes approximately 2 seconds, and is a non-blocking operation.
34    pub fn calibrate(&mut self) -> Result<(), InertialSensorError> {
35        match unsafe { bindings::imu_reset(self.port) } {
36            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
37            _ => Ok(()),
38        }
39    }
40
41    /// Get the total number of degrees the Inertial Sensor has spun about the
42    /// z-axis.
43    ///
44    /// This value is theoretically unbounded. Clockwise rotations are
45    /// represented with positive degree values, while counterclockwise
46    /// rotations are represented with negative ones.
47    pub fn get_rotation(&self) -> Result<f64, InertialSensorError> {
48        match unsafe { bindings::imu_get_rotation(self.port) } {
49            x if x == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
50            x => Ok(x),
51        }
52    }
53
54    /// Get the Inertial Sensor’s heading relative to the initial direction of
55    /// its x-axis.
56    ///
57    /// This value is bounded by [0,360). Clockwise rotations are represented
58    /// with positive degree values, while counterclockwise rotations are
59    /// represented with negative ones.
60    pub fn get_heading(&self) -> Result<f64, InertialSensorError> {
61        match unsafe { bindings::imu_get_heading(self.port) } {
62            x if x == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
63            x => Ok(x),
64        }
65    }
66
67    /// Get a quaternion representing the Inertial Sensor’s orientation.
68    pub fn get_quaternion(&self) -> Result<InertialSensorQuaternion, InertialSensorError> {
69        match unsafe { bindings::imu_get_quaternion(self.port) } {
70            x if x.x == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
71            x => Ok(InertialSensorQuaternion {
72                x: x.x,
73                y: x.y,
74                z: x.z,
75                w: x.w,
76            }),
77        }
78    }
79
80    /// Get the Euler angles representing the Inertial Sensor’s orientation.
81    pub fn get_euler(&self) -> Result<InertialSensorEuler, InertialSensorError> {
82        match unsafe { bindings::imu_get_euler(self.port) } {
83            x if x.pitch == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
84            x => Ok(InertialSensorEuler {
85                pitch: x.pitch,
86                roll: x.roll,
87                yaw: x.yaw,
88            }),
89        }
90    }
91
92    /// Get the Inertial Sensor’s pitch angle bounded by (-180,180).
93    pub fn get_pitch(&self) -> Result<f64, InertialSensorError> {
94        match unsafe { bindings::imu_get_pitch(self.port) } {
95            x if x == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
96            x => Ok(x),
97        }
98    }
99
100    /// Get the Inertial Sensor’s roll angle bounded by (-180,180).
101    pub fn get_roll(&self) -> Result<f64, InertialSensorError> {
102        match unsafe { bindings::imu_get_roll(self.port) } {
103            x if x == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
104            x => Ok(x),
105        }
106    }
107
108    /// Get the Inertial Sensor’s yaw angle bounded by (-180,180).
109    pub fn get_yaw(&self) -> Result<f64, InertialSensorError> {
110        match unsafe { bindings::imu_get_yaw(self.port) } {
111            x if x == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
112            x => Ok(x),
113        }
114    }
115
116    /// Get the Inertial Sensor’s raw gyroscope values.
117    pub fn get_gyro_rate(&self) -> Result<InertialSensorRaw, InertialSensorError> {
118        match unsafe { bindings::imu_get_gyro_rate(self.port) } {
119            x if x.x == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
120            x => Ok(InertialSensorRaw {
121                x: x.x,
122                y: x.y,
123                z: x.z,
124            }),
125        }
126    }
127
128    /// Get the Inertial Sensor’s raw accelerometer values.
129    pub fn get_accel(&self) -> Result<InertialSensorRaw, InertialSensorError> {
130        match unsafe { bindings::imu_get_accel(self.port) } {
131            x if x.x == bindings::PROS_ERR_F_ => Err(InertialSensorError::from_errno()),
132            x => Ok(InertialSensorRaw {
133                x: x.x,
134                y: x.y,
135                z: x.z,
136            }),
137        }
138    }
139
140    /// Get the Inertial Sensor’s status.
141    pub fn get_status(&self) -> Result<InertialSensorStatus, InertialSensorError> {
142        let status = unsafe { bindings::imu_get_status(self.port) };
143
144        if status == bindings::imu_status_e_E_IMU_STATUS_ERROR {
145            Err(InertialSensorError::from_errno())
146        } else {
147            Ok(InertialSensorStatus(status))
148        }
149    }
150
151    /// Resets the current reading of the Inertial Sensor’s rotation to zero.
152    pub fn reset_heading(&mut self) -> Result<(), InertialSensorError> {
153        match unsafe { bindings::imu_tare_heading(self.port) } {
154            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
155            _ => Ok(()),
156        }
157    }
158
159    /// Resets the current reading of the Inertial Sensor’s rotation to zero.
160    pub fn reset_rotation(&mut self) -> Result<(), InertialSensorError> {
161        match unsafe { bindings::imu_tare_rotation(self.port) } {
162            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
163            _ => Ok(()),
164        }
165    }
166
167    /// Resets the current reading of the Inertial Sensor’s pitch to zero.
168    pub fn reset_pitch(&mut self) -> Result<(), InertialSensorError> {
169        match unsafe { bindings::imu_tare_pitch(self.port) } {
170            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
171            _ => Ok(()),
172        }
173    }
174
175    /// Resets the current reading of the Inertial Sensor’s roll to zero.
176    pub fn reset_roll(&mut self) -> Result<(), InertialSensorError> {
177        match unsafe { bindings::imu_tare_roll(self.port) } {
178            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
179            _ => Ok(()),
180        }
181    }
182
183    /// Resets the current reading of the Inertial Sensor’s yaw to zero.
184    pub fn reset_yaw(&mut self) -> Result<(), InertialSensorError> {
185        match unsafe { bindings::imu_tare_yaw(self.port) } {
186            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
187            _ => Ok(()),
188        }
189    }
190
191    /// Reset all 3 euler values of the Inertial Sensor to 0.
192    pub fn reset_euler(&mut self) -> Result<(), InertialSensorError> {
193        match unsafe { bindings::imu_tare_euler(self.port) } {
194            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
195            _ => Ok(()),
196        }
197    }
198
199    /// Resets all 5 values of the Inertial Sensor to 0.
200    pub fn reset(&mut self) -> Result<(), InertialSensorError> {
201        match unsafe { bindings::imu_tare(self.port) } {
202            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
203            _ => Ok(()),
204        }
205    }
206
207    /// Sets the current reading of the Inertial Sensor’s euler values to target
208    /// euler values. Will default to +/- 180 if target exceeds +/- 180.
209    pub fn set_zero_euler(
210        &mut self,
211        euler: InertialSensorEuler,
212    ) -> Result<(), InertialSensorError> {
213        match unsafe {
214            bindings::imu_set_euler(
215                self.port,
216                bindings::euler_s_t {
217                    pitch: euler.pitch,
218                    roll: euler.roll,
219                    yaw: euler.yaw,
220                },
221            )
222        } {
223            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
224            _ => Ok(()),
225        }
226    }
227
228    /// Sets the current reading of the Inertial Sensor’s rotation to target
229    /// value.
230    pub fn set_rotation(&mut self, rotation: f64) -> Result<(), InertialSensorError> {
231        match unsafe { bindings::imu_set_rotation(self.port, rotation) } {
232            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
233            _ => Ok(()),
234        }
235    }
236
237    /// Sets the current reading of the Inertial Sensor’s heading to target
238    /// value Target will default to 360 if above 360 and default to 0 if below
239    /// 0.
240    pub fn set_heading(&mut self, heading: f64) -> Result<(), InertialSensorError> {
241        match unsafe { bindings::imu_set_heading(self.port, heading) } {
242            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
243            _ => Ok(()),
244        }
245    }
246
247    /// Sets the current reading of the Inertial Sensor’s pitch to target value
248    /// Will default to +/- 180 if target exceeds +/- 180.
249    pub fn set_pitch(&mut self, pitch: f64) -> Result<(), InertialSensorError> {
250        match unsafe { bindings::imu_set_pitch(self.port, pitch) } {
251            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
252            _ => Ok(()),
253        }
254    }
255
256    /// Sets the current reading of the Inertial Sensor’s roll to target value
257    /// Will default to +/- 180 if target exceeds +/- 180.
258    pub fn set_roll(&mut self, roll: f64) -> Result<(), InertialSensorError> {
259        match unsafe { bindings::imu_set_roll(self.port, roll) } {
260            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
261            _ => Ok(()),
262        }
263    }
264
265    /// Sets the current reading of the Inertial Sensor’s yaw to target value
266    /// Will default to +/- 180 if target exceeds +/- 180.
267    pub fn set_yaw(&mut self, yaw: f64) -> Result<(), InertialSensorError> {
268        match unsafe { bindings::imu_set_yaw(self.port, yaw) } {
269            bindings::PROS_ERR_ => Err(InertialSensorError::from_errno()),
270            _ => Ok(()),
271        }
272    }
273}
274
275impl DataSource for InertialSensor {
276    type Data = InertialSensorData;
277
278    type Error = InertialSensorError;
279
280    fn read(&self) -> Result<Self::Data, Self::Error> {
281        Ok(InertialSensorData {
282            status: self.get_status()?,
283            quaternion: self.get_quaternion()?,
284            euler: self.get_euler()?,
285            gyro_rate: self.get_gyro_rate()?,
286            accel: self.get_accel()?,
287        })
288    }
289}
290
291/// Represents the data that can be read from an inertial sensor.
292#[derive(Clone, Copy, Debug, PartialEq)]
293pub struct InertialSensorData {
294    /// The status of the inertial sensor.
295    pub status: InertialSensorStatus,
296    /// The quaternion representing the rotation of the inertial sensor.
297    pub quaternion: InertialSensorQuaternion,
298    /// The Euler angles representing the rotation of the inertial sensor.
299    pub euler: InertialSensorEuler,
300    /// The raw gyroscope values of the inertial sensor.
301    pub gyro_rate: InertialSensorRaw,
302    /// The raw accelerometer values of the inertial sensor.
303    pub accel: InertialSensorRaw,
304}
305
306/// Represents possible errors for inertial sensor operations.
307#[derive(Debug)]
308pub enum InertialSensorError {
309    /// Port is out of range (1-21).
310    PortOutOfRange,
311    /// Port cannot be configured as a inertial sensor.
312    PortNotInertialSensor,
313    /// The sensor is already calibrating.
314    SensorAlreadyCalibrating,
315    /// The sensor returned an unknown status code.
316    UnknownStatusCode(u32),
317    /// Unknown error.
318    Unknown(i32),
319}
320
321impl InertialSensorError {
322    fn from_errno() -> Self {
323        match get_errno() {
324            libc::ENXIO => Self::PortOutOfRange,
325            libc::ENODEV => Self::PortNotInertialSensor,
326            libc::EAGAIN => Self::SensorAlreadyCalibrating,
327            x => Self::Unknown(x),
328        }
329    }
330}
331
332impl From<InertialSensorError> for Error {
333    fn from(err: InertialSensorError) -> Self {
334        match err {
335            InertialSensorError::PortOutOfRange => Error::Custom("port out of range".into()),
336            InertialSensorError::PortNotInertialSensor => {
337                Error::Custom("port not a inertial sensor".into())
338            }
339            InertialSensorError::SensorAlreadyCalibrating => {
340                Error::Custom("sensor already calibrating".into())
341            }
342            InertialSensorError::UnknownStatusCode(n) => {
343                Error::Custom(format!("sensor returned unknown status code {}", n))
344            }
345            InertialSensorError::Unknown(n) => Error::System(n),
346        }
347    }
348}
349
350/// Represents raw values returned from an inertial sensor.
351#[derive(Clone, Copy, Debug, PartialEq)]
352pub struct InertialSensorRaw {
353    /// The raw x value returned from the inertial sensor.
354    pub x: f64,
355    /// The raw y value returned from the inertial sensor.
356    pub y: f64,
357    /// The raw z value returned from the inertial sensor.
358    pub z: f64,
359}
360
361/// Represents a Quaternion returned from an inertial sensor.
362#[derive(Clone, Copy, Debug, PartialEq)]
363pub struct InertialSensorQuaternion {
364    /// The x value of the Quaternion.
365    pub x: f64,
366    /// The y value of the Quaternion.
367    pub y: f64,
368    /// The z value of the Quaternion.
369    pub z: f64,
370    /// The w value of the Quaternion.
371    pub w: f64,
372}
373
374/// Represents the set of euler angles returned from an inertial sensor.
375#[derive(Clone, Copy, Debug, PartialEq)]
376pub struct InertialSensorEuler {
377    /// The counterclockwise rotation on the y axis.
378    pub pitch: f64,
379    /// The counterclockwise rotation on the x axis.
380    pub roll: f64,
381    /// The counterclockwise rotation on the z axis.
382    pub yaw: f64,
383}
384
385#[derive(Clone, Copy, PartialEq, Eq)]
386/// Indicates IMU status.
387pub struct InertialSensorStatus(bindings::imu_status_e);
388
389impl InertialSensorStatus {
390    #[inline]
391    /// Gets the raw status value.
392    pub fn into_raw(self) -> bindings::imu_status_e {
393        self.0
394    }
395
396    #[inline]
397    /// Checks whether the status value indicates that the IMU is calibrating.
398    pub fn is_calibrating(self) -> bool {
399        self.0 & bindings::imu_status_e_E_IMU_STATUS_CALIBRATING != 0
400    }
401}
402
403impl fmt::Debug for InertialSensorStatus {
404    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
405        f.debug_struct("InertialSensorStatus")
406            .field("is_calibrating", &self.is_calibrating())
407            .finish()
408    }
409}
vex_rt/imu.rs

vex_rt/
imu.rs
