static bool hmc5883lInit(magDev_t *mag)
{
busDevice_t *busdev = &mag->busdev;
// leave test mode
busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
hmc5883lConfigureDataReadyInterruptHandling(mag);
return true;
}
void hmc5883lInit(void)
{
int16_t magADC[3];
int i;
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
bool bret = true; // Error indicator
gpio_config_t gpio;
if (hmc5883Config) {
#ifdef STM32F303
if (hmc5883Config->gpioAHBPeripherals) {
RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE);
}
#endif
#ifdef STM32F10X
if (hmc5883Config->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE);
}
#endif
#ifdef STM32F40_41xxx
if (hmc5883Config->gpioAHB1Peripherals) {
RCC_AHB1PeriphClockCmd(hmc5883Config->gpioAHB1Peripherals, ENABLE);
}
#endif
gpio.pin = hmc5883Config->gpioPin;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(hmc5883Config->gpioPort, &gpio);
}
delay(50);
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS, HMC5883_BUS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
// The new gain setting is effective from the second measurement and on.
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x60, HMC5883_BUS); // Set the Gain to 2.5Ga (7:5->011)
delay(100);
hmc5883lRead(magADC);
for (i = 0; i < 10; i++) { // Collect 10 samples
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1, HMC5883_BUS);
delay(50);
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z];
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
// Apply the negative bias. (Same gain)
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS, HMC5883_BUS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++) {
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1, HMC5883_BUS);
delay(50);
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged.
xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z];
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
// leave test mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70, HMC5883_BUS); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20, HMC5883_BUS); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 0x00, HMC5883_BUS); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
if (!bret) { // Something went wrong so get a best guess
magGain[X] = 1.0f;
magGain[Y] = 1.0f;
magGain[Z] = 1.0f;
}
hmc5883lConfigureDataReadyInterruptHandling();
}
bool hmc5883lInit(void)
{
int16_t magADC[3];
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
bool bret = true; // Error indicator
delay(50);
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
// The new gain setting is effective from the second measurement and on.
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
delay(100);
hmc5883lRead(magADC);
int validSamples1 = 0;
int failedSamples1 = 0;
int saturatedSamples1 = 0;
while (validSamples1 < 10 && failedSamples1 < INITIALISATION_MAX_READ_FAILURES) { // Collect 10 samples
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(70);
if (hmc5883lRead(magADC)) { // Get the raw values in case the scales have already been changed.
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
++saturatedSamples1;
++failedSamples1;
} else {
++validSamples1;
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z];
}
} else {
++failedSamples1;
}
LED1_TOGGLE;
}
// Apply the negative bias. (Same gain)
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
int validSamples2 = 0;
int failedSamples2 = 0;
int saturatedSamples2 = 0;
while (validSamples2 < 10 && failedSamples2 < INITIALISATION_MAX_READ_FAILURES) { // Collect 10 samples
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(70);
if (hmc5883lRead(magADC)) { // Get the raw values in case the scales have already been changed.
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
++saturatedSamples2;
++failedSamples2;
} else {
++validSamples2;
// Since the measurements are noisy, they should be averaged.
xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z];
}
} else {
++failedSamples2;
}
LED1_TOGGLE;
}
if (failedSamples1 >= INITIALISATION_MAX_READ_FAILURES || failedSamples2 >= INITIALISATION_MAX_READ_FAILURES) {
addBootlogEvent4(BOOT_EVENT_HMC5883L_READ_OK_COUNT, BOOT_EVENT_FLAGS_NONE, validSamples1, validSamples2);
addBootlogEvent4(BOOT_EVENT_HMC5883L_READ_FAILED, BOOT_EVENT_FLAGS_WARNING, failedSamples1, failedSamples2);
bret = false;
}
if (saturatedSamples1 > 0 || saturatedSamples2 > 0) {
addBootlogEvent4(BOOT_EVENT_HMC5883L_SATURATION, BOOT_EVENT_FLAGS_WARNING, saturatedSamples1, saturatedSamples2);
}
if (bret) {
magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
} else {
// Something went wrong so get a best guess
magGain[X] = 1.0f;
magGain[Y] = 1.0f;
magGain[Z] = 1.0f;
}
// leave test mode
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
hmc5883lConfigureDataReadyInterruptHandling();
return bret;
}
