Угловая компенсация компаса, LSM303D Опции

[rat]

День добрый. Запустил 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;

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

[Massi]

http://media.digikey.com/pdf/Data%20Sheets...-RD%20Guide.pdf

[rat]

http://media.digikey.com/pdf/Data%20Sheets...-RD%20Guide.pdf

По ссылке методика для конкретного компаса и с использованием заточенного под него ПО, мой компас уже не подходит. Интересует теория и практические советы по калибровке и какие точности были получены. Неужели никто из форумчан не делал электронный компас?