From a69ecfa06b48a1d8dbfa39a37892f77ccdf5182f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
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