#10 (stage 1) implement complementary filter to fuse acc/gyro

Cargo.toml

@@ -22,13 +22,13 @@ rokid = ["rusb"]
 [dependencies]
 bytemuck = { version = "1.13.1", optional = true }
 byteorder = "1.4"
-nalgebra = { version = "0.32.3", default-features=false, features = ["std"]}
+nalgebra = { version = "0.32.3", default-features = false, features = ["std"] }
 rusb = { version = "0.9.2", optional = true }
 serialport = { version = "4.2", optional = true }
 tinyjson = { version = "2.5.1", optional = true }
 
 [target.'cfg(target_os = "android")'.dependencies]
-hidapi = { version = "2.4.1", default-features=false, features = [ "linux-static-libusb" ], optional = true }
+hidapi = { version = "2.4.1", default-features = false, features = ["linux-static-libusb"], optional = true }
 
 [target.'cfg(not(target_os = "android"))'.dependencies]
 hidapi = { version = "2.4.1", optional = true }

examples/sensor_fusion.rs

@@ -0,0 +1,25 @@
+// Copyright (C) 2023, Alex Badics
+// This file is part of ar-drivers-rs
+// Licensed under the MIT license. See LICENSE file in the project root for details.
+
+use ar_drivers::any_fusion;
+
+fn main() {
+    let mut fusion = any_fusion().unwrap(); // Declare conn as mutable
+
+    let serial = fusion.glasses().serial().unwrap();
+    println!("Got glasses, serial={}", serial);
+
+    println!("");
+    loop {
+        fusion.update();
+        let quaternion = fusion.attitude_quaternion();
+        let frd = fusion.attitude_frd_deg();
+        let inconsistency = fusion.inconsistency();
+
+        println!("quaternion:\t{:10.7}", quaternion);
+        println!("euler:\t{:10.7}", frd.transpose());
+
+        println!("inconsistency:\t{:10.7}", inconsistency);
+    }
+}

rustfmt.toml

@@ -0,0 +1,10 @@
+format_code_in_doc_comments = true
+format_macro_matchers = true
+group_imports = "StdExternalCrate"
+hex_literal_case = "Lower"
+imports_granularity = "Module"
+newline_style = "Unix"
+normalize_comments = true
+normalize_doc_attributes = true
+use_field_init_shorthand = true
+use_try_shorthand = true

src/lib.rs

@@ -36,10 +36,13 @@
 
 use nalgebra::{Isometry3, Matrix3, UnitQuaternion, Vector2, Vector3};
 
+use crate::naive_cf::NaiveCF;
+
 #[cfg(feature = "grawoow")]
 pub mod grawoow;
 #[cfg(feature = "mad_gaze")]
 pub mod mad_gaze;
+mod naive_cf;
 #[cfg(feature = "nreal")]
 pub mod nreal_air;
 #[cfg(feature = "nreal")]
@@ -77,6 +80,58 @@ pub enum Error {
 
 type Result<T> = std::result::Result<T, Error>;
 
+/*
+high level interface of glasses & state estimation, with the following built-in fusion pipeline:
+
+(the first version should only use complementary filter for simplicity and sanity test)
+
+- roll/pitch <= acc + gyro (complementary filter)
+  - assuming that acc vector always pointed up, spacecraft moving in that direction can create 1G artificial gravity
+    - TODO: this obviously assumes no steadily accelerating frame, at which point up d_acc has to be used for correction
+  - TODO: use ESKF (error-state/multiplicatory KF, https://arxiv.org/abs/1711.02508)
+- gyro-yaw <= gyro (integrate over time)
+- mag-yaw <= mag + roll/pitch (arctan)
+  - TODO: mag calibration?
+     (continuous ellipsoid fitting, assuming homogeneous E-M environment & hardpoint-mounted E-M interference)
+- yaw <= mag-yaw + gyro-gyro (complementary filter)
+  - TODO: use EKF
+
+CAUTION: unlike [[GlassesEvent]], all states & outputs should use FRD reference frame
+ (forward, right, down, corresponding to roll, pitch, yaw in Euler angles-represented rotation)
+
+FRD is the standard frame for aerospace, and is also the default frame for NALgebra
+*/
+pub trait Fusion: Send {
+    fn glasses(&mut self) -> &mut Box<dyn ARGlasses>;
+    // TODO: only declared mutable as many API of ARGlasses are also mutable
+
+    /// primary estimation output
+    /// can be used to convert to Euler angles of different conventions
+    fn attitude_quaternion(&self) -> UnitQuaternion<f32>;
+
+    /// use FRD frame as error in Quaternion is multiplicative & is over-defined
+    fn inconsistency(&self) -> f32;
+
+    fn update(&mut self) -> ();
+}
+
+impl dyn Fusion {
+    pub fn attitude_frd_rad(&self) -> Vector3<f32> {
+        let (roll, pitch, yaw) = (self.attitude_quaternion()).euler_angles();
+        Vector3::new(roll, pitch, yaw)
+    }
+
+    pub fn attitude_frd_deg(&self) -> Vector3<f32> {
+        self.attitude_frd_rad().map(|x| x.to_degrees())
+    }
+}
+
+pub fn any_fusion() -> Result<Box<dyn Fusion>> {
+    // let glasses = any_glasses()?;
+    let glasses = any_glasses()?;
+    Ok(Box::new(NaiveCF::new(glasses)?))
+}
+
 impl std::error::Error for Error {
     fn source(&self) -> Option<&(dyn std::error::Error + 'static)> {
         match self {
@@ -243,30 +298,43 @@ pub struct DisplayMatrices {
     pub isometry: Isometry3<f64>,
 }
 
+fn upcast<G: ARGlasses + 'static>(result: Result<G>) -> Result<Box<dyn ARGlasses>> {
+    result.map(|glasses| Box::new(glasses) as Box<dyn ARGlasses>)
+}
+
 /// Convenience function to detect and connect to any of the supported glasses
 #[cfg(not(target_os = "android"))]
 pub fn any_glasses() -> Result<Box<dyn ARGlasses>> {
-    #[cfg(feature = "rokid")]
-    if let Ok(glasses) = rokid::RokidAir::new() {
-        return Ok(Box::new(glasses));
-    };
-    #[cfg(feature = "nreal")]
-    if let Ok(glasses) = nreal_air::NrealAir::new() {
-        return Ok(Box::new(glasses));
-    };
-    #[cfg(feature = "nreal")]
-    if let Ok(glasses) = nreal_light::NrealLight::new() {
-        return Ok(Box::new(glasses));
-    };
-    #[cfg(feature = "grawoow")]
-    if let Ok(glasses) = grawoow::GrawoowG530::new() {
-        return Ok(Box::new(glasses));
-    };
-    #[cfg(feature = "mad_gaze")]
-    if let Ok(glasses) = mad_gaze::MadGazeGlow::new() {
-        return Ok(Box::new(glasses));
-    };
-    Err(Error::NotFound)
+    let glasses_factories: Vec<(&str, fn() -> Result<Box<dyn ARGlasses>>)> = vec![
+        #[cfg(feature = "rokid")]
+        ("RokidAir", || upcast(rokid::RokidAir::new())),
+        #[cfg(feature = "nreal")]
+        ("NrealAir", || upcast(nreal_air::NrealAir::new())),
+        #[cfg(feature = "nreal")]
+        ("NrealLight", || upcast(nreal_light::NrealLight::new())),
+        #[cfg(feature = "grawoow")]
+        ("GrawoowG530", || upcast(grawoow::GrawoowG530::new())),
+        #[cfg(feature = "mad_gaze")]
+        ("MadGazeGlow", || upcast(mad_gaze::MadGazeGlow::new())),
+    ];
+
+    glasses_factories
+        .into_iter()
+        .find_map(|(glasses_type, factory)| {
+            let factory: fn() -> Result<Box<dyn ARGlasses>> = factory;
+
+            factory()
+                .map_err(|e| {
+                    //
+                    println!("can't find {}: {}", glasses_type, e)
+                })
+                .ok()
+                .map(|v| {
+                    println!("found {}", glasses_type);
+                    v
+                })
+        })
+        .ok_or(Error::NotFound)
 }
 
 impl From<std::io::Error> for Error {