Reading Accelerometer Data to Make a Tilt-Compensated Compass

[CGrov.1]

I'm making a tilt-compensate compass using an STM32L476RG, an MMC5983, and an LIS2HH12. I am able to get a compass heading and tilt and roll angles but the tilt compensation seems to make the compass heading error worse. I have calibrated both the magnetometer and accelerometer. Bellow are my calculations:

void calc_tilt(){
 
 
	float pitch = asinf(-accel.x);
	float roll = asinf((accel.y)/ (cosf(pitch)));
 
	accel.pitch = pitch;//rotation arond y-axis
	accel.roll = roll;//rotation around x-axis
 
 
	if ((pitch == 90) || (pitch ==-90)){
		roll = 0;
	}
 
}
 
 
void calc_comp_field(){
 
	 mag.x_comp = mag.x*cosf(accel.pitch) + mag.z*sinf(accel.pitch);
	 mag.y_comp = mag.x*sinf(accel.pitch)*sinf(accel.roll) + mag.y*cosf(accel.roll) - mag.z*sinf(accel.roll)*cosf(accel.pitch);
	 mag.z_comp = -mag.x*cosf(accel.roll)*sinf(accel.pitch) + mag.y*sinf(accel.roll) + mag.z*cosf(accel.roll)*cosf(accel.pitch);
 
}
 
 
void getHead(){
 
float heading = 0;
 
	if((mag.x_comp ==0)&&(mag.y_comp<0)){
		heading = 90;
	} else if ((mag.x_comp==0)&&(mag.y_comp>0)){
		heading = 0;
	}
 
	heading = (atan2f(-mag.y_comp,mag.x_comp))*(180/M_PI);
 
	
 
	if(heading < 0){
		heading = heading + 360;
	} else if (heading >360){
		heading = heading - 360;
	}
 
 
 
	mag.heading = heading;
 
}

I also found that when I combine the accelerometer lsb and msb I need to do it opposite* (see code segment bellow for clarification on this) or it will yield a cubic gain matrix rather than a spherical one. The magnetometer functions as expected.

*code: reg_vals[I] is a register into which my data is read. it is read in the order xl, xh, yl, yh, zl, zh, so reg_vals[0] is xlsb and so on. I would expect to do (lsb<<8)|msb but this doesn't work and I am unable to get an accurate tilt measurment but I found that msb<<8|lsb works fine. Any idea where I'm giong wrong?

float Ax_Raw = ((int16_t)(reg_vals[1]<<8) | reg_vals[0]);
float Ay_Raw = ((int16_t)(reg_vals[3]<<8) | reg_vals[2]);
float Az_Raw = ((int16_t)(reg_vals[5]<<8) | reg_vals[4]);

[turboscrew]

The endianness only applies to memory, not processor registers. Also, it would be a good habit to cast the result to float too, before assignment.

float Ax_Raw = (float)((int16_t)(reg_vals[1]<<8) | reg_vals[0]);

I'd also output the readouts to verify the chip orientations.

[CGrov.1]

I verified the chip orientation and double checked that it matched the other chip. Still no joy. When I tilt it, the un-compensated value changes in the same direction (either increasing or decreasing the heading) but to a lesser degree so it is slightly more accurate than the "compensated" value. This has me thinking that maybe I am compensating in the "wrong" direction, making the heading error worse. I've tried playing around with the sign of the compensated values but still nothing.

[raptorhal2]

In axis transformations, sequence is important. I believe you need to roll before you pitch.

Cheers, Hal

[CGrov.1]

can you clarify what you mean? In my calc_tilt() function I need to calculate pitch first, as that value is used to calculate roll.