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

AHRS: allow the gps/accelerometers to control yaw when rolled

Browse Source 
this solves a problem with yaw estimation when at high roll angles,
when the compass code becomes ineffective
This commit is contained in:
Andrew Tridgell 2012-07-20 11:49:40 +10:00
parent 404803a734
commit 4f41b876b1
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -486,11 +486,12 @@ AP_AHRS_DCM::drift_correction(float deltat)
error.z = earth_error_Z; error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION #endif // YAW_INDEPENDENT_DRIFT_CORRECTION
// only use the gps/accelerometers for earth frame yaw correction // to reduce the impact of two competing yaw controllers, we
// if we are not using a compass. Otherwise we have two competing // reduce the impact of the gps/accelerometers on yaw when we are
// controllers for yaw correction // flat, but still allow for yaw correction using the
// accelerometers at high roll angles.
if (_compass && _compass->use_for_yaw()) { if (_compass && _compass->use_for_yaw()) {
error.z = 0; error.z *= sin(fabs(roll));
} }
// convert the error term to body frame // convert the error term to body frame