LSM303AGR转换加速度计和磁力计的输出到俯仰，滚转和偏航

您好，转换加速度计和磁力计的输出与俯仰，滚转和偏航角度有关。我正在尝试使用DT0058的方程式。我写了下面的函数。
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）;
}
当罗盘围绕z轴旋转时，它会改变偏航值但非常不规则，例如从130到40然后形成-100到-30等。当我仅改变俯仰或滚转值时，偏航也会改变。我认为这不正确，因为设备的方向没有改变。例如，如果俯仰是1，则偏航是-50，如果俯仰是14偏航= -120。我不明白这些结果。我认为俯仰和滚动是正确的。
我在正常模式下使用加速度计，满量程默认值（+ - 2g），odr = 400 Hz，连续模式下的magetometer，100Hz。我不使用中断，只检查可用的数据。
谢谢你的所有答案。



以下为原文




Hello, I have a problem with conversion accelerometer's and magnetometer's output to pitch, roll and yaw angles. I am trying to use equaons 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.