void initCompass() {
#ifdef MPU6050
initMpu6050();
#endif
bool bret = true;
write(HMC58X3_R_CONFB, 2 << 5);
write(HMC58X3_R_MODE, 1);
delay(100);
//get one sample and discard it
while (!readRawAxis());
if (bias_collect(0x010 + HMC_POS_BIAS)) bret = false;
if (bias_collect(0x010 + HMC_NEG_BIAS)) bret = false;
if (bret){
for (uint8_t axis=0; axis<3; axis++)
magGain[axis]=820.0*HMC58X3_X_SELF_TEST_GAUSS*2.0*10.0/xyz_total[axis]; // note: xyz_total[axis] is always positive
}else{
magGain[0] = 1.0;
magGain[1] = 1.0;
magGain[2] = 1.0;
}
write(HMC58X3_R_CONFA , 0x78 ); //Configuration Register A -- output rate: 75Hz ; normal measurement mode
write(HMC58X3_R_CONFB , 0x20 ); //Configuration Register B -- configuration gain 1.3Ga
write(HMC58X3_R_MODE , 0x00 ); //Mode register -- continuous Conversion Mode
delay(100);
for (uint8_t axis = 0; axis < 3; axis++) {
magZero[axis] = LoadIntegerFromEEPROM(axis * 2);
}
}
void calibrate_compass() {
static int16_t magZeroTempMin[3];
static int16_t magZeroTempMax[3];
byte axis = 0;
while (!readRawAxis());
for (axis = 0; axis < 3; axis++) {
magZero[axis] = 0;
magZeroTempMin[axis] = magADC[axis];
magZeroTempMax[axis] = magADC[axis];
}
timer = millis();
SET_PAN_SERVO_SPEED(2000);
while (millis() - timer < 5000) {
while (!readRawAxis());
for (axis = 0; axis < 3; axis++) {
if (magADC[axis] < magZeroTempMin[axis]) {
magZeroTempMin[axis] = magADC[axis];
}
if (magADC[axis] > magZeroTempMax[axis]) {
magZeroTempMax[axis] = magADC[axis];
}
}
delay(14);
}
SET_PAN_SERVO_SPEED(PAN_0);
delay(1000);
SET_PAN_SERVO_SPEED(1000);
timer = millis();
while (millis() - timer < 5000) {
while (!readRawAxis());
for (axis = 0; axis < 3; axis++) {
if (magADC[axis] < magZeroTempMin[axis]) {
magZeroTempMin[axis] = magADC[axis];
}
if (magADC[axis] > magZeroTempMax[axis]) {
magZeroTempMax[axis] = magADC[axis];
}
}
delay(14);
}
SET_PAN_SERVO_SPEED(PAN_0);
for (axis = 0; axis < 3; axis++) {
magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) >> 1;
StoreIntegerToEEPROM(magZero[axis], axis * 2);
}
}
MagnetometerScaled HMC5883L::readScaledAxis()
{
MagnetometerRaw raw = readRawAxis();
MagnetometerScaled scaled = MagnetometerScaled();
scaled.XAxis = raw.XAxis * m_Scale;
scaled.ZAxis = raw.ZAxis * m_Scale;
scaled.YAxis = raw.YAxis * m_Scale;
return scaled;
}
MagnetometerScaled Compass::readScaledAxis()
{
MagnetometerRaw raw = readRawAxis();
MagnetometerScaled scaled = MagnetometerScaled();
scaled.XAxis = raw.XAxis * scale_;
scaled.ZAxis = raw.ZAxis * scale_;
scaled.YAxis = raw.YAxis * scale_;
return scaled;
}
bool HMC5883L::readScaledAxis()
{
// first get raw readings
if(!readRawAxis()) {
printf("ERROR in readScaledAxis of %s\n", deviceName );
return false;
}
scaled.XAxis = ( double ) raw.XAxis * scale;
scaled.YAxis = ( double ) raw.YAxis * scale;
scaled.ZAxis = ( double ) raw.ZAxis * scale;
return true;
}
static uint8_t bias_collect(uint8_t bias) {
int16_t abs_magADC;
write(HMC58X3_R_CONFA, bias); // Reg A DOR=0x010 + MS1,MS0 set to pos or negative bias
for (uint8_t i=0; i<10; i++) { // Collect 10 samples
write(HMC58X3_R_MODE, 1);
delay(100);
while(!readRawAxis()); // Get the raw values in case the scales have already been changed.
for (uint8_t axis=0; axis<3; axis++) {
abs_magADC = abs(magADC[axis]);
xyz_total[axis]+= abs_magADC; // Since the measurements are noisy, they should be averaged rather than taking the max.
if ((int16_t)(1<<12) < abs_magADC) return false; // Detect saturation. if false Breaks out of the for loop. No sense in continuing if we saturated.
}
}
return true;
}
int getHeading() {
while (!readRawAxis());
/*smoothed[0] = (digitalSmooth(magADC[0], xSmooth) * magGain[0]) - magZero[0];
smoothed[1] = (digitalSmooth(magADC[1], ySmooth) * magGain[1]) - magZero[1];
smoothed[2] = (digitalSmooth(magADC[2], zSmooth) * magGain[2]) - magZero[2];*/
//double heading = atan2(smoothed[1], smoothed[0]) ;
double heading = atan2((magADC[1]* magGain[1]) - magZero[1], (magADC[0]* magGain[0]) - magZero[0]) ;
if (heading < 0)
heading += 2 * M_PI;
if (heading > 2 * M_PI)
heading -= 2 * M_PI;
return (int) ((heading * 1800.0 / M_PI) + DECLINATION + OFFSET) % 3600;
}
