cancel
Showing results for 
Search instead for 
Did you mean: 

lsm303agr magnetometer heading calculation

nathanb
Associate

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

}

1 REPLY 1
Federica Bossi
ST Employee

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.

In order to give better visibility on the answered topics, please click on 'Accept as Solution' on the reply which solved your issue or answered your question.