cu_sensor_payloads/
imu.rs

1use bincode::{Decode, Encode};
2use cu29::prelude::*;
3use cu29::units::si::acceleration::meter_per_second_squared;
4use cu29::units::si::angular_velocity::radian_per_second;
5use cu29::units::si::f32::{
6    Acceleration, AngularVelocity, MagneticFluxDensity, ThermodynamicTemperature,
7};
8use cu29::units::si::magnetic_flux_density::microtesla;
9use cu29::units::si::thermodynamic_temperature::degree_celsius;
10use serde::{Deserialize, Serialize};
11
12/// Standardized IMU payload carrying acceleration, angular velocity, and optional magnetometer data.
13#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect)]
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_units(
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
83/// Magnetometer payload split from the main IMU data for composition.
84#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize, Encode, Decode)]
85pub struct MagnetometerPayload {
86    pub mag_x: MagneticFluxDensity,
87    pub mag_y: MagneticFluxDensity,
88    pub mag_z: MagneticFluxDensity,
89}
90
91impl Default for MagnetometerPayload {
92    fn default() -> Self {
93        Self {
94            mag_x: MagneticFluxDensity::new::<microtesla>(0.0),
95            mag_y: MagneticFluxDensity::new::<microtesla>(0.0),
96            mag_z: MagneticFluxDensity::new::<microtesla>(0.0),
97        }
98    }
99}
100
101impl MagnetometerPayload {
102    /// Build a magnetometer payload from raw microtesla values.
103    pub fn from_raw(mag_ut: [f32; 3]) -> Self {
104        let [mag_x, mag_y, mag_z] = mag_ut.map(MagneticFluxDensity::new::<microtesla>);
105        Self {
106            mag_x,
107            mag_y,
108            mag_z,
109        }
110    }
111
112    /// Build a magnetometer payload from unit-carrying types.
113    pub fn from_units(
114        mag_x: MagneticFluxDensity,
115        mag_y: MagneticFluxDensity,
116        mag_z: MagneticFluxDensity,
117    ) -> Self {
118        Self {
119            mag_x,
120            mag_y,
121            mag_z,
122        }
123    }
124}
125
126/// Combined payload allowing optional magnetometer data.
127#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize, Encode, Decode)]
128pub struct ImuWithMagPayload {
129    pub imu: ImuPayload,
130    pub mag: Option<MagnetometerPayload>,
131}
132
133impl ImuWithMagPayload {
134    pub fn new(imu: ImuPayload, mag: Option<MagnetometerPayload>) -> Self {
135        Self { imu, mag }
136    }
137}
138
139#[cfg(test)]
140mod tests {
141    use super::*;
142    use bincode::config;
143
144    #[test]
145    fn round_trip_encode_decode() {
146        let payload = ImuPayload::from_raw([9.8, -2.0, 0.5], [0.01, -0.02, 0.5], 36.5);
147
148        let cfg = config::standard();
149        let mut buffer = [0u8; 128];
150        let len = bincode::encode_into_slice(payload, &mut buffer, cfg).unwrap();
151        let (decoded, used) =
152            bincode::decode_from_slice::<ImuPayload, _>(&buffer[..len], cfg).unwrap();
153
154        assert_eq!(used, len);
155        assert_eq!(decoded.accel_x.value, payload.accel_x.value);
156        assert_eq!(decoded.gyro_y.value, payload.gyro_y.value);
157        assert_eq!(
158            decoded.temperature.get::<degree_celsius>(),
159            payload.temperature.get::<degree_celsius>()
160        );
161    }
162
163    #[test]
164    fn builds_from_units() {
165        let accel = Acceleration::new::<meter_per_second_squared>(9.81);
166        let gyro = AngularVelocity::new::<radian_per_second>(0.25);
167        let temp = ThermodynamicTemperature::new::<degree_celsius>(20.0);
168
169        let payload = ImuPayload::from_units(accel, accel, accel, gyro, gyro, gyro, temp);
170
171        assert_eq!(payload.accel_x.value, accel.value);
172        assert_eq!(payload.gyro_z.value, gyro.value);
173    }
174
175    #[test]
176    fn magnetometer_round_trip() {
177        let mag_payload = MagnetometerPayload::from_raw([42.0, -13.0, 8.0]);
178        let cfg = config::standard();
179        let mut buffer = [0u8; 128];
180        let len = bincode::encode_into_slice(mag_payload, &mut buffer, cfg).unwrap();
181        let (decoded, used) =
182            bincode::decode_from_slice::<MagnetometerPayload, _>(&buffer[..len], cfg).unwrap();
183
184        assert_eq!(used, len);
185        assert_eq!(decoded.mag_x.value, mag_payload.mag_x.value);
186        assert_eq!(decoded.mag_z.value, mag_payload.mag_z.value);
187    }
188
189    #[test]
190    fn combined_payload_handles_optional_mag() {
191        let imu = ImuPayload::from_raw([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], 22.0);
192        let mag = MagnetometerPayload::from_raw([7.0, 8.0, 9.0]);
193        let combined = ImuWithMagPayload::new(imu, Some(mag));
194
195        let cfg = config::standard();
196        let mut buffer = [0u8; 256];
197        let len = bincode::encode_into_slice(combined, &mut buffer, cfg).unwrap();
198        let (decoded, used) =
199            bincode::decode_from_slice::<ImuWithMagPayload, _>(&buffer[..len], cfg).unwrap();
200
201        assert_eq!(used, len);
202        assert_eq!(decoded.imu.accel_y.value, imu.accel_y.value);
203        assert_eq!(decoded.mag.unwrap().mag_y.value, mag.mag_y.value);
204    }
205}
cu_sensor_payloads/imu.rs

cu_sensor_payloads/
imu.rs
