fusion_sys/
lib.rs

1#![allow(non_upper_case_globals)]
2#![allow(non_camel_case_types)]
3#![allow(non_snake_case)]
4
5mod bindings;
6use bindings::*;
7
8use std::os::raw::c_void;
9
10use std::sync::{Arc, Mutex};
11
12#[derive(Debug)]
13pub struct Fusion {
14    fusion: Arc<Mutex<Ahrs>>,
15    gravity: f64,
16}
17
18unsafe impl Send for Fusion {}
19unsafe impl Sync for Fusion {}
20
21struct Ahrs {
22    ahrs: *mut c_void,
23}
24
25impl std::fmt::Debug for Ahrs {
26    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
27        f.debug_struct("Ahrs").finish()
28    }
29}
30
31impl Ahrs {
32    fn new() -> Self {
33        Self {
34            ahrs: unsafe { create_ahrs() },
35        }
36    }
37
38    fn get(&self) -> *mut c_void {
39        self.ahrs
40    }
41}
42
43/// Safety: Ahrs is not Sync nor Send, but the higher level Fusion ensures that it is only accessed from one thread.
44unsafe impl Send for Ahrs {}
45unsafe impl Sync for Ahrs {}
46
47impl Fusion {
48    pub fn new(gravity: f64) -> Self {
49        Self {
50            fusion: Arc::new(Mutex::new(Ahrs::new())),
51            gravity,
52        }
53    }
54
55    /// Update the AHRS with new sensor data.
56    /// gyroscope units are degrees/s
57    /// accelerometer units are g
58    pub fn update_no_magnetometer(&mut self, dt: f64, gyro: [f64; 3], accel: [f64; 3]) {
59        let lock = self.fusion.lock().unwrap();
60        unsafe {
61            update_no_magnetometer(
62                lock.get(),
63                dt,
64                gyro[0],
65                gyro[1],
66                gyro[2],
67                accel[0],
68                accel[1],
69                accel[2],
70            );
71        }
72    }
73
74    /// Update the AHRS with new sensor data.
75    /// gyroscope units are radians/s
76    /// accelerometer units are m/s^2
77    pub fn update_no_magnetometer_ros(&mut self, dt: f64, gyro: [f64; 3], accel: [f64; 3]) {
78        self.update_no_magnetometer(
79            dt,
80            gyro.map(|v| v.to_degrees()),
81            accel.map(|a| a / self.gravity),
82        )
83    }
84
85    /// Get orientation in world frame as a quaternion.
86    /// The quaternion is in the form [w, x, y, z].
87    pub fn get_quaternion(&self) -> [f64; 4] {
88        let mut q = vec![0.0; 4]; // TODO test without vec on stack
89        let lock = self.fusion.lock().unwrap();
90        unsafe {
91            get_quaternion(
92                lock.get(),
93                q.as_mut_ptr().offset(0),
94                q.as_mut_ptr().offset(1),
95                q.as_mut_ptr().offset(2),
96                q.as_mut_ptr().offset(3),
97            );
98        }
99        [q[0], q[1], q[2], q[3]]
100    }
101
102    /// Get linear acceleration in sensor frame.
103    /// The acceleration is in the form [x, y, z].
104    /// Units are g.
105    pub fn get_linear_acceleration(&self) -> [f64; 3] {
106        let mut a = vec![0.0; 3]; // TODO test without vec on stack
107        let lock = self.fusion.lock().unwrap();
108        unsafe {
109            get_linear_acceleration(
110                lock.get(),
111                a.as_mut_ptr().offset(0),
112                a.as_mut_ptr().offset(1),
113                a.as_mut_ptr().offset(2),
114            );
115        }
116        [a[0], a[1], a[2]]
117    }
118
119    pub fn get_linear_acceleration_ros(&self) -> [f64; 3] {
120        self.get_linear_acceleration().map(|a| a * self.gravity)
121    }
122}
123
124impl Drop for Fusion {
125    fn drop(&mut self) {
126        let lock = self.fusion.lock().unwrap();
127        unsafe {
128            free_ahrs(lock.get());
129        }
130    }
131}
132
133#[cfg(test)]
134mod tests {
135    use super::*;
136
137    #[test]
138    fn linking() {
139        let mut fusion = Fusion::new(9.81);
140        fusion.update_no_magnetometer(0.01, [0.0, 0.0, 0.0], [0.0, 0.0, 1.0]);
141        let _ = fusion.get_quaternion();
142    }
143}
fusion_sys/lib.rs

fusion_sys/
lib.rs
