From 2273819d74e67194ed941f83ea8084bae79ceb14 Mon Sep 17 00:00:00 2001
From: xdavidwu <xdavidwuph@gmail.com>
Date: Wed, 18 May 2022 13:37:41 +0800
Subject: [PATCH] ahrs: tilt: fix direction

---
 ahrs/tilt/tilt.ha | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/ahrs/tilt/tilt.ha b/ahrs/tilt/tilt.ha
index 0e6087f..4bc6e39 100644
--- a/ahrs/tilt/tilt.ha
+++ b/ahrs/tilt/tilt.ha
@@ -9,14 +9,14 @@ export fn new(neutral: (f64, f64, f64)) tilt = {
 };
 
 export fn estimate(tilt: tilt, readings: (f64, f64, f64)) quaternion::quaternion = {
-	// r = q neutral q^(-1), find q
+	// neutral = q r q^(-1), find q
 	let r = quaternion::normalize((0.0, readings.0, readings.1, readings.2));
 	const dot = tilt.neutral.1 * r.1  + tilt.neutral.2 * r.2 + tilt.neutral.3 * r.3;
 	const angle = math::acosf64(dot);
 	const axis = (
-		tilt.neutral.2 * r.3 - tilt.neutral.3 * r.2,
-		tilt.neutral.3 * r.1 - tilt.neutral.1 * r.3,
-		tilt.neutral.1 * r.2 - tilt.neutral.2 * r.1,
+		r.2 * tilt.neutral.3 - r.3 * tilt.neutral.2,
+		r.3 * tilt.neutral.1 - r.1 * tilt.neutral.3,
+		r.1 * tilt.neutral.2 - r.2 * tilt.neutral.1,
 	);
 	return quaternion::from_axis_angle(axis, angle);
 };
-- 
2.45.2