From 0641ccfca1114f6d0adedbd98c8be68e46315442 Mon Sep 17 00:00:00 2001 From: xdavidwu Date: Fri, 20 May 2022 15:29:10 +0800 Subject: [PATCH] ahrs: complementary: ignore yaw from tilt --- ahrs/complementary/complementary.ha | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ahrs/complementary/complementary.ha b/ahrs/complementary/complementary.ha index 892fd33..3d18ad5 100644 --- a/ahrs/complementary/complementary.ha +++ b/ahrs/complementary/complementary.ha @@ -17,11 +17,13 @@ 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( + let accel = quaternion::to_euler(tilt::estimate(complementary.tilt, readings)); + let orig = quaternion::to_euler(complementary.integration.q); + let qc = quaternion::from_euler((accel.0, accel.1, orig.2)); + complementary.integration.q = quaternion::normalize(quaternion::slerp( complementary.integration.q, - q_accel, - complementary.alpha); + qc, + complementary.alpha)); }; export fn destroy(complementary: complementary) void = { -- 2.45.2