Наклонокомпенсированный компас на MPU9250

Всем доброго здравия!
Пытаюсь делать наклонокомпенсированный компас на MPU9250. Начитался теории. Но как-то не цветет каменный цветок Поэтому прошу помощь зала.
Что делал.
Установил платку в нужной мне позиции (получилась вверх тормашками и повернута на 90 градусов). Определился с системой координат: ось, которая смотрит вперед будет Х, вправо У, вниз Z. Соответственно определил откуда считывать показания акселя и магнитометра по каждой оси.
Далее провел калибровку магнитометра путем нахождения минимумов-максимумов по каждой оси и высчитывания по ним смещения (мин+макс)/2 а также множителя для выравнивания размеров каждой оси.
Если теперь в горизонтали вычислять направление на север как atan2(my,mx) то все отлично.
Пытаюсь применить вот такую математику

Код (C++):

float tiltCompensate(Vector mag, Vector normAccel)
{
float pitch;
float roll;
pitch = asin(-normAccel.XAxis);
roll = asin(normAccel.YAxis);
if (roll > 0.78 || roll < -0.78 || pitch > 0.78 || pitch < -0.78)
{
return -1;
}

float cosRoll = cos(roll);
float sinRoll = sin(roll);
float cosPitch = cos(pitch);
float sinPitch = sin(pitch);

float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;
float heading = atan2(Yh, Xh);
return heading;
}
 

И никак не выходит компенсация. Похоже, что у меня перевернута какая-то ось.
Посему вопрос. Как правильно должны быть определены оси магнитометра и акселерометра? Какие значения они должны выдавать при наклонах по питчу и роллу? Может надо нормализовать показания, то подскажите к каким значениям? Задача то как-бы стандартная и давно решенная, может кто поделится своим решением?

 

Не претендую на то, что я прав, но случайно у вас нет путаницы с осями?

Оси OZ у гироскопа и магнитного датчик не сонаправлены.

 

Не, это я знаю. Оси я "нащупывал". Метода такая.
Магнитометр. Ориентируем передом на север. Тогда максимум у Х. Поворачиваем по часовой на 90 - минимум у У, по Х почти 0. Ось Z при повороте в горизонте не меняется.
Акселерометр. Опускаем ось Х по магнитометру вертикально вниз - она становится 1. Наклоняем вправо - ось У становится 1. В горизонте ось Z равна 1.