From 15d446bde23dfce0aded97a0910d15567df8b2eb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Jan 2012 15:48:07 +1100 Subject: [PATCH] DCM: prevent a segmentation fault when compass is not enabled when compass is disabled _compass is NULL --- libraries/AP_DCM/AP_DCM.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index fd201551c4..abf00d9ba9 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -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) _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 + } } /*************************************************