1 files changed, 7 insertions(+), 1 deletions(-)

M 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,