/** Get 3-axis heading measurements.
* In the event the ADC reading overflows or underflows for the given channel,
* or if there is a math overflow during the bias measurement, this data
* register will contain the value -4096. This register value will clear when
* after the next valid measurement is made. Note that this method automatically
* clears the appropriate bit in the MODE register if Single mode is active.
* @param x 16-bit signed integer container for X-axis heading
* @param y 16-bit signed integer container for Y-axis heading
* @param z 16-bit signed integer container for Z-axis heading
* @see HMC5883L_RA_DATAX_H
*/
void HMC5883L::getHeading(int16_t *x, int16_t *y, int16_t *z) {
int16_t rawx, rawy, rawz;
getRawHeading(&rawx, &rawy, &rawz);
*x = (int16_t) (scaleFactors[gain][0]*rawx);
*y = (int16_t) (scaleFactors[gain][1]*rawy);
*z = (int16_t) (scaleFactors[gain][2]*rawz);
}
// --------------------------------------------------------------------------
// This Function calculates the offset in the Magnetometer
// using Positive and Negative bias Self test capability
// This function updates X_offset Y_offset and Z_offset Global variables
// Call Initialize before
void HMC5883L::calibration_offset(int select)
{
int compass_x=0, compass_y=0, compass_z=0;
float compass_x_scaled=0, compass_y_scaled=0, compass_z_scaled=0;
int i = 0;
// ***********************************************************
// offset_calibration() function performs two taskes
// 1. It calculates the diffrence in the gain of the each axis magnetometer axis, using
// inbuilt self excitation function of HMC5883L (Which is useless if it is used as a compass
// unless you are very unlucy and got a crapy sensor or live at very High or low temperature)
// 2. It calculates the mean of each axes magnetic field, when the Magnetometer is rotated 360 degree
// 3. Do Both
// ***********************************************************
// *****************************************************************************************
// Gain offset estimation
// *****************************************************************************************
if (select == 1 | select == 3)
{
// User input in the function
// Configuring the Control register for Positive Bais mode
Serial.print(" Calibrating the Magnetometer (Gain) ");
Wire.beginTransmission(devAddr);
Wire.write(0x00);
Wire.write(0b01110001); // bit configuration = 0 A A DO2 DO1 DO0 MS1 MS2
/*
A A DO2 DO1 DO0 Sample Rate [Hz] MS1 MS0 Measurment Mode
0 0 = No Average 0 0 0 = 0.75 0 0 = Normal
0 1 = 2 Sample average 0 0 1 = 1.5 0 1 = Positive Bias
1 0 = 4 Sample Average 0 1 0 = 3 1 0 = Negative Bais
1 1 = 8 Sample Average 0 1 1 = 7.5 1 1 = -
1 0 0 = 15 (Default)
1 0 1 = 30
1 1 0 = 75
1 1 1 = -
*/
Wire.endTransmission();
for(i=0 ; i<50 ; i++)
{
getRawHeading(&compass_x, &compass_y, &compass_z); // Disregarding the first data
delay(10);
}
// Reading the Positive baised Data
while(compass_x<200 | compass_y<200 | compass_z<200)
{
// Making sure the data is with Positive baised
getRawHeading(&compass_x, &compass_y, &compass_z);
delay(10);
Serial.print(".");
}
compass_x_scaled = compass_x * mgPerDigit;
compass_y_scaled = compass_y * mgPerDigit;
compass_z_scaled = compass_z * mgPerDigit;
// Offset = 1160 - Data_positive
compass_x_gainError = (float)COMPASS_XY_EXCITATION / compass_x_scaled;
compass_y_gainError = (float)COMPASS_XY_EXCITATION / compass_y_scaled;
compass_z_gainError = (float)COMPASS_Z_EXCITATION / compass_z_scaled;
// Configuring the Control register for Negative Bais mode
Wire.beginTransmission(devAddr);
Wire.write(0x00);
Wire.write(0b01110010); // bit configuration = 0 A A DO2 DO1 DO0 MS1 MS2
/*
A A DO2 DO1 DO0 Sample Rate [Hz] MS1 MS0 Measurment Mode
0 0 = No Average 0 0 0 = 0.75 0 0 = Normal
0 1 = 2 Sample average 0 0 1 = 1.5 0 1 = Positive Bias
1 0 = 4 Sample Average 0 1 0 = 3 1 0 = Negative Bais
1 1 = 8 Sample Average 0 1 1 = 7.5 1 1 = -
1 0 0 = 15 (Default)
1 0 1 = 30
1 1 0 = 75
1 1 1 = -
*/
Wire.endTransmission();
for(i=0 ; i<50 ; i++)
{
getRawHeading(&compass_x, &compass_y, &compass_z); // Disregarding the first data
delay(10);
}
// Reading the Negative baised Data
while(compass_x>-200 | compass_y>-200 | compass_z>-200){ // Making sure the data is with negative baised
getRawHeading(&compass_x, &compass_y, &compass_z);
Serial.print(".");
}
compass_x_scaled = compass_x * mgPerDigit;
compass_y_scaled = compass_y * mgPerDigit;
compass_z_scaled = compass_z * mgPerDigit;
// Taking the average of the offsets
maggainerr[0] = (float)((COMPASS_XY_EXCITATION / abs(compass_x_scaled)) + compass_x_gainError) / 2;
maggainerr[1] = (float)((COMPASS_XY_EXCITATION / abs(compass_y_scaled)) + compass_y_gainError) / 2;
maggainerr[2] = (float)((COMPASS_Z_EXCITATION / abs(compass_z_scaled)) + compass_z_gainError) / 2;
Serial.println("");
Serial.print(" Gain:");
Serial.print(maggainerr[0]);Serial.print(", ");
Serial.print(maggainerr[1]);Serial.print(", ");
Serial.println(maggainerr[2]);
Serial.println(" Done");
}
// Configuring the Control register for normal mode
Wire.beginTransmission(devAddr);
Wire.write(0x00);
Wire.write(0b01111000); // bit configuration = 0 A A DO2 DO1 DO0 MS1 MS2
/*
A A DO2 DO1 DO0 Sample Rate [Hz] MS1 MS0 Measurment Mode
0 0 = No Average 0 0 0 = 0.75 0 0 = Normal
0 1 = 2 Sample average 0 0 1 = 1.5 0 1 = Positive Bias
1 0 = 4 Sample Average 0 1 0 = 3 1 0 = Negative Bais
1 1 = 8 Sample Average 0 1 1 = 7.5 1 1 = -
1 0 0 = 15 (Default)
1 0 1 = 30
1 1 0 = 75
1 1 1 = -
*/
Wire.endTransmission();
// *****************************************************************************************
// Offset estimation
// *****************************************************************************************
if (select == 2 | select == 3)
{
Serial.println(" Calibrating the Magnetometer (Offset) ");
// User input in the function
for(i=0 ; i<50 ; i++)
{
getRawHeading(&compass_x, &compass_y, &compass_z); // Disregarding the first data
delay(10);
}
float x_max=-4000,y_max=-4000,z_max=-4000;
float x_min=4000,y_min=4000,z_min=4000;
float last_time = 0;
unsigned long t = millis();
Serial.println(" Rotate Sensor for 20 Sec. ");
while(millis()-t <= 20000)
{
getRawHeading(&compass_x, &compass_y, &compass_z);
compass_x_scaled = (float)compass_x * mgPerDigit * maggainerr[0];
compass_y_scaled = (float)compass_y * mgPerDigit * maggainerr[1];
compass_z_scaled = (float)compass_z * mgPerDigit * maggainerr[2];
x_max = max(x_max, compass_x_scaled);
y_max = max(y_max, compass_y_scaled);
z_max = max(z_max, compass_z_scaled);
x_min = min(x_min, compass_x_scaled);
y_min = min(y_min, compass_y_scaled);
z_min = min(z_min, compass_z_scaled);
if(((millis()-t) - last_time) > 1000)
{
last_time = millis() - t;
Serial.print(".");
}
delay(10);
}
magoffset[0] = ((x_max - x_min) / 2) - x_max;
magoffset[1] = ((y_max - y_min) / 2) - y_max;
magoffset[2] = ((z_max - z_min) / 2) - z_max;
Serial.println("");
Serial.print(" Offset:");
Serial.print(magoffset[0]);Serial.print(", ");
Serial.print(magoffset[1]);Serial.print(", ");
Serial.println(magoffset[2]);
Serial.println(" Done");
}
}
bool HMC5883L::calibrate(int8_t testGain) {
// Keep the current status ...
uint8_t previousGain = getGain();
// Set the gain
if (testGain < 0) {
testGain = gain;
}
setGain(testGain);
// To check the HMC5883L for proper operation, a self test
// feature in incorporated in which the sensor offset straps
// are excited to create a nominal field strength (bias field)
// to be measured. To implement self test, the least significant
// bits (MS1 and MS0) of configuration register A are changed
// from 00 to 01 (positive bias) or 10 (negetive bias)
setMeasurementBias(HMC5883L_BIAS_POSITIVE);
// Then, by placing the mode register into single-measurement mode ...
setMode(HMC5883L_MODE_SINGLE);
// Two data acquisition cycles will be made on each magnetic vector.
// The first acquisition will be a set pulse followed shortly by
// measurement data of the external field. The second acquisition
// will have the offset strap excited (about 10 mA) in the positive
// bias mode for X, Y, and Z axes to create about a ±1.1 gauss self
// test field plus the external field.
// The first acquisition values will be subtracted from the
// second acquisition, and the net measurement will be placed into
// the data output registers.
int16_t x,y,z;
getRawHeading(&x,&y,&z);
// In the event the ADC reading overflows or underflows for the
// given channel, or if there is a math overflow during the bias
// measurement, this data register will contain the value -4096.
// This register value will clear when after the next valid
// measurement is made.
if (min(x,min(y,z)) == -4096) {
scaleFactors[testGain][0] = 1.0f;
scaleFactors[testGain][1] = 1.0f;
scaleFactors[testGain][2] = 1.0f;
return false;
}
getRawHeading(&x,&y,&z);
if (min(x,min(y,z)) == -4096) {
scaleFactors[testGain][0] = 1.0f;
scaleFactors[testGain][1] = 1.0f;
scaleFactors[testGain][2] = 1.0f;
return false;
}
// Since placing device in positive bias mode
// (or alternatively negative bias mode) applies
// a known artificial field on all three axes,
// the resulting ADC measurements in data output
// registers can be used to scale the sensors.
float xExpectedSelfTestValue =
HMC5883L_SELF_TEST_X_AXIS_ABSOLUTE_GAUSS *
HMC5883L_LSB_PER_GAUS[testGain];
float yExpectedSelfTestValue =
HMC5883L_SELF_TEST_Y_AXIS_ABSOLUTE_GAUSS *
HMC5883L_LSB_PER_GAUS[testGain];
float zExpectedSelfTestValue =
HMC5883L_SELF_TEST_Z_AXIS_ABSOLUTE_GAUSS *
HMC5883L_LSB_PER_GAUS[testGain];
scaleFactors[testGain][0] = xExpectedSelfTestValue/x;
scaleFactors[testGain][1] = yExpectedSelfTestValue/y;
scaleFactors[testGain][2] = zExpectedSelfTestValue/z;
setGain(previousGain);
setMeasurementBias(HMC5883L_BIAS_NORMAL);
return true;
}
/** Get raw Z-axis heading measurement.
* @return 16-bit signed integer with Z-axis heading
* @see HMC5883L_RA_DATAZ_H
*/
int16_t HMC5883L::getRawHeadingZ() {
int16_t x,y,z;
getRawHeading(&x,&y,&z);
return z;
}
