From a604e30c7c1a4b8f3071a8a08be7c02c9744638a Mon Sep 17 00:00:00 2001 From: "tridge60@gmail.com" Date: Wed, 10 Aug 2011 14:07:15 +0000 Subject: [PATCH] cope with double initialisation of the compass if we've already initialised, then the orientation matrix will already be right git-svn-id: https://arducopter.googlecode.com/svn/trunk@3072 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index f978b07585..a6abd59363 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -104,16 +104,27 @@ AP_Compass_HMC5843::init() } if ( base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { // a 5883L supports the sample averaging config - product_id = AP_COMPASS_TYPE_HMC5883L; - calibration_gain = 0x60; - expected_xy = 766; - expected_z = 713; + int old_product_id = product_id; + + product_id = AP_COMPASS_TYPE_HMC5883L; + calibration_gain = 0x60; + expected_xy = 766; + expected_z = 713; + + if (old_product_id != product_id) { + /* now we know the compass type we need to rotate the + * orientation matrix that we were given + */ + Matrix3f rot_mat = _orientation_matrix.get(); + _orientation_matrix.set_and_save(rot_mat * Matrix3f(ROTATION_YAW_90)); + } } else if (base_config == (NormalOperation | DataOutputRate_75HZ<<2)) { product_id = AP_COMPASS_TYPE_HMC5843; } else { // not behaving like either supported compass type return false; } + while( success == 0 && numAttempts < 5 ) {