From 9e248751311b00bc03e308115cc909388da175d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 23 Apr 2018 16:22:57 +0200 Subject: [PATCH] attitude_estimator_q: fuse accel data only if close to 1g if GPS velocity not used This reduces attitude drift on vehicles w/o GPS. The default behavior is unchanged, to use it, disable ATT_ACC_COMP. --- .../attitude_estimator_q_main.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 34b22bdb18..0780cb5cfd 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -606,7 +606,16 @@ bool AttitudeEstimatorQ::update(float dt) (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) ); - corr += (k % (_accel - _pos_acc).normalized()) * _w_accel; + // If we are not using acceleration compensation based on GPS velocity, + // fuse accel data only if its norm is close to 1 g (reduces drift). + const float accel_norm_sq = _accel.length_squared(); + const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f; + const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f; + + if (_acc_comp || (accel_norm_sq > lower_accel_limit * lower_accel_limit && + accel_norm_sq < upper_accel_limit * upper_accel_limit)) { + corr += (k % (_accel - _pos_acc).normalized()) * _w_accel; + } // Gyro bias estimation if (spinRate < 0.175f) {