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; }