From c5873d8bc568bcaff8e561fdaeef48936f765fe8 Mon Sep 17 00:00:00 2001
From: xdavidwu <xdavidwuph@gmail.com>
Date: Wed, 18 May 2022 15:25:43 +0800
Subject: [PATCH] ahrs: implement complementary filter

---
 ahrs/complementary/complementary.ha | 31 +++++++++++++++++++++++++++++
 ahrs/complementary/types.ha         | 10 ++++++++++
 math/quaternion/algebra.ha          |  4 ++++
 math/quaternion/slerp.ha            | 21 +++++++++++++++++++
 tools/gy-801-dump/main.ha           | 12 ++++++++++-
 5 files changed, 77 insertions(+), 1 deletion(-)
 create mode 100644 ahrs/complementary/complementary.ha
 create mode 100644 ahrs/complementary/types.ha
 create mode 100644 math/quaternion/slerp.ha

diff --git a/ahrs/complementary/complementary.ha b/ahrs/complementary/complementary.ha
new file mode 100644
index 0000000..892fd33
--- /dev/null
+++ b/ahrs/complementary/complementary.ha
@@ -0,0 +1,31 @@
+use ahrs::integration;
+use ahrs::tilt;
+use math::quaternion;
+
+export fn new(neutral: (f64, f64, f64), alpha: f64) complementary = {
+	return alloc(complementary_impl {
+		alpha = alpha,
+		integration = integration::new(),
+		tilt = tilt::new(neutral),
+	});
+};
+
+export fn update(complementary: complementary, readings: (f64, f64, f64), dt: f64) void =
+	integration::update(complementary.integration, readings, dt);
+
+export fn read_euler(complementary: complementary) (f64, f64, f64) =
+	integration::read_euler(complementary.integration);
+
+export fn update_accelerometer(complementary: complementary, readings: (f64, f64, f64)) void = {
+	let q_accel = tilt::estimate(complementary.tilt, readings);
+	complementary.integration.q = quaternion::slerp(
+		complementary.integration.q,
+		q_accel,
+		complementary.alpha);
+};
+
+export fn destroy(complementary: complementary) void = {
+	integration::destroy(complementary.integration);
+	tilt::destroy(complementary.tilt);
+	free(complementary);
+};
diff --git a/ahrs/complementary/types.ha b/ahrs/complementary/types.ha
new file mode 100644
index 0000000..bb4ef3d
--- /dev/null
+++ b/ahrs/complementary/types.ha
@@ -0,0 +1,10 @@
+use ahrs::integration;
+use ahrs::tilt;
+
+export type complementary_impl = struct {
+	alpha: f64,
+	integration: integration::integration,
+	tilt: tilt::tilt,
+};
+
+export type complementary = *complementary_impl;
diff --git a/math/quaternion/algebra.ha b/math/quaternion/algebra.ha
index e88d037..f1e68a7 100644
--- a/math/quaternion/algebra.ha
+++ b/math/quaternion/algebra.ha
@@ -21,3 +21,7 @@ export fn normalize(a: quaternion) quaternion = {
 	const norm = norm(a);
 	return (a.0 / norm, a.1 / norm, a.2 / norm, a.3 / norm);
 };
+
+export fn conjugate(a: quaternion) quaternion = {
+	return (a.0, -a.1, -a.2, -a.3);
+};
diff --git a/math/quaternion/slerp.ha b/math/quaternion/slerp.ha
new file mode 100644
index 0000000..5857c0f
--- /dev/null
+++ b/math/quaternion/slerp.ha
@@ -0,0 +1,21 @@
+use math;
+
+export fn slerp(a: quaternion, b: quaternion, t: f64) quaternion = {
+	const cos_theta = a.0 * b.0 + a.1 * b.1 + a.2 * b.2 + a.3 * b.3;
+	const theta = math::acosf64(cos_theta);
+	const sin_theta = math::sinf64(theta);
+	const a_scale = math::sinf64((1.0 - t) * theta) / sin_theta;
+	const b_scale = math::sinf64(t * theta) / sin_theta;
+
+	return if (cos_theta < 0.0) (
+		a.0 * a_scale - b.0 * b_scale,
+		a.1 * a_scale - b.1 * b_scale,
+		a.2 * a_scale - b.2 * b_scale,
+		a.3 * a_scale - b.3 * b_scale,
+	) else (
+		a.0 * a_scale + b.0 * b_scale,
+		a.1 * a_scale + b.1 * b_scale,
+		a.2 * a_scale + b.2 * b_scale,
+		a.3 * a_scale + b.3 * b_scale,
+	);
+};
diff --git a/tools/gy-801-dump/main.ha b/tools/gy-801-dump/main.ha
index 9a3a294..f039173 100644
--- a/tools/gy-801-dump/main.ha
+++ b/tools/gy-801-dump/main.ha
@@ -1,3 +1,4 @@
+use ahrs::complementary;
 use ahrs::tilt;
 use ahrs::integration;
 use fmt;
@@ -63,18 +64,23 @@ export fn main() void = {
 	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);
+	const complementary = complementary::new(acc_readings, 0.02);
 
 	for (let i = 0; !exit; i += 1) {
 		timerfd::read(fd)!;
 		const gyro_readings = L3G4200D::read(L3G4200D)!;
+		const acc_readings = ADXL345::read(ADXL345)!;
 		let rps = (gyro_readings.0 / 180.0 * math::PI,
 			gyro_readings.1 / 180.0 * math::PI,
 			gyro_readings.2 / 180.0 * math::PI);
 		integration::update(integrator, rps, 0.01);
+		complementary::update(complementary, rps, 0.01);
+		complementary::update_accelerometer(complementary, acc_readings);
+
 		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);
+			let complementary_attitude = complementary::read_euler(complementary);
 
 			fmt::printfln("Accelerometer: {} {} {} g",
 				acc_readings.0, acc_readings.1, acc_readings.2)!;
@@ -88,6 +94,10 @@ export fn main() void = {
 				tilt_attitude.0 / math::PI * 180.0,
 				tilt_attitude.1 / math::PI * 180.0,
 				tilt_attitude.2 / math::PI * 180.0)!;
+			fmt::printfln("Attitude (complementary): {} {} {} deg",
+				complementary_attitude.0 / math::PI * 180.0,
+				complementary_attitude.1 / math::PI * 180.0,
+				complementary_attitude.2 / math::PI * 180.0)!;
 		};
 	};
 };
-- 
2.45.2