1 files changed, 6 insertions(+), 4 deletions(-)

M ahrs/complementary/complementary.ha

@@ 17,11 17,13 @@ export fn read_euler(complementary: complementary) (f64, f64, f64) =
 	integration::read_euler(complementary.integration);
 
 export fn update_accelerometer(complementary: complementary, readings: (f64, f64, f64)) void = {
-	let q_accel = tilt::estimate(complementary.tilt, readings);
-	complementary.integration.q = quaternion::slerp(
+	let accel = quaternion::to_euler(tilt::estimate(complementary.tilt, readings));
+	let orig = quaternion::to_euler(complementary.integration.q);
+	let qc = quaternion::from_euler((accel.0, accel.1, orig.2));
+	complementary.integration.q = quaternion::normalize(quaternion::slerp(
 		complementary.integration.q,
-		q_accel,
-		complementary.alpha);
+		qc,
+		complementary.alpha));
 };
 
 export fn destroy(complementary: complementary) void = {