~xdavidwu/motion-control

b48f1323615cddd0d4085fb142f08de27b4103d9 — xdavidwu 2 years ago c5873d8
ahrs: tilt: guard against acos(>1), acos(<-1)
1 files changed, 7 insertions(+), 1 deletions(-)

M ahrs/tilt/tilt.ha
M ahrs/tilt/tilt.ha => ahrs/tilt/tilt.ha +7 -1
@@ 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,