From 900388a85a76327e745edaba526630542bc0ed55 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 25 Feb 2012 16:02:20 +1100 Subject: [PATCH] DCM: disable compass null offsets when setting initial yaw we need to ensure the compass null offsets code doesn't see a sudden yaw change, or it will change the offsets by a large amount very suddenly --- libraries/AP_DCM/AP_DCM.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 8d591ff4d3..d82124aa3d 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -407,7 +407,9 @@ AP_DCM::drift_correction(void) _compass->calculate(_dcm_matrix); // now construct a new DCM matrix + _compass->null_offsets_disable(); rotation_matrix_from_euler(_dcm_matrix, roll, pitch, _compass->heading); + _compass->null_offsets_enable(); _have_initial_yaw = true; } } else if (_gps && _gps->status() == GPS::GPS_OK) { @@ -424,7 +426,13 @@ AP_DCM::drift_correction(void) // DCM matrix to the current // roll/pitch values, but with yaw // from the GPS + if (!_compass) { + _compass->null_offsets_disable(); + } rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course)); + if (!_compass) { + _compass->null_offsets_enable(); + } _have_initial_yaw = true; error_course = 0; }