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

DCM: disable compass null offsets when setting initial yaw 

we need to ensure the compass null offsets code doesn't see a sudden
yaw change, or it will change the offsets by a large amount very
suddenly
This commit is contained in:
Andrew Tridgell 2012-02-25 16:02:20 +11:00
parent e2bbc795ad
commit 900388a85a
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_DCM/AP_DCM.cpp
View File

@ -407,7 +407,9 @@ AP_DCM::drift_correction(void)
_compass->calculate(_dcm_matrix);
// now construct a new DCM matrix
_compass->null_offsets_disable();
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, _compass->heading);
_compass->null_offsets_enable();
_have_initial_yaw = true;
}
} else if (_gps && _gps->status() == GPS::GPS_OK) {
@ -424,7 +426,13 @@ AP_DCM::drift_correction(void)
// DCM matrix to the current
// roll/pitch values, but with yaw
// from the GPS
if (!_compass) {
_compass->null_offsets_disable();
}
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course));
if (!_compass) {
_compass->null_offsets_enable();
}
_have_initial_yaw = true;
error_course = 0;
}