5 files changed, 77 insertions(+), 1 deletions(-)

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

@@ 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;

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

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

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