Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 18:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

cope with double initialisation of the compass

Browse Source 
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
This commit is contained in:
tridge60@gmail.com 2011-08-10 14:07:15 +00:00
parent 19d2195ea3
commit 4f099ae158
1 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -104,16 +104,27 @@ AP_Compass_HMC5843::init()
} }
if ( base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { if ( base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config // a 5883L supports the sample averaging config
product_id = AP_COMPASS_TYPE_HMC5883L; int old_product_id = product_id;
calibration_gain = 0x60;
expected_xy = 766; product_id = AP_COMPASS_TYPE_HMC5883L;
expected_z = 713; 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)) { } else if (base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843; product_id = AP_COMPASS_TYPE_HMC5843;
} else { } else {
// not behaving like either supported compass type // not behaving like either supported compass type
return false; return false;
} }
while( success == 0 && numAttempts < 5 ) while( success == 0 && numAttempts < 5 )
{ {