lsm303agr magnetometer heading calculation
Hi,
I am working with the lsm303agr e-compass. I am stuck with the hard iron callibration. I I wrote two functions fuctions for calculating the heading:
I was trying to follow the https://gist.github.com/srlm-io/fafee8feed8bd5661266#file-hardironcalibration-ino but i am still stuck. (I am working in C)
void HardIronCompensation(void) {
HAL_StatusTypeDef ret;
//min en max calculating
int16_t Magn_Xvalue = Magn_X();
if (Magn_Xvalue < Xmin) {
Xmin = Magn_Xvalue;
}
if (Magn_Xvalue > Xmax) {
Xmax = Magn_Xvalue;
}
int16_t Magn_Yvalue = Magn_Y();
if (Magn_Yvalue < Ymin) {
Ymin = Magn_Yvalue;
}
if (Magn_Yvalue > Ymax) {
Ymax = Magn_Yvalue;
}
int16_t Magn_Zvalue = Magn_Z();
if (Magn_Zvalue < Zmin) {
Zmin = Magn_Zvalue;
}
if (Magn_Zvalue > Zmax) {
Zmax = Magn_Zvalue;
}
//hard iron compansation calculations
Magn_Hard_Xvalue = (Xmax - Xmin) / 2;
Magn_Hard_Yvalue = (Ymax - Xmin) / 2;
Magn_Hard_Zvalue = (Zmax - Zmin) / 2;
//16 bit bit split MSB and LSB
uint8_t Magn_Hard_Xvalue_L = Magn_Hard_Xvalue & 0xFF;
uint8_t Magn_Hard_Xvalue_H = (Magn_Hard_Xvalue >> 8) & 0XFF;
uint8_t Magn_Hard_Yvalue_L = Magn_Hard_Yvalue & 0xFF;
uint8_t Magn_Hard_Yvalue_H = (Magn_Hard_Yvalue >> 8) & 0XFF;
uint8_t Magn_Hard_Zvalue_L = Magn_Hard_Zvalue & 0xFF;
uint8_t Magn_Hard_Zvalue_H = (Magn_Hard_Zvalue >> 8) & 0XFF;
//putting data in hardiron in registers
//OFFSET_X_REG_L_M
ret = HAL_I2C_Mem_Write(&hi2c2, LSM303AGR_MAG_ADDR_W,
LSM303AGR_MAG_OFFSET_X_REG_L,
I2C_MEMADD_SIZE_8BIT, &Magn_Hard_Xvalue_L, 1, HAL_MAX_DELAY);
if (ret != HAL_OK) {
} else {
//printf("Magn_Xvalue_L set\n");
}
//OFFSET_X_REG_H_M
ret = HAL_I2C_Mem_Write(&hi2c2, LSM303AGR_MAG_ADDR_W,
LSM303AGR_MAG_OFFSET_X_REG_H,
I2C_MEMADD_SIZE_8BIT, &Magn_Hard_Xvalue_H, 1, HAL_MAX_DELAY);
if (ret != HAL_OK) {
} else {
//printf("Magn_Xvalue_H set\n");
}
//OFFSET_Y_REG_L_M
ret = HAL_I2C_Mem_Write(&hi2c2, LSM303AGR_MAG_ADDR_W,
LSM303AGR_MAG_OFFSET_Y_REG_L,
I2C_MEMADD_SIZE_8BIT, &Magn_Hard_Yvalue_L, 1, HAL_MAX_DELAY);
if (ret != HAL_OK) {
} else {
//printf("Magn_Yvalue_L set\n");
}
//OFFSET_Y_REG_H_M
ret = HAL_I2C_Mem_Write(&hi2c2, LSM303AGR_MAG_ADDR_W,
LSM303AGR_MAG_OFFSET_Y_REG_H,
I2C_MEMADD_SIZE_8BIT, &Magn_Hard_Yvalue_H, 1, HAL_MAX_DELAY);
if (ret != HAL_OK) {
} else {
//printf("Magn_Yvalue_H set\n");
}
//OFFSET_Z_REG_L_M
ret = HAL_I2C_Mem_Write(&hi2c2, LSM303AGR_MAG_ADDR_W,
LSM303AGR_MAG_OFFSET_Z_REG_L,
I2C_MEMADD_SIZE_8BIT, &Magn_Hard_Zvalue_L, 1, HAL_MAX_DELAY);
if (ret != HAL_OK) {
} else {
//printf("Magn_Zvalue_L set\n");
}
//OFFSET_Z_REG_H_M
ret = HAL_I2C_Mem_Write(&hi2c2, LSM303AGR_MAG_ADDR_W,
LSM303AGR_MAG_OFFSET_Z_REG_H,
I2C_MEMADD_SIZE_8BIT, &Magn_Hard_Zvalue_H, 1, HAL_MAX_DELAY);
if (ret != HAL_OK) {
} else {
//printf("Magn_Zvalue_H set\n");
}
}
uint8_t LSM303AGR_HEADING(void) {
HAL_StatusTypeDef ret;
uint8_t data[12];
uint8_t databuffer[12];
int heading2;
//double heading;
//printf("************MAG OUTPUT UITLEZEN************\n");
//STATUS_REG_M OUT
HAL_Delay(90); //90 ms wachten voor stable output
ret = HAL_I2C_Mem_Read(&hi2c2, LSM303AGR_MAG_ADDR_R,
LSM303AGR_MAG_STATUS_REG,
I2C_MEMADD_SIZE_8BIT, data, 1, HAL_MAX_DELAY);
if (ret != HAL_OK) {
} else {
//data = buf[0];
sprintf(databuffer, "STATUS_REG_M OUT:%d\n", data[0]);
//printf(databuffer);
}
// Signs choosen so that, when axis is down, the value is + 1g
float Accl_Xvalue = -Accl_X();
float Accl_Yvalue = Accl_Y();
float Accl_Zvalue = Accl_Z();
float Magn_Xvalue = Magn_X() - Magn_Hard_Xvalue;
float Magn_Yvalue = -Magn_Y() - Magn_Hard_Yvalue;
float Magn_Zvalue = -Magn_Z() - Magn_Hard_Zvalue;
// Freescale solution
float roll = atan2(Accl_Yvalue, Magn_Zvalue);
float pitch = atan(
-Accl_Xvalue / (Accl_Yvalue * sin(roll) + Accl_Zvalue * cos(roll)));
// Signs should be choosen so that, when the axis is down, the value is + positive.
float magn_fy_fs = Magn_Zvalue * sin(roll) - Magn_Yvalue * cos(roll);
float magn_fx_fs = Magn_Xvalue * cos(pitch)
+ Magn_Yvalue * sin(pitch) * sin(roll)
+ Magn_Zvalue * sin(pitch) * cos(roll);
//heading berekenen
float heading = (atan2(magn_fy_fs, magn_fx_fs) * 180) / 3.14;
//heading is eerst van -180 to +180 graden -> gecorigeerd naar 0 tot +360 graden
if (heading < 0) {
heading = heading + 360;
}
heading2 = (int) heading;
sprintf(databuffer, "HEADING:%d\n", heading2);
ssd1306_SetCursor(0,23);
ssd1306_WriteString(databuffer,Font_11x18,0x01);
ssd1306_UpdateScreen();
printf(databuffer);
return data[0];
}