Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Doug Weibel 2011-11-05 09:01:20 -06:00
parent c7077fa9e8
commit 947950398c
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_DCM/AP_DCM.cpp
View File

@ -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("*");
}