compass: fixed output rate and averaging for 5883L

we need to setup the right output rate, or the compass gets very laggy

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2736 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-07-03 08:32:58 +00:00
parent a61ac47050
commit cde84445b3
1 changed files with 21 additions and 1 deletions

View File

@ -31,6 +31,22 @@
#define ContinuousConversion 0x00
#define SingleConversion 0x01
// ConfigRegA valid sample averaging for 5883L
#define SampleAveraging_1 0x00
#define SampleAveraging_2 0x01
#define SampleAveraging_4 0x02
#define SampleAveraging_8 0x03
// ConfigRegA valid data output rates for 5883L
#define DataOutputRate_0_75HZ 0x00
#define DataOutputRate_1_5HZ 0x01
#define DataOutputRate_3HZ 0x02
#define DataOutputRate_7_5HZ 0x03
#define DataOutputRate_15HZ 0x04
#define DataOutputRate_30HZ 0x05
#define DataOutputRate_75HZ 0x06
// Public Methods //////////////////////////////////////////////////////////////
bool
AP_Compass_HMC5843::init()
@ -59,7 +75,11 @@ AP_Compass_HMC5843::init()
// force positiveBias (compass should return 715 for all channels)
Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ConfigRegA);
Wire.send(PositiveBiasConfig);
if (product_id == AP_COMPASS_TYPE_HMC5843) {
Wire.send(PositiveBiasConfig);
} else {
Wire.send(SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | PositiveBiasConfig);
}
if (0 != Wire.endTransmission())
continue; // compass not responding on the bus
delay(50);