Browse Source

DCM: fixed the sense of the compass GPS test in initial yaw

we were only disabling null offsets when we didn't have a compass,
which doesn't make much sense!
mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
eb35e0e8ac
  1. 4
      libraries/AP_DCM/AP_DCM.cpp

4
libraries/AP_DCM/AP_DCM.cpp

@ -426,11 +426,11 @@ AP_DCM::drift_correction(void) @@ -426,11 +426,11 @@ AP_DCM::drift_correction(void)
// DCM matrix to the current
// roll/pitch values, but with yaw
// from the GPS
if (!_compass) {
if (_compass) {
_compass->null_offsets_disable();
}
rotation_matrix_from_euler(_dcm_matrix, roll, pitch, ToRad(_gps->ground_course));
if (!_compass) {
if (_compass) {
_compass->null_offsets_enable();
}
_have_initial_yaw = true;

Loading…
Cancel
Save