LSM303AGR - conversion accelerometer's and magnetometer's output to pitch, roll and yaw.

Question asked by Wojciech Jasiewicz on Mar 1, 2018
Latest reply on Mar 29, 2018 by Miroslav B

Hello, I have a problem with conversion accelerometer's and magnetometer's output to pitch, roll and yaw angles. I am trying to use equations from DT0058. I wrote function like below.

void newPitchRollYaw(float* const acc, float* const mag)
{
float roll = atan2(acc[LSM303AGR_Y], acc[LSM303AGR_Z] + acc[LSM303AGR_X] * 0.01);
float pitch = atan(-acc[LSM303AGR_X] / (acc[LSM303AGR_Y] * sin(roll) + acc[LSM303AGR_Z] * cos(roll)));

float By2 = mag[LSM303AGR_Z] * sin(roll) - mag[LSM303AGR_Y] * cos(roll);
float Bz2 = mag[LSM303AGR_Y] * sin(roll) + mag[LSM303AGR_Z] * cos(roll);
float Bx3 = mag[LSM303AGR_X] * cos(pitch) + Bz2 * sin(pitch);

float yaw = atan2( By2 , Bx3);
float factor = LSM303AGR_180_DEGREES / LSM303AGR_PI;

log_PushLine(e_logLevel_Info, "pitch: %f , roll: %f , yaw: %f", pitch * factor , roll * factor , yaw * factor);

float Qw = cos(roll/2)*cos(pitch/2)*cos(roll/2) + sin(roll/2)*sin(pitch/2)*sin(roll/2);
float Qx = sin(roll/2)*cos(pitch/2)*cos(roll/2) - cos(roll/2)*sin(pitch/2)*sin(roll/2);
float Qy = cos(roll/2)*sin(pitch/2)*cos(roll/2) + sin(roll/2)*cos(pitch/2)*sin(roll/2);
float Qz = cos(roll/2)*cos(pitch/2)*sin(roll/2) - sin(roll/2)*sin(pitch/2)*cos(roll/2);

log_PushLine(e_logLevel_Info, "qw: %f , qx: %f , qy: %f , qz: %f", Qw * factor, Qx * factor, Qy * factor, Qz * factor);

}

When compass rotates around z-axis it changes yaw value but very irregularly, for example from 130 to 40 and then form -100 to - 30 etc. When I change only pitch or roll value, yaw is also changed. I think that it's not correct, because  device's direction is no changed. For example if pitch is 1, yaw is -50, if pitch is 14 yaw = -120. I don't understand these results. I think that pitch and roll are correct.

I use accelerometer in normal mode, full scale default ( +- 2g), odr = 400 Hz, magetometer in continuous mode, 100Hz. I don't use interrupts, only check data available.