1use fusion_imu_sys as sys;
2
3#[allow(missing_docs)]
5#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
6#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
7#[cfg_attr(feature = "defmt-03", derive(defmt::Format))]
8#[repr(C)]
9pub struct Vector {
10 pub x: f32,
11 pub y: f32,
12 pub z: f32,
13}
14
15impl Vector {
16 pub fn new(x: f32, y: f32, z: f32) -> Self {
18 Self { x, y, z }
19 }
20}
21
22impl From<sys::FusionVector> for Vector {
23 fn from(value: sys::FusionVector) -> Self {
24 let values: sys::FusionVector__bindgen_ty_1 = unsafe { value.axis };
25 Self {
26 x: values.x,
27 y: values.y,
28 z: values.z,
29 }
30 }
31}
32
33impl From<Vector> for sys::FusionVector {
34 fn from(value: Vector) -> Self {
35 sys::FusionVector {
36 array: [value.x, value.y, value.z],
37 }
38 }
39}
40
41#[allow(missing_docs)]
43#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
44#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
45#[cfg_attr(feature = "defmt-03", derive(defmt::Format))]
46#[repr(C)]
47pub struct Quaternion {
48 pub w: f32,
49 pub x: f32,
50 pub y: f32,
51 pub z: f32,
52}
53
54impl Quaternion {
55 pub fn to_euler(self) -> Euler {
57 unsafe { sys::FusionQuaternionToEuler(self.into()).into() }
58 }
59}
60
61impl From<sys::FusionQuaternion> for Quaternion {
62 fn from(value: sys::FusionQuaternion) -> Self {
63 let values: sys::FusionQuaternion__bindgen_ty_1 = unsafe { value.element };
64 Self {
65 w: values.w,
66 x: values.x,
67 y: values.y,
68 z: values.z,
69 }
70 }
71}
72
73impl From<Quaternion> for sys::FusionQuaternion {
74 fn from(value: Quaternion) -> Self {
75 sys::FusionQuaternion {
76 array: [value.w, value.x, value.y, value.z],
77 }
78 }
79}
80
81#[allow(missing_docs)]
85#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
86#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
87#[cfg_attr(feature = "defmt-03", derive(defmt::Format))]
88#[repr(C)]
89pub struct Matrix {
90 pub xx: f32,
91 pub xy: f32,
92 pub xz: f32,
93 pub yx: f32,
94 pub yy: f32,
95 pub yz: f32,
96 pub zx: f32,
97 pub zy: f32,
98 pub zz: f32,
99}
100
101impl From<sys::FusionMatrix> for Matrix {
102 fn from(value: sys::FusionMatrix) -> Self {
103 let values: sys::FusionMatrix__bindgen_ty_1 = unsafe { value.element };
104 Self {
105 xx: values.xx,
106 xy: values.xy,
107 xz: values.xz,
108 yx: values.yx,
109 yy: values.yy,
110 yz: values.yz,
111 zx: values.zx,
112 zy: values.zy,
113 zz: values.zz,
114 }
115 }
116}
117
118impl From<Matrix> for sys::FusionMatrix {
119 fn from(value: Matrix) -> Self {
120 sys::FusionMatrix {
121 array: [
122 [value.xx, value.xy, value.xz],
123 [value.yx, value.yy, value.yz],
124 [value.zx, value.zy, value.zz],
125 ],
126 }
127 }
128}
129
130#[allow(missing_docs)]
135#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
136#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
137#[cfg_attr(feature = "defmt-03", derive(defmt::Format))]
138#[repr(C)]
139pub struct Euler {
140 pub roll: f32,
141 pub pitch: f32,
142 pub yaw: f32,
143}
144
145impl From<sys::FusionEuler> for Euler {
146 fn from(value: sys::FusionEuler) -> Self {
147 let values: sys::FusionEuler__bindgen_ty_1 = unsafe { value.angle };
148 Self {
149 roll: values.roll,
150 pitch: values.pitch,
151 yaw: values.yaw,
152 }
153 }
154}
155
156impl From<Euler> for sys::FusionEuler {
157 fn from(value: Euler) -> Self {
158 sys::FusionEuler {
159 array: [value.roll, value.pitch, value.yaw],
160 }
161 }
162}
163
164#[allow(missing_docs)]
166#[derive(Debug, Clone, Copy, Default, PartialEq, PartialOrd)]
167#[repr(C)]
168pub enum Convention {
169 #[default]
170 NorthWestUp,
171 EastNorthUp,
172 NorthWestDown,
173}
174
175#[cfg(test)]
176mod tests {
177 use super::*;
178
179 #[test]
180 fn vector_maps_from_sys_array() {
181 let sys_vector = sys::FusionVector {
182 array: [1.0, 2.0, 3.0],
183 };
184
185 let vector = Vector::from(sys_vector);
187
188 assert_eq!(
189 vector,
190 Vector {
191 x: 1.0,
192 y: 2.0,
193 z: 3.0,
194 }
195 );
196 }
197
198 #[test]
199 fn vector_maps_from_sys_axis() {
200 let sys_vector = sys::FusionVector {
201 axis: sys::FusionVector__bindgen_ty_1 {
202 x: 1.0,
203 y: 2.0,
204 z: 3.0,
205 },
206 };
207
208 let vector = Vector::from(sys_vector);
210
211 assert_eq!(
212 vector,
213 Vector {
214 x: 1.0,
215 y: 2.0,
216 z: 3.0,
217 }
218 );
219 }
220
221 #[test]
222 fn vector_maps_to_sys_array() {
223 let vector = Vector {
224 x: 1.0,
225 y: 2.0,
226 z: 3.0,
227 };
228
229 let sys_vector = sys::FusionVector::from(vector);
231
232 let values = unsafe { sys_vector.array };
233 assert_eq!(values, [1.0, 2.0, 3.0]);
234 }
235
236 #[test]
237 fn vector_maps_to_sys_axis() {
238 let vector = Vector {
239 x: 1.0,
240 y: 2.0,
241 z: 3.0,
242 };
243
244 let sys_vector = sys::FusionVector::from(vector);
246
247 let values = unsafe { sys_vector.axis };
248 assert_eq!(values.x, 1.0);
249 assert_eq!(values.y, 2.0);
250 assert_eq!(values.z, 3.0);
251 }
252
253 #[test]
254 fn quaternion_maps_from_sys_array() {
255 let sys_quaternion = sys::FusionQuaternion {
256 array: [1.0, 2.0, 3.0, 4.0],
257 };
258
259 let quaternion = Quaternion::from(sys_quaternion);
261
262 assert_eq!(
263 quaternion,
264 Quaternion {
265 w: 1.0,
266 x: 2.0,
267 y: 3.0,
268 z: 4.0,
269 }
270 );
271 }
272
273 #[test]
274 fn quaternion_maps_from_sys_element() {
275 let sys_quaternion = sys::FusionQuaternion {
276 element: sys::FusionQuaternion__bindgen_ty_1 {
277 w: 1.0,
278 x: 2.0,
279 y: 3.0,
280 z: 4.0,
281 },
282 };
283
284 let quaternion = Quaternion::from(sys_quaternion);
286
287 assert_eq!(
288 quaternion,
289 Quaternion {
290 w: 1.0,
291 x: 2.0,
292 y: 3.0,
293 z: 4.0,
294 }
295 );
296 }
297
298 #[test]
299 fn quaternion_maps_to_sys_array() {
300 let quaternion = Quaternion {
301 w: 1.0,
302 x: 2.0,
303 y: 3.0,
304 z: 4.0,
305 };
306
307 let sys_quaternion = sys::FusionQuaternion::from(quaternion);
309
310 let values = unsafe { sys_quaternion.array };
311 assert_eq!(values, [1.0, 2.0, 3.0, 4.0]);
312 }
313
314 #[test]
315 fn quaternion_maps_to_sys_element() {
316 let quaternion = Quaternion {
317 w: 1.0,
318 x: 2.0,
319 y: 3.0,
320 z: 4.0,
321 };
322
323 let sys_quaternion = sys::FusionQuaternion::from(quaternion);
325
326 let values = unsafe { sys_quaternion.element };
327 assert_eq!(values.w, 1.0);
328 assert_eq!(values.x, 2.0);
329 assert_eq!(values.y, 3.0);
330 assert_eq!(values.z, 4.0);
331 }
332
333 #[test]
334 fn matrix_maps_from_sys_array() {
335 let sys_matrix = sys::FusionMatrix {
336 array: [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]],
337 };
338
339 let matrix = Matrix::from(sys_matrix);
341
342 assert_eq!(
343 matrix,
344 Matrix {
345 xx: 1.0,
346 xy: 2.0,
347 xz: 3.0,
348 yx: 4.0,
349 yy: 5.0,
350 yz: 6.0,
351 zx: 7.0,
352 zy: 8.0,
353 zz: 9.0,
354 }
355 );
356 }
357
358 #[test]
359 fn matrix_maps_from_sys_element() {
360 let sys_matrix = sys::FusionMatrix {
361 element: sys::FusionMatrix__bindgen_ty_1 {
362 xx: 1.0,
363 xy: 2.0,
364 xz: 3.0,
365 yx: 4.0,
366 yy: 5.0,
367 yz: 6.0,
368 zx: 7.0,
369 zy: 8.0,
370 zz: 9.0,
371 },
372 };
373
374 let matrix = Matrix::from(sys_matrix);
376
377 assert_eq!(
378 matrix,
379 Matrix {
380 xx: 1.0,
381 xy: 2.0,
382 xz: 3.0,
383 yx: 4.0,
384 yy: 5.0,
385 yz: 6.0,
386 zx: 7.0,
387 zy: 8.0,
388 zz: 9.0,
389 }
390 );
391 }
392
393 #[test]
394 fn matrix_maps_to_sys_array() {
395 let matrix = Matrix {
396 xx: 1.0,
397 xy: 2.0,
398 xz: 3.0,
399 yx: 4.0,
400 yy: 5.0,
401 yz: 6.0,
402 zx: 7.0,
403 zy: 8.0,
404 zz: 9.0,
405 };
406
407 let sys_matrix = sys::FusionMatrix::from(matrix);
409
410 let values = unsafe { sys_matrix.array };
411 assert_eq!(values, [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]]);
412 }
413
414 #[test]
415 fn matrix_maps_to_sys_element() {
416 let matrix = Matrix {
417 xx: 1.0,
418 xy: 2.0,
419 xz: 3.0,
420 yx: 4.0,
421 yy: 5.0,
422 yz: 6.0,
423 zx: 7.0,
424 zy: 8.0,
425 zz: 9.0,
426 };
427
428 let sys_matrix = sys::FusionMatrix::from(matrix);
430
431 let values = unsafe { sys_matrix.element };
432 assert_eq!(values.xx, 1.0,);
433 assert_eq!(values.xy, 2.0);
434 assert_eq!(values.xz, 3.0);
435 assert_eq!(values.yx, 4.0);
436 assert_eq!(values.yy, 5.0);
437 assert_eq!(values.yz, 6.0);
438 assert_eq!(values.zx, 7.0);
439 assert_eq!(values.zy, 8.0);
440 assert_eq!(values.zz, 9.0);
441 }
442
443 #[test]
444 fn euler_maps_from_sys_array() {
445 let sys_euler = sys::FusionEuler {
446 array: [1.0, 2.0, 3.0],
447 };
448
449 let euler = Euler::from(sys_euler);
451
452 assert_eq!(
453 euler,
454 Euler {
455 roll: 1.0,
456 pitch: 2.0,
457 yaw: 3.0
458 }
459 );
460 }
461
462 #[test]
463 fn euler_maps_from_sys_element() {
464 let sys_euler = sys::FusionEuler {
465 angle: sys::FusionEuler__bindgen_ty_1 {
466 roll: 1.0,
467 pitch: 2.0,
468 yaw: 3.0,
469 },
470 };
471
472 let euler = Euler::from(sys_euler);
474
475 assert_eq!(
476 euler,
477 Euler {
478 roll: 1.0,
479 pitch: 2.0,
480 yaw: 3.0,
481 }
482 );
483 }
484
485 #[test]
486 fn euler_maps_to_sys_array() {
487 let euler = Euler {
488 roll: 1.0,
489 pitch: 2.0,
490 yaw: 3.0,
491 };
492
493 let sys_euler = sys::FusionEuler::from(euler);
495
496 let values = unsafe { sys_euler.array };
497 assert_eq!(values, [1.0, 2.0, 3.0]);
498 }
499
500 #[test]
501 fn euler_maps_to_sys_element() {
502 let euler = Euler {
503 roll: 1.0,
504 pitch: 2.0,
505 yaw: 3.0,
506 };
507
508 let sys_euler = sys::FusionEuler::from(euler);
510
511 let values = unsafe { sys_euler.angle };
512 assert_eq!(values.roll, 1.0);
513 assert_eq!(values.pitch, 2.0);
514 assert_eq!(values.yaw, 3.0);
515 }
516}