From b48f1323615cddd0d4085fb142f08de27b4103d9 Mon Sep 17 00:00:00 2001
From: xdavidwu <xdavidwuph@gmail.com>
Date: Wed, 18 May 2022 16:48:02 +0800
Subject: [PATCH] ahrs: tilt: guard against acos(>1), acos(<-1)

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

diff --git a/ahrs/tilt/tilt.ha b/ahrs/tilt/tilt.ha
index 4bc6e39..e2be04f 100644
--- a/ahrs/tilt/tilt.ha
+++ b/ahrs/tilt/tilt.ha
@@ -11,7 +11,13 @@ export fn new(neutral: (f64, f64, f64)) tilt = {
 export fn estimate(tilt: tilt, readings: (f64, f64, f64)) quaternion::quaternion = {
 	// 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;
+	let dot = tilt.neutral.1 * r.1  + tilt.neutral.2 * r.2 + tilt.neutral.3 * r.3;
+	if (dot > 1f64) {
+		dot = 1f64;
+	};
+	if (dot < -1f64) {
+		dot = -1f64;
+	};
 	const angle = math::acosf64(dot);
 	const axis = (
 		r.2 * tilt.neutral.3 - r.3 * tilt.neutral.2,
-- 
2.45.2