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

Тема в разделе "Схемотехника, компоненты, модули", создана пользователем sslobodyan, 20 май 2018.

  1. sslobodyan

    sslobodyan Гик

    Всем доброго здравия!
    Пытаюсь делать наклонокомпенсированный компас на 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;
    }
     
    И никак не выходит компенсация. Похоже, что у меня перевернута какая-то ось.
    Посему вопрос. Как правильно должны быть определены оси магнитометра и акселерометра? Какие значения они должны выдавать при наклонах по питчу и роллу? Может надо нормализовать показания, то подскажите к каким значениям? Задача то как-бы стандартная и давно решенная, может кто поделится своим решением?
     
  2. Daniil

    Daniil Гик

    Не претендую на то, что я прав, но случайно у вас нет путаницы с осями?
    Screenshot_2018-05-20-18-20-08-844_com.google.android.apps.docs.png
    Оси OZ у гироскопа и магнитного датчик не сонаправлены.
     
  3. sslobodyan

    sslobodyan Гик

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