From f61ae9e9e52216eb96e2b480bd119c294dbe83b3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Oct 2014 11:15:33 +1100 Subject: [PATCH] AP_AHRS: restore DCM attitude before update() The DCM drift correction code uses the current attitude to calculate error values to update its gyro drift correction. If we were using EKF then without this patch the DCM code running as an alternative AHRS source would be using the EKF attitude for calculating the error value, leading to very bad gyro drift estimation --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 2930194bd3..6f872f59c2 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -52,6 +52,16 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const void AP_AHRS_NavEKF::update(void) { + // we need to restore the old DCM attitude values as these are + // used internally in DCM to calculate error values for gyro drift + // correction + roll = _dcm_attitude.x; + pitch = _dcm_attitude.y; + yaw = _dcm_attitude.z; + roll_sensor = degrees(roll)*100; + pitch_sensor = degrees(pitch)*100; + yaw_sensor = degrees(yaw)*100; + AP_AHRS_DCM::update(); // keep DCM attitude available for get_secondary_attitude()