1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160

#![no_std]

mod fusion_vector_impl;
mod fusion_quaternion_impl;
mod fusion_matrix_impl;
mod fusion_euler_impl;
mod fusion_impl;
mod fusion_ahrs_impl;
mod fusion_gyr_offset_impl;
mod nalgebra;

pub enum FusionConvention {
    /* North-West-Up */
    NWU,
    /* East-North-Up */
    ENU,
    /* North-East-Down */
    NED,
}

pub struct Fusion {
    pub(crate) gyr_misalignment: FusionMatrix,
    pub(crate) gyr_sensitivity: FusionVector,
    pub(crate) gyr_offset: FusionVector,
    pub(crate) acc_misalignment: FusionMatrix,
    pub(crate) acc_sensitivity: FusionVector,
    pub(crate) acc_offset: FusionVector,
    pub(crate) soft_iron_matrix: FusionMatrix,
    pub(crate) hard_iron_offset: FusionVector,
    pub(crate) ahrs: FusionAhrs,
    pub(crate) offset: FusionGyrOffset,
    pub(crate) last_dt: f32,
}

pub struct FusionAhrs {
    settings: FusionAhrsSettings,
    quaternion: FusionQuaternion,
    acc: FusionVector,
    initialising: bool,
    ramped_gain: f32,
    ramped_gain_step: f32,
    angular_rate_recovery: bool,
    half_accelerometer_feedback: FusionVector,
    half_magnetometer_feedback: FusionVector,
    accelerometer_ignored: bool,
    acceleration_recovery_trigger: i32,
    acceleration_recovery_timeout: i32,
    magnetometer_ignored: bool,
    magnetic_recovery_trigger: i32,
    magnetic_recovery_timeout: i32,
}

pub struct FusionAhrsSettings {
    pub convention: FusionConvention,
    pub gain: f32,
    pub gyr_range: f32,
    pub acc_rejection: f32,
    pub mag_rejection: f32,
    pub recovery_trigger_period: i32,
}

#[derive(Copy, Clone)]
pub struct Angle {
    pub roll: f32,
    pub pitch: f32,
    pub yaw: f32,
}

#[derive(Copy, Clone)]
#[allow(dead_code)]
pub struct FusionVector {
    pub x: f32,
    pub y: f32,
    pub z: f32,
}

#[derive(Copy, Clone)]
pub struct FusionMatrix {
    pub xx: f32,
    pub xy: f32,
    pub xz: f32,
    pub yx: f32,
    pub yy: f32,
    pub yz: f32,
    pub zx: f32,
    pub zy: f32,
    pub zz: f32,
}

#[derive(Copy, Clone)]
pub struct FusionQuaternion {
    pub w: f32,
    pub x: f32,
    pub y: f32,
    pub z: f32,
}

#[derive(Copy, Clone)]
#[allow(dead_code)]
pub struct FusionEuler {
    pub angle: Angle,
}

pub struct FusionGyrOffset {
    filter_coefficient: f32,
    timeout: u32,
    timer: u32,
    gyroscope_offset: FusionVector,
}

// Timeout in seconds.
const TIMEOUT: u32 = 5;

// Cutoff frequency in Hz.
const CUTOFF_FREQUENCY: f32 = 0.02f32;

// Threshold in degrees per second.
const THRESHOLD: f32 = 3f32;

fn fusion_degrees_to_radians(degrees: f32) -> f32 {
    degrees * (core::f32::consts::PI / 180.0f32)
}

fn fusion_radians_to_degrees(radians: f32) -> f32 {
    radians * (180.0f32 / core::f32::consts::PI)
}

fn asin_safe(value: f32) -> f32 {
    use libm::{asinf};
    if value <= -1.0f32 {
        return core::f32::consts::PI / -2.0f32;
    }
    if value >= 1.0f32 {
        return core::f32::consts::PI / 2.0f32;
    }
    asinf(value)
}

fn fusion_fast_inverse_sqrt(x: f32) -> f32 {
    union Union32 {
        f: f32,
        i: i32,
    }

    let mut union32 = Union32 { f: x };
    unsafe {
        union32.i = 0x5F1F1412 - (union32.i >> 1);
        union32.f * (1.69000231f32 - 0.714158168f32 * x * union32.f * union32.f)
    }
}


#[test]
fn fusion_fast_inverse_sqrt_test() {
    use libm::{fabsf};
    let result = fusion_fast_inverse_sqrt(9.0f32);
    let actual = 1f32 / result;
    let expected = 3f32;
    assert!(fabsf(actual - expected) < 0.01f32);
}