From c98682fdcb9334d4dc1df85726e361fe064968e0 Mon Sep 17 00:00:00 2001
From: xdavidwu <xdavidwuph@gmail.com>
Date: Tue, 17 May 2022 21:18:01 +0800
Subject: [PATCH] gy-801-dump: perform quaternion forward integration

---
 ahrs/integration/integration.ha | 24 ++++++++++++++++++++++++
 ahrs/integration/types.ha       |  7 +++++++
 math/quaternion/algebra.ha      |  8 ++++++++
 math/quaternion/conversion.ha   | 27 +++++++++++++++++++++++++++
 math/quaternion/types.ha        |  3 +++
 tools/gy-801-dump/main.ha       | 12 ++++++++++++
 6 files changed, 81 insertions(+)
 create mode 100644 ahrs/integration/integration.ha
 create mode 100644 ahrs/integration/types.ha
 create mode 100644 math/quaternion/algebra.ha
 create mode 100644 math/quaternion/conversion.ha
 create mode 100644 math/quaternion/types.ha

diff --git a/ahrs/integration/integration.ha b/ahrs/integration/integration.ha
new file mode 100644
index 0000000..a3cafeb
--- /dev/null
+++ b/ahrs/integration/integration.ha
@@ -0,0 +1,24 @@
+use math;
+use math::quaternion;
+
+export fn new() integration = {
+	return alloc(integration_impl {
+		q = (1.0, 0.0, 0.0, 0.0),
+	});
+};
+
+export fn update(integration: integration, readings: (f64, f64, f64), dt: f64) void = {
+	let angle = math::sqrtf64(readings.0 * readings.0
+		+ readings.1 * readings.1 + readings.2 * readings.2);
+	let axis = (readings.0 / angle, readings.1 / angle, readings.2 / angle);
+	let q = quaternion::from_axis_angle(axis, angle * dt);
+	integration.q = quaternion::product(integration.q, q);
+};
+
+export fn read_euler(integration: integration) (f64, f64, f64) = {
+	return quaternion::to_euler(integration.q);
+};
+
+export fn destroy(integration: integration) void = {
+	free(integration);
+};
diff --git a/ahrs/integration/types.ha b/ahrs/integration/types.ha
new file mode 100644
index 0000000..8903210
--- /dev/null
+++ b/ahrs/integration/types.ha
@@ -0,0 +1,7 @@
+use math::quaternion;
+
+export type integration_impl = struct {
+	q: quaternion::quaternion,
+};
+
+export type integration = *integration_impl;
diff --git a/math/quaternion/algebra.ha b/math/quaternion/algebra.ha
new file mode 100644
index 0000000..aa76a48
--- /dev/null
+++ b/math/quaternion/algebra.ha
@@ -0,0 +1,8 @@
+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,
+		a.0 * b.1 + a.1 * b.0 + a.2 * b.3 - a.3 * b.2,
+		a.0 * b.2 - a.1 * b.3 + a.2 * b.0 + a.3 * b.1,
+		a.0 * b.3 + a.1 * b.2 - a.2 * b.1 + a.3 * b.0 ,
+	);
+};
diff --git a/math/quaternion/conversion.ha b/math/quaternion/conversion.ha
new file mode 100644
index 0000000..c5df741
--- /dev/null
+++ b/math/quaternion/conversion.ha
@@ -0,0 +1,27 @@
+use fmt;
+use math;
+
+// quarternion from axis-angle, angle is in radians
+export fn from_axis_angle(axis: (f64, f64, f64), angle: f64) quaternion = {
+	const angle = angle / 2.0f64;
+	const s = math::sinf64(angle);
+	return (math::cosf64(angle), axis.0 * s, axis.1 * s, axis.2 * s);
+};
+
+export fn to_euler(q: quaternion) (f64, f64, f64) = {
+	return (
+		math::atanf64(2.0 * (q.0 * q.1 + q.2 * q.3) / (1.0 - 2.0 * (q.1 * q.1 + q.2 * q.2))),
+		math::asinf64(2.0 * (q.0 * q.2 - q.3 * q.1)),
+		math::atanf64(2.0 * (q.0 * q.3 + q.1 * q.2) / (1.0 - 2.0 * (q.2 * q.2 + q.3 * q.3))),
+	);
+};
+
+@test fn conversion() void = {
+	let euler = (2.0 / 180.0 * math::PI, 3.0 / 180.0 * math::PI, 4.0 / 180.0 * math::PI);
+	fmt::println(euler.0, euler.1, euler.2)!;
+	let angle = math::sqrtf64(euler.0 * euler.0 + euler.1 * euler.1 + euler.2 * euler.2);
+	let axis = (euler.0 / angle, euler.1 / angle, euler.2 / angle);
+	let q = from_axis_angle(axis, angle);
+	let res = to_euler(q);
+	fmt::println(res.0, res.1, res.2)!;
+};
diff --git a/math/quaternion/types.ha b/math/quaternion/types.ha
new file mode 100644
index 0000000..efc1cdf
--- /dev/null
+++ b/math/quaternion/types.ha
@@ -0,0 +1,3 @@
+export type quaternion = (f64, f64, f64, f64);
+
+//export def IDENTITY: quaternion	= (1.0, 0.0, 0.0, 0.0);
diff --git a/tools/gy-801-dump/main.ha b/tools/gy-801-dump/main.ha
index ea5cd16..f4293fa 100644
--- a/tools/gy-801-dump/main.ha
+++ b/tools/gy-801-dump/main.ha
@@ -1,9 +1,11 @@
+use ahrs::integration;
 use fmt;
 use fs;
 use io;
 use os;
 use rt;
 use linux::timerfd;
+use math;
 use sensors::ADXL345;
 use sensors::L3G4200D;
 use strconv;
@@ -54,13 +56,23 @@ export fn main() void = {
 	defer io::close(fd)!;
 	timerfd::set(fd, (500 * time::MILLISECOND): timerfd::interval, 0)!;
 	signal::handle(signal::SIGINT, &sigint);
+	const integrator = integration::new();
 
 	for (!exit) {
 		timerfd::read(fd)!;
 		const acc_readings = ADXL345::read(ADXL345)!;
 		const gyro_readings = L3G4200D::read(L3G4200D)!;
+		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.5);
+		let attitude = integration::read_euler(integrator);
 
 		fmt::printfln("Accelerometer: {} {} {} g", acc_readings.0, acc_readings.1, acc_readings.2)!;
 		fmt::printfln("Gyroscope: {} {} {} dps", gyro_readings.0, gyro_readings.1, gyro_readings.2)!;
+		fmt::printfln("Attitude: {} {} {} deg",
+			attitude.0 / math::PI * 180.0,
+			attitude.1 / math::PI * 180.0,
+			attitude.2 / math::PI * 180.0)!;
 	};
 };
-- 
2.45.2