День добрый. Запустил LSM303D. Данные с магнитного датчика, акселерометра и датчика температуры идут на ура. Направление на севео посчитал по формуле:

180*atan2f(magnetic_Y, magnetic_X)/3.1415 с коррекцией отрицательного угла.

Работает нормально, если выставлен по уровню(горизонтирован).
Углы крена и тангажа:

pitch = asinf(-acceleration_X);
roll = asinf(acceleration_Y / cosf(pitch));

Тоже считаются верно.

Потом попытался компенсировать углы крена и тангажа, когда компас не горизонтирован. Считал по формуле:

xh = magnetic_X * cosf(pitch) + magnetic_Z * sinf(pitch);
yh = magnetic_X * sinf(roll) * sinf(pitch) + magnetic_Y * cosf(roll) - magnetic_Z * sinf(roll) * cosf(pitch);

angle_compensated= 180*atan2f(yh, xh)/3.1415;

Считает неправильно. Прошу знающих комрадов подсказать - где ошибка в формуле?