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.

Thanks for all answers.

HI,

I can tell you some suggestion based on my experience with LSM303D and LSM303DLHC.

1) Have you make the magnetometer calibration before start with measures?

2) seem you haven't make all the test for border limit explained in application note for trigonometric functions.

You wrote: "When compass rotates around z-axis it changes yaw value but very irregularly"

Answer: this is not correct. I confirm that this must change only yaw.

I personally move the processing on Scilab or Matlab from raw data to examine if data are good for yaw, pitch roll calculations.

I suggest to search for discovery-stm32f3 where there is a source code to calculate Heading=Yaw from LSM303DLHC that is the same procedure.

Regards

Maurizio