diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 99655c9475..07144631a0 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -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);