// Rearranged & Extended by Crashpilot
static void Mag_getRawADC_With_Gain(void) // Read aligned
{
int16_t magADC[3];
uint8_t i;
hmc5883lRead(magADC); // Does that now: alignSensors(ALIGN_MAG, magADC);
for (i = 0; i < 3; i++) magADCfloat[i] = (float)magADC[i] * magCal[i];
}
static void Mag_getRawADC(void)
{
static int16_t rawADC[3];
hmc5883lRead(rawADC);
// no way? is THIS finally the proper orientation?? (by GrootWitBaas)
magADC[ROLL] = rawADC[2]; // X
magADC[PITCH] = -rawADC[0]; // Y
magADC[YAW] = -rawADC[1]; // Z
}
int compassGetADC(flightDynamicsTrims_t *magZero)
{
static uint32_t t, tCal = 0;
static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
uint32_t axis;
if ((int32_t)(currentTime - t) < 0)
return 0; //each read is spaced by 100ms
t = currentTime + 100000;
// Read mag sensor
hmc5883lRead(magADC);
alignSensors(magADC, magADC, magAlign);
if (f.CALIBRATE_MAG) {
tCal = t;
for (axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = magADC[axis];
magZeroTempMax.raw[axis] = magADC[axis];
}
f.CALIBRATE_MAG = 0;
}
if (magInit) { // we apply offset only once mag calibration is done
magADC[X] -= magZero->raw[X];
magADC[Y] -= magZero->raw[Y];
magADC[Z] -= magZero->raw[Z];
}
if (tCal != 0) {
if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
LED0_TOGGLE;
for (axis = 0; axis < 3; axis++) {
if (magADC[axis] < magZeroTempMin.raw[axis])
magZeroTempMin.raw[axis] = magADC[axis];
if (magADC[axis] > magZeroTempMax.raw[axis])
magZeroTempMax.raw[axis] = magADC[axis];
}
} else {
tCal = 0;
for (axis = 0; axis < 3; axis++) {
magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
}
saveAndReloadCurrentProfileToCurrentProfileSlot();
}
}
return 1;
}
int Mag_getADC(void)
{
static uint32_t t, tCal = 0;
static int16_t magZeroTempMin[3];
static int16_t magZeroTempMax[3];
uint32_t axis;
if ((int32_t)(currentTime - t) < 0)
return 0; //each read is spaced by 100ms
t = currentTime + 100000;
// Read mag sensor
hmc5883lRead(magADC);
if (f.CALIBRATE_MAG) {
tCal = t;
for (axis = 0; axis < 3; axis++) {
mcfg.magZero[axis] = 0;
magZeroTempMin[axis] = magADC[axis];
magZeroTempMax[axis] = magADC[axis];
}
f.CALIBRATE_MAG = 0;
}
if (magInit) { // we apply offset only once mag calibration is done
magADC[X] -= mcfg.magZero[X];
magADC[Y] -= mcfg.magZero[Y];
magADC[Z] -= mcfg.magZero[Z];
}
if (tCal != 0) {
if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
LED0_TOGGLE;
for (axis = 0; axis < 3; axis++) {
if (magADC[axis] < magZeroTempMin[axis])
magZeroTempMin[axis] = magADC[axis];
if (magADC[axis] > magZeroTempMax[axis])
magZeroTempMax[axis] = magADC[axis];
}
} else {
tCal = 0;
for (axis = 0; axis < 3; axis++)
mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
writeEEPROM(1, true);
}
}
return 1;
}
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();
}
void hmc5883lInit(float *calGain) // THE RESULT IS ALIGNED
{
gpio_config_t gpio;
int16_t magADC[3];
uint8_t i, k;
float xyz_total[3] = { 0, 0, 0 }, multiplier;
float SELF_TEST_GAUSS[3] = {1.16f, 1.16f, 1.08f};// HMC58X3_*_SELF_TEST_GAUSS for X Y Z
bool GainOK = true; // Error indicator
if (hse_value == 8000000) {
// PB12 - MAG_DRDY output on rev4 hardware
gpio.pin = Pin_12;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOB, &gpio);
} else if (hse_value == 12000000) {
// PC14 - MAG_DRDY output on rev5 hardware
gpio.pin = Pin_14;
gpioInit(GPIOC, &gpio);
}
delay(100);
i2cWrite(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.
if (cfg.mag_gain)
{ // not zero
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x60);// Set the Gain 2,5 GAUSS
multiplier = 13200.0f; // 660 * 2 * 10
}
else
{ // zero
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 2 << 5); // Set the Gain 1,9 GAUSS
multiplier = 16400.0f; // 820 * 2 * 10
}
delay(100);
hmc5883lRead(magADC); // read one dataset and discard it!
for (i = 0; i < 10; i++) // Collect 10 samples
{
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(100);
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
for (k = 0; k < 3; k++)
{
if(abs(magADC[k]) >= 2047) GainOK = false;// Detect saturation. Official range (-2048, 2047)
xyz_total[k] += (float)magADC[k]; // Since the measurements are noisy, they should be averaged rather than taking the max.
}
LED1_TOGGLE;
}
// Apply the negative bias. (Same gain)
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++)
{
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(100);
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
for (k = 0; k < 3; k++)
{
if(abs(magADC[k]) >= 2047) GainOK = false;// Detect saturation. Official range (-2048, 2047)
xyz_total[k] -= (float)magADC[k]; // Since the measurements are noisy, they should be averaged rather than taking the max.
}
LED1_TOGGLE;
}
// leave test mode
// i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70);// Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x78); // Configuration Register A -- 01111000 num samples: 8 ; output rate: 75Hz ; normal measurement mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
#ifdef debugmode
maggainok = GainOK;
#endif
for (i = 0; i < 3; i++)
{
if(GainOK) calGain[i] = fabs(multiplier * SELF_TEST_GAUSS[i] / xyz_total[i]);
else calGain[i] = 1.0f;
}
}
void hmc5883lInit(void)
{
gpio_config_t gpio;
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
if (hse_value == 8000000) {
// PB12 - MAG_DRDY output on rev4 hardware
gpio.pin = Pin_12;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOB, &gpio);
} else if (hse_value == 12000000) {
// PC14 - MAG_DRDY output on rev5 hardware
gpio.pin = Pin_14;
gpioInit(GPIOC, &gpio);
}
delay(50);
i2cWrite(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_ADDRESS, HMC58X3_R_CONFB, 0x60); // 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);
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); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++) {
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
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); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // 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;
}
}
void hmc5883lInit(float *calibrationGain)
{
GPIO_InitTypeDef GPIO_InitStructure;
float magGain[3];
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
// PB12 - MAG_DRDY output on rev4 hardware
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
delay(50);
i2cWrite(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_ADDRESS, HMC58X3_R_CONFB, 2 << 5); // Set the Gain
delay(100);
hmc5883lRead(magADC);
for (i = 0; i < 10; i++) { // Collect 10 samples
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
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[0] += magADC[0];
xyz_total[1] += magADC[1];
xyz_total[2] += magADC[2];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
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); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++) {
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
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[0] -= magADC[0];
xyz_total[1] -= magADC[1];
xyz_total[2] -= magADC[2];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
magGain[0] = fabs(820.0 * HMC58X3_X_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[0]);
magGain[1] = fabs(820.0 * HMC58X3_Y_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[1]);
magGain[2] = fabs(820.0 * HMC58X3_Z_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[2]);
// leave test mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
if (!bret) { // Something went wrong so get a best guess
magGain[0] = 1.0;
magGain[1] = 1.0;
magGain[2] = 1.0;
}
// if parameter was passed, give calibration values back
if (calibrationGain) {
calibrationGain[0] = magGain[0];
calibrationGain[1] = magGain[1];
calibrationGain[2] = magGain[2];
}
}
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
#ifdef USE_MAG_DATA_READY_SIGNAL
if (hmc5883Config && hmc5883Config->io) {
intIO = IOGetByTag(hmc5883Config->io);
IOInit(intIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
IOConfigGPIO(intIO, IOCFG_IN_FLOATING);
}
#endif
delay(50);
i2cWrite(HMC5883L_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(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
delay(100);
hmc5883lRead(magADC);
for (i = 0; i < 10; i++) { // Collect 10 samples
i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
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(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++) {
i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
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(HMC5883L_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(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // 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;
}
#ifdef USE_MAG_DATA_READY_SIGNAL
do {
if (!(hmc5883Config && intIO))
break;
# ifdef ENSURE_MAG_DATA_READY_IS_HIGH
if (!IORead(intIO))
break;
# endif
EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler);
EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(intIO, true);
} while (0);
#endif
}
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;
}
