Browse Source

DCM: prevent a segmentation fault when compass is not enabled

when compass is disabled _compass is NULL
master
Andrew Tridgell 13 years ago
parent
commit
15d446bde2
  1. 11
      libraries/AP_DCM/AP_DCM.cpp

11
libraries/AP_DCM/AP_DCM.cpp

@ -195,7 +195,9 @@ AP_DCM::accel_adjust(void) @@ -195,7 +195,9 @@ AP_DCM::accel_adjust(void)
void
AP_DCM::matrix_reset(void)
{
_compass->null_offsets_disable();
if (_compass != NULL) {
_compass->null_offsets_disable();
}
_dcm_matrix.a.x = 1.0f;
_dcm_matrix.a.y = 0.0f;
_dcm_matrix.a.z = 0.0f;
@ -208,8 +210,11 @@ AP_DCM::matrix_reset(void) @@ -208,8 +210,11 @@ AP_DCM::matrix_reset(void)
_omega_I.x = 0.0f;
_omega_I.y = 0.0f;
_omega_I.z = 0.0f;
_compass->null_offsets_enable(); // This call is needed to restart the nulling
// Otherwise the reset in the DCM matrix can mess up the nulling
if (_compass != NULL) {
_compass->null_offsets_enable(); // This call is needed to restart the nulling
// Otherwise the reset in the DCM matrix can mess up
// the nulling
}
}
/*************************************************

Loading…
Cancel
Save