cu_sensor_payloads/
imu.rs

1use bincode::de::Decoder;
2use bincode::enc::Encoder;
3use bincode::error::{DecodeError, EncodeError};
4use bincode::{Decode, Encode};
5use serde::{Deserialize, Serialize};
6use uom::si::acceleration::meter_per_second_squared;
7use uom::si::angular_velocity::radian_per_second;
8use uom::si::f32::{Acceleration, AngularVelocity, MagneticFluxDensity, ThermodynamicTemperature};
9use uom::si::magnetic_flux_density::microtesla;
10use uom::si::thermodynamic_temperature::degree_celsius;
11
12/// Standardized IMU payload carrying acceleration, angular velocity, and optional magnetometer data.
13#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
14pub struct ImuPayload {
15    pub accel_x: Acceleration,
16    pub accel_y: Acceleration,
17    pub accel_z: Acceleration,
18    pub gyro_x: AngularVelocity,
19    pub gyro_y: AngularVelocity,
20    pub gyro_z: AngularVelocity,
21    pub temperature: ThermodynamicTemperature,
22}
23
24impl Default for ImuPayload {
25    fn default() -> Self {
26        Self {
27            accel_x: Acceleration::new::<meter_per_second_squared>(0.0),
28            accel_y: Acceleration::new::<meter_per_second_squared>(0.0),
29            accel_z: Acceleration::new::<meter_per_second_squared>(0.0),
30            gyro_x: AngularVelocity::new::<radian_per_second>(0.0),
31            gyro_y: AngularVelocity::new::<radian_per_second>(0.0),
32            gyro_z: AngularVelocity::new::<radian_per_second>(0.0),
33            temperature: ThermodynamicTemperature::new::<degree_celsius>(0.0),
34        }
35    }
36}
37
38impl ImuPayload {
39    /// Build an IMU payload from plain scalar values.
40    ///
41    /// * `accel_mps2` - acceleration in m/s².
42    /// * `gyro_rad` - angular velocity in rad/s.
43    /// * `temperature_c` - temperature in °C.
44    pub fn from_raw(accel_mps2: [f32; 3], gyro_rad: [f32; 3], temperature_c: f32) -> Self {
45        let [accel_x, accel_y, accel_z] =
46            accel_mps2.map(Acceleration::new::<meter_per_second_squared>);
47        let [gyro_x, gyro_y, gyro_z] = gyro_rad.map(AngularVelocity::new::<radian_per_second>);
48        let temperature = ThermodynamicTemperature::new::<degree_celsius>(temperature_c);
49
50        Self {
51            accel_x,
52            accel_y,
53            accel_z,
54            gyro_x,
55            gyro_y,
56            gyro_z,
57            temperature,
58        }
59    }
60
61    /// Build an IMU payload from unit-carrying types.
62    pub fn from_uom(
63        accel_x: Acceleration,
64        accel_y: Acceleration,
65        accel_z: Acceleration,
66        gyro_x: AngularVelocity,
67        gyro_y: AngularVelocity,
68        gyro_z: AngularVelocity,
69        temperature: ThermodynamicTemperature,
70    ) -> Self {
71        Self {
72            accel_x,
73            accel_y,
74            accel_z,
75            gyro_x,
76            gyro_y,
77            gyro_z,
78            temperature,
79        }
80    }
81}
82
83impl Encode for ImuPayload {
84    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
85        Encode::encode(&self.accel_x.value, encoder)?;
86        Encode::encode(&self.accel_y.value, encoder)?;
87        Encode::encode(&self.accel_z.value, encoder)?;
88        Encode::encode(&self.gyro_x.value, encoder)?;
89        Encode::encode(&self.gyro_y.value, encoder)?;
90        Encode::encode(&self.gyro_z.value, encoder)?;
91        Encode::encode(&self.temperature.get::<degree_celsius>(), encoder)?;
92        Ok(())
93    }
94}
95
96impl Decode<()> for ImuPayload {
97    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
98        let accel_x = Acceleration::new::<meter_per_second_squared>(Decode::decode(decoder)?);
99        let accel_y = Acceleration::new::<meter_per_second_squared>(Decode::decode(decoder)?);
100        let accel_z = Acceleration::new::<meter_per_second_squared>(Decode::decode(decoder)?);
101
102        let gyro_x = AngularVelocity::new::<radian_per_second>(Decode::decode(decoder)?);
103        let gyro_y = AngularVelocity::new::<radian_per_second>(Decode::decode(decoder)?);
104        let gyro_z = AngularVelocity::new::<radian_per_second>(Decode::decode(decoder)?);
105
106        let temperature = ThermodynamicTemperature::new::<degree_celsius>(Decode::decode(decoder)?);
107
108        Ok(Self {
109            accel_x,
110            accel_y,
111            accel_z,
112            gyro_x,
113            gyro_y,
114            gyro_z,
115            temperature,
116        })
117    }
118}
119
120/// Magnetometer payload split from the main IMU data for composition.
121#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
122pub struct MagnetometerPayload {
123    pub mag_x: MagneticFluxDensity,
124    pub mag_y: MagneticFluxDensity,
125    pub mag_z: MagneticFluxDensity,
126}
127
128impl Default for MagnetometerPayload {
129    fn default() -> Self {
130        Self {
131            mag_x: MagneticFluxDensity::new::<microtesla>(0.0),
132            mag_y: MagneticFluxDensity::new::<microtesla>(0.0),
133            mag_z: MagneticFluxDensity::new::<microtesla>(0.0),
134        }
135    }
136}
137
138impl MagnetometerPayload {
139    /// Build a magnetometer payload from raw microtesla values.
140    pub fn from_raw(mag_ut: [f32; 3]) -> Self {
141        let [mag_x, mag_y, mag_z] = mag_ut.map(MagneticFluxDensity::new::<microtesla>);
142        Self {
143            mag_x,
144            mag_y,
145            mag_z,
146        }
147    }
148
149    /// Build a magnetometer payload from unit-carrying types.
150    pub fn from_uom(
151        mag_x: MagneticFluxDensity,
152        mag_y: MagneticFluxDensity,
153        mag_z: MagneticFluxDensity,
154    ) -> Self {
155        Self {
156            mag_x,
157            mag_y,
158            mag_z,
159        }
160    }
161}
162
163impl Encode for MagnetometerPayload {
164    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
165        Encode::encode(&self.mag_x.get::<microtesla>(), encoder)?;
166        Encode::encode(&self.mag_y.get::<microtesla>(), encoder)?;
167        Encode::encode(&self.mag_z.get::<microtesla>(), encoder)?;
168        Ok(())
169    }
170}
171
172impl Decode<()> for MagnetometerPayload {
173    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
174        let mag_x = MagneticFluxDensity::new::<microtesla>(Decode::decode(decoder)?);
175        let mag_y = MagneticFluxDensity::new::<microtesla>(Decode::decode(decoder)?);
176        let mag_z = MagneticFluxDensity::new::<microtesla>(Decode::decode(decoder)?);
177
178        Ok(Self {
179            mag_x,
180            mag_y,
181            mag_z,
182        })
183    }
184}
185
186/// Combined payload allowing optional magnetometer data.
187#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
188pub struct ImuWithMagPayload {
189    pub imu: ImuPayload,
190    pub mag: Option<MagnetometerPayload>,
191}
192
193impl ImuWithMagPayload {
194    pub fn new(imu: ImuPayload, mag: Option<MagnetometerPayload>) -> Self {
195        Self { imu, mag }
196    }
197}
198
199impl Encode for ImuWithMagPayload {
200    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
201        Encode::encode(&self.imu, encoder)?;
202        Encode::encode(&self.mag.is_some(), encoder)?;
203        if let Some(mag) = self.mag {
204            Encode::encode(&mag, encoder)?;
205        }
206        Ok(())
207    }
208}
209
210impl Decode<()> for ImuWithMagPayload {
211    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
212        let imu = ImuPayload::decode(decoder)?;
213        let has_mag: bool = Decode::decode(decoder)?;
214        let mag = if has_mag {
215            Some(MagnetometerPayload::decode(decoder)?)
216        } else {
217            None
218        };
219
220        Ok(Self { imu, mag })
221    }
222}
223
224#[cfg(test)]
225mod tests {
226    use super::*;
227    use bincode::config;
228
229    #[test]
230    fn round_trip_encode_decode() {
231        let payload = ImuPayload::from_raw([9.8, -2.0, 0.5], [0.01, -0.02, 0.5], 36.5);
232
233        let cfg = config::standard();
234        let mut buffer = [0u8; 128];
235        let len = bincode::encode_into_slice(payload, &mut buffer, cfg).unwrap();
236        let (decoded, used) =
237            bincode::decode_from_slice::<ImuPayload, _>(&buffer[..len], cfg).unwrap();
238
239        assert_eq!(used, len);
240        assert_eq!(decoded.accel_x.value, payload.accel_x.value);
241        assert_eq!(decoded.gyro_y.value, payload.gyro_y.value);
242        assert_eq!(
243            decoded.temperature.get::<degree_celsius>(),
244            payload.temperature.get::<degree_celsius>()
245        );
246    }
247
248    #[test]
249    fn builds_from_units() {
250        let accel = Acceleration::new::<meter_per_second_squared>(9.81);
251        let gyro = AngularVelocity::new::<radian_per_second>(0.25);
252        let temp = ThermodynamicTemperature::new::<degree_celsius>(20.0);
253
254        let payload = ImuPayload::from_uom(accel, accel, accel, gyro, gyro, gyro, temp);
255
256        assert_eq!(payload.accel_x.value, accel.value);
257        assert_eq!(payload.gyro_z.value, gyro.value);
258    }
259
260    #[test]
261    fn magnetometer_round_trip() {
262        let mag_payload = MagnetometerPayload::from_raw([42.0, -13.0, 8.0]);
263        let cfg = config::standard();
264        let mut buffer = [0u8; 128];
265        let len = bincode::encode_into_slice(mag_payload, &mut buffer, cfg).unwrap();
266        let (decoded, used) =
267            bincode::decode_from_slice::<MagnetometerPayload, _>(&buffer[..len], cfg).unwrap();
268
269        assert_eq!(used, len);
270        assert_eq!(decoded.mag_x.value, mag_payload.mag_x.value);
271        assert_eq!(decoded.mag_z.value, mag_payload.mag_z.value);
272    }
273
274    #[test]
275    fn combined_payload_handles_optional_mag() {
276        let imu = ImuPayload::from_raw([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], 22.0);
277        let mag = MagnetometerPayload::from_raw([7.0, 8.0, 9.0]);
278        let combined = ImuWithMagPayload::new(imu, Some(mag));
279
280        let cfg = config::standard();
281        let mut buffer = [0u8; 256];
282        let len = bincode::encode_into_slice(combined, &mut buffer, cfg).unwrap();
283        let (decoded, used) =
284            bincode::decode_from_slice::<ImuWithMagPayload, _>(&buffer[..len], cfg).unwrap();
285
286        assert_eq!(used, len);
287        assert_eq!(decoded.imu.accel_y.value, imu.accel_y.value);
288        assert_eq!(decoded.mag.unwrap().mag_y.value, mag.mag_y.value);
289    }
290}
cu_sensor_payloads/imu.rs

cu_sensor_payloads/
imu.rs
