From eff6778515c3ba3fa29f1eca1f7a8ee8fc556015 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Mar 2012 09:02:46 +1100 Subject: [PATCH] Quaternion: use gyro drift value from sensor driver --- libraries/AP_Quaternion/AP_Quaternion.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h index c1e2894a76..20028f1efe 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.h +++ b/libraries/AP_Quaternion/AP_Quaternion.h @@ -25,6 +25,10 @@ public: b_x = 0; b_z = -1; + // limit the drift to the drift rate reported by the + // sensor driver + gyroMeasDrift = imu->get_gyro_drift_rate(); + // scaled gyro drift limits beta = sqrt(3.0f / 4.0f) * gyroMeasError; zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift;