imu_fusion/
lib.rs

1#![no_std]
2
3mod fusion_vector_impl;
4mod fusion_quaternion_impl;
5mod fusion_matrix_impl;
6mod fusion_euler_impl;
7mod fusion_impl;
8mod fusion_ahrs_impl;
9mod fusion_gyr_offset_impl;
10mod nalgebra;
11
12pub enum FusionConvention {
13    /* North-West-Up */
14    NWU,
15    /* East-North-Up */
16    ENU,
17    /* North-East-Down */
18    NED,
19}
20
21pub struct Fusion {
22    pub gyr_misalignment: FusionMatrix,
23    pub gyr_sensitivity: FusionVector,
24    pub gyr_offset: FusionVector,
25    pub acc_misalignment: FusionMatrix,
26    pub acc_sensitivity: FusionVector,
27    pub acc_offset: FusionVector,
28    pub soft_iron_matrix: FusionMatrix,
29    pub hard_iron_offset: FusionVector,
30    pub ahrs: FusionAhrs,
31    pub offset: FusionGyrOffset,
32    pub last_timestamp: f32,
33}
34
35pub struct FusionAhrs {
36    pub settings: FusionAhrsSettings,
37    pub quaternion: FusionQuaternion,
38    pub acc: FusionVector,
39    pub initialising: bool,
40    pub ramped_gain: f32,
41    pub ramped_gain_step: f32,
42    pub angular_rate_recovery: bool,
43    pub half_accelerometer_feedback: FusionVector,
44    pub half_magnetometer_feedback: FusionVector,
45    pub accelerometer_ignored: bool,
46    pub acceleration_recovery_trigger: i32,
47    pub acceleration_recovery_timeout: i32,
48    pub magnetometer_ignored: bool,
49    pub magnetic_recovery_trigger: i32,
50    pub magnetic_recovery_timeout: i32,
51}
52
53pub struct FusionAhrsSettings {
54    pub convention: FusionConvention,
55    pub gain: f32,
56    pub gyr_range: f32,
57    pub acc_rejection: f32,
58    pub mag_rejection: f32,
59    pub recovery_trigger_period: i32,
60}
61
62pub struct FusionAhrsFlags {
63    pub initializing: bool,
64    pub angular_rate_recovery: bool,
65    pub acceleration_recovery: bool,
66    pub magnetic_recovery: bool,
67}
68
69#[derive(Copy, Clone)]
70pub struct Angle {
71    pub roll: f32,
72    pub pitch: f32,
73    pub yaw: f32,
74}
75
76#[derive(Copy, Clone)]
77#[allow(dead_code)]
78pub struct FusionVector {
79    pub x: f32,
80    pub y: f32,
81    pub z: f32,
82}
83
84#[derive(Copy, Clone)]
85pub struct FusionMatrix {
86    pub xx: f32,
87    pub xy: f32,
88    pub xz: f32,
89    pub yx: f32,
90    pub yy: f32,
91    pub yz: f32,
92    pub zx: f32,
93    pub zy: f32,
94    pub zz: f32,
95}
96
97#[derive(Copy, Clone)]
98pub struct FusionQuaternion {
99    pub w: f32,
100    pub x: f32,
101    pub y: f32,
102    pub z: f32,
103}
104
105#[derive(Copy, Clone)]
106#[allow(dead_code)]
107pub struct FusionEuler {
108    pub angle: Angle,
109}
110
111pub struct FusionGyrOffset {
112    pub filter_coefficient: f32,
113    pub timeout: u32,
114    pub timer: u32,
115    pub gyroscope_offset: FusionVector,
116}
117
118// Timeout in seconds.
119const TIMEOUT: u32 = 5;
120
121// Cutoff frequency in Hz.
122const CUTOFF_FREQUENCY: f32 = 0.02f32;
123
124// Threshold in degrees per second.
125const THRESHOLD: f32 = 3f32;
126
127fn fusion_degrees_to_radians(degrees: f32) -> f32 {
128    degrees * (core::f32::consts::PI / 180.0f32)
129}
130
131fn fusion_radians_to_degrees(radians: f32) -> f32 {
132    radians * (180.0f32 / core::f32::consts::PI)
133}
134
135fn asin_safe(value: f32) -> f32 {
136    use libm::{asinf};
137    if value <= -1.0f32 {
138        return core::f32::consts::PI / -2.0f32;
139    }
140    if value >= 1.0f32 {
141        return core::f32::consts::PI / 2.0f32;
142    }
143    asinf(value)
144}
145
146fn fusion_fast_inverse_sqrt(x: f32) -> f32 {
147    union Union32 {
148        f: f32,
149        i: i32,
150    }
151
152    let mut union32 = Union32 { f: x };
153    unsafe {
154        union32.i = 0x5F1F1412 - (union32.i >> 1);
155        union32.f * (1.69000231f32 - 0.714158168f32 * x * union32.f * union32.f)
156    }
157}
158
159
160#[test]
161fn fusion_fast_inverse_sqrt_test() {
162    use libm::{fabsf};
163    let result = fusion_fast_inverse_sqrt(9.0f32);
164    let actual = 1f32 / result;
165    let expected = 3f32;
166    assert!(fabsf(actual - expected) < 0.01f32);
167}
imu_fusion/lib.rs

imu_fusion/
lib.rs
