Get VQF working

Cargo.toml

@@ -4,12 +4,11 @@ members = [
   "networking/firmware_protocol",
   "networking/solarxr",
   "networking/tokio_shutdown",
-  "overlay",
   "skeletal_model/rust",
   "skeletal_model/napi",
   "vqf",
 ]
-exclude = ["da_demo", "nrf_demo", "firmware"]
+exclude = ["da_demo", "nrf_demo", "firmware", "overlay",]
 default-members = [
   "autoupdater",
   "networking/firmware_protocol",
@@ -30,12 +29,12 @@ authors = ["Ryan Butler <[email protected]>"]
 repository = "https://github.com/SlimeVR/SlimeVR-Rust"
 
 edition = "2021"
-rust-version = "1.65" # GATs and let-else
+rust-version = "1.73"
 
 
 # These may be opted into use by members of the workspace
 [workspace.dependencies]
 log = "0.4"
 eyre = "0.6"
-nalgebra = "0.31"
+nalgebra = "0.32"
 feature_macros = { git = "https://github.com/TheButlah/feature_macros" }

firmware/Cargo.toml

@@ -63,9 +63,9 @@ mcu-nrf52832 = [
 ]
 
 # Wi-fi dependencies
-net-wifi = ["esp-wifi?/wifi"]                 # use wifi
+net-wifi = ["esp-wifi?/wifi"] # use wifi
 net-ble = ["esp-wifi?/ble"]
-net-stubbed = []                                            # Stubs out network
+net-stubbed = []              # Stubs out network
 
 # Supported IMUs
 imu-bmi160 = []
@@ -80,6 +80,7 @@ log-uart = ["defmt_esp_println?/uart"]
 # Fusion algorithms for unfused imus
 fusion-stubbed = [] # Stubs out fusion so it returns the same pose every time
 fusion-dcm = []
+fusion-vqf = []
 
 # Enable to flash without needing `espflash`
 direct-boot = ["esp32c3-hal?/direct-boot"]
@@ -171,7 +172,11 @@ esp-wifi = { git = "https://github.com/esp-rs/esp-wifi.git", rev = "76ba312", fe
 smoltcp = { version = "0.9", default-features = false, features = [
 ], optional = true }
 embassy-net = { version = "*", optional = true, features = [
-  "nightly", "tcp", "udp", "dhcpv4", "medium-ethernet"
+  "nightly",
+  "tcp",
+  "udp",
+  "dhcpv4",
+  "medium-ethernet",
 ] }
 
 # Generic BLE
@@ -205,6 +210,7 @@ bmi160 = "0.1"
 
 # Sensor fusion
 dcmimu = "0.2"
+vqf = { path = "../vqf" }
 
 # Other crates
 static_cell = "1"
@@ -268,7 +274,7 @@ opt-level = 'z'
 
 [profile.release]
 debug = true # Symbols get stripped when flashing anyway
-lto = false # LTO doesn't work for esp-wifi
+lto = false  # LTO doesn't work for esp-wifi
 
 
 ###################

firmware/build.rs

@@ -67,7 +67,7 @@ fn check_features_compatible() {
 	mandatory_and_unique!("imu-stubbed", "imu-mpu6050", "imu-bmi160");
 	mandatory_and_unique!("log-rtt", "log-usb-serial", "log-uart");
 	mandatory_and_unique!("net-wifi", "net-ble", "net-stubbed");
-	mandatory_and_unique!("fusion-stubbed", "fusion-dcm");
+	mandatory_and_unique!("fusion-stubbed", "fusion-dcm", "fusion-vqf");
 
 	#[cfg(any(feature = "mcu-nrf52840", feature = "mcu-nrf52832"))]
 	mandatory_and_unique!(

firmware/src/imu/fusion/mod.rs

@@ -1,8 +1,10 @@
 mod dcm;
 mod stubbed;
+mod vqf;
 
 pub use self::dcm::Dcm;
 pub use self::stubbed::Stubbed;
+pub use self::vqf::VqfFusion;
 
 use crate::imu::{FusedData, Imu, UnfusedData};
 
@@ -36,9 +38,11 @@ impl<I: Imu<Data = UnfusedData>, F: Fuser> Imu for FusedImu<I, F> {
 /// Builds a new fuser. The concrete impl is determined by a feature flag.
 pub fn new_fuser() -> impl Fuser {
 	#[cfg(feature = "fusion-stubbed")]
-	let f = Stubbed::new();
+	let fusion_algorithm = Stubbed::new();
 	#[cfg(feature = "fusion-dcm")]
-	let f = Dcm::new();
+	let fusion_algorithm = Dcm::new();
+	#[cfg(feature = "fusion-vqf")]
+	let fusion_algorithm = VqfFusion::new();
 
-	f
+	fusion_algorithm
 }

firmware/src/imu/fusion/vqf.rs

@@ -0,0 +1,28 @@
+use vqf::{Vqf, VqfParameters};
+
+use crate::imu::fusion::Fuser;
+use crate::imu::{FusedData, UnfusedData};
+
+pub struct VqfFusion {
+	vqf: Vqf,
+}
+
+impl VqfFusion {
+	#[allow(dead_code)]
+	pub fn new() -> Self {
+		let param = VqfParameters::default();
+		Self {
+			vqf: Vqf::new(0.01389, 0.01389, 0.01389, param),
+		}
+	}
+}
+
+impl Fuser for VqfFusion {
+	fn process(&mut self, unfused: &UnfusedData) -> FusedData {
+		self.vqf
+			.update(unfused.gyro.clone(), unfused.accel.clone(), None);
+
+		let q = self.vqf.getQuat6D();
+		FusedData { q }
+	}
+}

rust-toolchain.toml

@@ -1,4 +1,4 @@
 [toolchain]
-channel = "1.65"          # See workspace Cargo.toml
+channel = "1.73"          # See workspace Cargo.toml
 components = ["rust-src"]
 profile = "default"

vqf/Cargo.toml

@@ -5,6 +5,12 @@ edition = "2021"
 license = "MIT"
 publish = false
 
+[features]
+default = ["single-precision"]
+debug-state = []
+single-precision = []
+
+
 [dependencies]
-nalgebra = { version = "0.32", default-features = false, features = ["libm"] }
+nalgebra = { version = "0.31", default-features = false, features = ["libm"] }
 num-traits = { version = "0.2", default-features = false, features = ["libm"] }