From beb6c4dbf43162da8b72959906967119743a5983 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 5 Nov 2011 09:01:20 -0600 Subject: [PATCH] Changed the DCM drift correction integrator limit to a vector magnitude of 30 degrees/second. The drift correction integrator limit previously was near/at the gyro saturation limit. If we have that much drift there is a serious hardware problem. 30 degrees/second is arbitrary but should handle all temperature variation, etc. --- libraries/AP_DCM/AP_DCM.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index c0ed1f2f00..2d881e5866 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -343,10 +343,10 @@ AP_DCM::drift_correction(void) _omega_P += _error_yaw * _kp_yaw; // Adding yaw correction to proportional correction vector. _omega_I += _error_yaw * _ki_yaw; // adding yaw correction to integrator correction vector. - // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros + // Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second integrator_magnitude = _omega_I.length(); - if (integrator_magnitude > radians(300)) { - _omega_I *= (0.5f * radians(300) / integrator_magnitude); // Why do we have this discontinuous? EG, why the 0.5? + if (integrator_magnitude > radians(30)) { + _omega_I *= (radians(30) / integrator_magnitude); } //Serial.print("*"); }