From cb52b6f36729a93421965fc83fa1afd20d80c0ab Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Nov 2013 13:59:38 +1100 Subject: [PATCH] AP_AHRS: removed limit on normalisation of accel reference vectors this could lead to a bias in the accel drift correction --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 524ab0b7fa..a212deb126 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -561,14 +561,12 @@ AP_AHRS_DCM::drift_correction(float deltat) // calculate the error term in earth frame. Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS); - float length = GA_b.length(); - if (length > 1.0f) { - GA_b /= length; - if (GA_b.is_inf()) { - // wait for some non-zero acceleration information - return; - } + GA_b.normalize(); + if (GA_b.is_inf()) { + // wait for some non-zero acceleration information + return; } + Vector3f error = GA_b % GA_e; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0