lsm303agr magnetometer heading calculation

[nathanb]

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

}

[Federica Bossi]

Hi @nathanb ,

Welcome to ST Community!

I can't correct your code but I suggest you to have a look at our design tips for this product, you also find an example in Matlab for the implementation.

Hope this helps.