From 1b2a0bd7154b1c92ce061905a72be3a978df4493 Mon Sep 17 00:00:00 2001
From: xdavidwu <xdavidwuph@gmail.com>
Date: Wed, 18 May 2022 13:24:42 +0800
Subject: [PATCH] ahrs: implement tilt

---
 ahrs/tilt/tilt.ha          | 30 ++++++++++++++++++++++++++++++
 ahrs/tilt/types.ha         |  7 +++++++
 math/quaternion/algebra.ha | 15 +++++++++++++++
 tools/gy-801-dump/main.ha  | 11 +++++++++++
 4 files changed, 63 insertions(+)
 create mode 100644 ahrs/tilt/tilt.ha
 create mode 100644 ahrs/tilt/types.ha

diff --git a/ahrs/tilt/tilt.ha b/ahrs/tilt/tilt.ha
new file mode 100644
index 0000000..0e6087f
--- /dev/null
+++ b/ahrs/tilt/tilt.ha
@@ -0,0 +1,30 @@
+use math;
+use math::quaternion;
+
+export fn new(neutral: (f64, f64, f64)) tilt = {
+	const n = quaternion::normalize((0.0, neutral.0, neutral.1, neutral.2));
+	return alloc(tilt_impl {
+		neutral = n,
+	});
+};
+
+export fn estimate(tilt: tilt, readings: (f64, f64, f64)) quaternion::quaternion = {
+	// r = q neutral 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,
+	);
+	return quaternion::from_axis_angle(axis, angle);
+};
+
+export fn estimate_euler(tilt: tilt, readings: (f64, f64, f64)) (f64, f64, f64) = {
+	return quaternion::to_euler(estimate(tilt, readings));
+};
+
+export fn destroy(tilt: tilt) void = {
+	free(tilt);
+};
diff --git a/ahrs/tilt/types.ha b/ahrs/tilt/types.ha
new file mode 100644
index 0000000..a327c42
--- /dev/null
+++ b/ahrs/tilt/types.ha
@@ -0,0 +1,7 @@
+use math::quaternion;
+
+export type tilt_impl = struct {
+	neutral: quaternion::quaternion,
+};
+
+export type tilt = *tilt_impl;
diff --git a/math/quaternion/algebra.ha b/math/quaternion/algebra.ha
index aa76a48..e88d037 100644
--- a/math/quaternion/algebra.ha
+++ b/math/quaternion/algebra.ha
@@ -1,3 +1,5 @@
+use math;
+
 export fn product(a: quaternion, b: quaternion) quaternion = {
 	return (
 		a.0 * b.0 - a.1 * b.1 - a.2 * b.2 - a.3 * b.3,
@@ -6,3 +8,16 @@ export fn product(a: quaternion, b: quaternion) quaternion = {
 		a.0 * b.3 + a.1 * b.2 - a.2 * b.1 + a.3 * b.0 ,
 	);
 };
+
+export fn norm2(a: quaternion) f64 = {
+	return a.0 * a.0 + a.1 * a.1 + a.2 * a.2 + a.3 * a.3;
+};
+
+export fn norm(a: quaternion) f64 = {
+	return math::sqrtf64(norm2(a));
+};
+
+export fn normalize(a: quaternion) quaternion = {
+	const norm = norm(a);
+	return (a.0 / norm, a.1 / norm, a.2 / norm, a.3 / norm);
+};
diff --git a/tools/gy-801-dump/main.ha b/tools/gy-801-dump/main.ha
index 0b15473..9a3a294 100644
--- a/tools/gy-801-dump/main.ha
+++ b/tools/gy-801-dump/main.ha
@@ -1,3 +1,4 @@
+use ahrs::tilt;
 use ahrs::integration;
 use fmt;
 use fs;
@@ -58,6 +59,11 @@ export fn main() void = {
 	signal::handle(signal::SIGINT, &sigint);
 	const integrator = integration::new();
 
+	timerfd::read(fd)!;
+	const acc_readings = ADXL345::read(ADXL345)!;
+	fmt::printfln("Neutral acc.: {} {} {} g", acc_readings.0, acc_readings.1, acc_readings.2)!;
+	const tilt = tilt::new(acc_readings);
+
 	for (let i = 0; !exit; i += 1) {
 		timerfd::read(fd)!;
 		const gyro_readings = L3G4200D::read(L3G4200D)!;
@@ -68,6 +74,7 @@ export fn main() void = {
 		if (i % 50 == 0) {
 			const acc_readings = ADXL345::read(ADXL345)!;
 			let attitude = integration::read_euler(integrator);
+			let tilt_attitude = tilt::estimate_euler(tilt, acc_readings);
 
 			fmt::printfln("Accelerometer: {} {} {} g",
 				acc_readings.0, acc_readings.1, acc_readings.2)!;
@@ -77,6 +84,10 @@ export fn main() void = {
 				attitude.0 / math::PI * 180.0,
 				attitude.1 / math::PI * 180.0,
 				attitude.2 / math::PI * 180.0)!;
+			fmt::printfln("Attitude (tilt): {} {} {} deg",
+				tilt_attitude.0 / math::PI * 180.0,
+				tilt_attitude.1 / math::PI * 180.0,
+				tilt_attitude.2 / math::PI * 180.0)!;
 		};
 	};
 };
-- 
2.45.2