feat: add FusionOffset bindings

examples/simple.rs

@@ -4,7 +4,7 @@ use std::{
     path::Path,
 };
 
-use fusion_imu::{FusionAhrs, Vector};
+use fusion_imu::{FusionAhrs, FusionOffset, Vector};
 use plotpy::{Curve, Legend, Plot};
 
 fn main() {
@@ -13,6 +13,7 @@ fn main() {
     let lines = reader.lines();
 
     let mut fusion = FusionAhrs::new();
+    let mut offset = FusionOffset::new(100);
     let mut prev_time = 0.0;
 
     let mut ts = Vec::new();
@@ -49,6 +50,7 @@ fn main() {
             y: elements.next().unwrap(),
             z: elements.next().unwrap(),
         };
+        let gyroscope = offset.update(gyroscope);
         fusion.update_no_magnetometer(gyroscope, accelerometer, delta_time);
 
         let quat = fusion.get_quaternion().to_euler();
@@ -145,5 +147,5 @@ fn main() {
         .add(&legend);
 
     let path = Path::new("/tmp/fusion/examples_plots").join("plot.svg");
-    plot.save_and_show(&path).unwrap();
+    plot.set_show_errors(true).save_and_show(&path).unwrap();
 }

src/lib.rs

@@ -7,11 +7,13 @@ mod calibration;
 mod flags;
 mod internal_states;
 mod math;
+mod offset;
 mod settings;
 
 pub use ahrs::*;
 pub use calibration::*;
 pub use flags::*;
 pub use internal_states::*;
 pub use math::*;
+pub use offset::*;
 pub use settings::*;

src/offset.rs

@@ -0,0 +1,34 @@
+use core::mem::MaybeUninit;
+
+use fusion_imu_sys as sys;
+
+use crate::Vector;
+
+/// Gyroscope offset algorithm structure.
+pub struct FusionOffset {
+    inner: sys::FusionOffset,
+}
+
+impl FusionOffset {
+    /// Create a new `FusionOffset` instance.
+    ///
+    /// Sample rate in Hz.
+    pub fn new(sample_rate: u32) -> Self {
+        let mut offset = MaybeUninit::uninit();
+        unsafe {
+            sys::FusionOffsetInitialise(offset.as_mut_ptr(), sample_rate);
+            FusionOffset {
+                inner: offset.assume_init(),
+            }
+        }
+    }
+
+    /// Updates the gyroscope offset algorithm and returns the corrected
+    /// gyroscope measurement. Values are in degrees per second.
+    pub fn update(&mut self, gyroscope: Vector) -> Vector {
+        unsafe {
+            sys::FusionOffsetUpdate(&mut self.inner as *mut sys::FusionOffset, gyroscope.into())
+                .into()
+        }
+    }
+}