get vqf compiling in fw

SlimeVR · Nov 12, 2023 · da575f9 · da575f9
1 parent 062aede
commit da575f9
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 3 deletions.
diff --git a/firmware/src/imu/fusion/vqf.rs b/firmware/src/imu/fusion/vqf.rs
@@ -3,7 +3,6 @@ use vqf::{Vqf, VqfParameters};
 use crate::imu::fusion::Fuser;
 use crate::imu::{FusedData, UnfusedData};
 
-/// Extended Kalman filtering in direction cosine matrix formation
 pub struct VqfFusion {
 	vqf: Vqf,
 }
@@ -13,14 +12,13 @@ impl VqfFusion {
 	pub fn new() -> Self {
 		let param = VqfParameters::default();
 		Self {
-			vqf: Vqf::new(0.01818, 0.01818, 0.01818, param),
+			vqf: Vqf::new(0.01389, 0.01389, 0.01389, param),
 		}
 	}
 }
 
 impl Fuser for VqfFusion {
 	fn process(&mut self, unfused: &UnfusedData) -> FusedData {
-		//elapsed
 		self.vqf
 			.update(unfused.gyro.clone(), unfused.accel.clone(), None);
 

diff --git a/vqf/src/lib.rs b/vqf/src/lib.rs
@@ -1207,6 +1207,24 @@ fn filterStep<const N: usize, const M: usize>(
 	return y;
 }
 
+#[no_mangle]
+fn fminf(a: f32, b: f32) -> f32 {
+	if a < b {
+		a
+	} else {
+		b
+	}
+}
+
+#[no_mangle]
+fn fmaxf(a: f32, b: f32) -> f32 {
+	if a > b {
+		a
+	} else {
+		b
+	}
+}
+
 #[cfg(test)]
 mod test {
 	use crate::Quat;