4 files changed, 63 insertions(+), 0 deletions(-)

A ahrs/tilt/tilt.ha
A ahrs/tilt/types.ha
M math/quaternion/algebra.ha
M tools/gy-801-dump/main.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);
+};

@@ 0,0 1,7 @@
+use math::quaternion;
+
+export type tilt_impl = struct {
+	neutral: quaternion::quaternion,
+};
+
+export type tilt = *tilt_impl;

@@ 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);
+};

@@ 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)!;
 		};
 	};
 };