aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AHRS: if we don't have gps correction enabled, zero the yaw correction

Browse Source
This commit is contained in:
Andrew Tridgell 2012-07-20 16:23:56 +10:00
parent 613849a8df
commit 5f5ab9c07c
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -489,9 +489,13 @@ AP_AHRS_DCM::drift_correction(float deltat)
// to reduce the impact of two competing yaw controllers, we
// reduce the impact of the gps/accelerometers on yaw when we are
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles.
// accelerometers at high roll angles as long as we have a GPS
if (_compass && _compass->use_for_yaw()) {
error.z *= sin(fabs(roll));
if (_gps && _gps->status() == GPS::GPS_OK && gps_gain == 1.0) {
error.z *= sin(fabs(roll));
} else {
error.z = 0;
}
}
// convert the error term to body frame