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

AHRS: force an extra read of the compass on startup 

the first read from the compass can be bad. This ensures we have a
good value when getting the initial AHRS yaw.

Thanks to Randy and Jason for the bug report!
This commit is contained in:
Andrew Tridgell 2012-08-17 17:57:39 +10:00
parent 8853a104f0
commit 7948b3aee7
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -325,7 +325,10 @@ AP_AHRS_DCM::drift_correction_yaw(void)
if (_compass->last_update != _compass_last_update) { if (_compass->last_update != _compass_last_update) {
yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6; yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6;
_compass_last_update = _compass->last_update; _compass_last_update = _compass->last_update;
if (!_have_initial_yaw) { // we force an additional compass read()
// here. This has the effect of throwing away
// the first compass value, which can be bad
if (!_have_initial_yaw && _compass->read()) {
float heading = _compass->calculate_heading(_dcm_matrix); float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading); _dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero(); _omega_yaw_P.zero();