From e93ff44a97657e94b111b784acd02db65f2827e2 Mon Sep 17 00:00:00 2001 From: Staroselskii Georgii Date: Mon, 24 Aug 2015 17:53:17 +0300 Subject: [PATCH] AP_Compass: make a transition to milligauss in Compass library --- libraries/AP_Compass/Compass.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index eee00bd6a5..decaadd938 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -517,6 +517,9 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets) if (i < COMPASS_MAX_INSTANCES) { _state[i].offset.set(offsets); save_offsets(i); + + /* Ugly hack to update offsets in milligauss that are going to be used across all the codebase in the future */ + _state[i].offset_milligauss = offsets * _backends[i]->get_conversion_ratio(); } } @@ -624,7 +627,7 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x); // Tilt compensated magnetic field Y component: - const Vector3f &field = get_field(); + const Vector3f &field = get_field_milligauss(); float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y; @@ -664,6 +667,11 @@ bool Compass::configured(uint8_t i) return false; } + // exit immediately if all offsets (mG) are zero + if (is_zero(get_offsets_milligauss(i).length())) { + return false; + } + #if COMPASS_MAX_INSTANCES > 1 // backup detected dev_id int32_t dev_id_orig = _state[i].dev_id;