Я потратил 2 дня (и ночи) на эту конкретную проблему и полностью потерпел неудачу в компенсации наклона выходных данных моего магнитометра.
Я перепробовал все, что мог прочитать в Google и в примерах с открытым исходным кодом, ничто не помогло мне найти правильную компенсацию наклона для оба (крен и шаг). .
Настройка: я использую калиброванные значения, но не единичные значения.
У меня есть объединенный вектор силы тяжести, который работает правильно и точно.
Датчик представляет собой 9dof BMX055 (http://www.mouser)..com/ds/2/621/BST-BMX055-DS000-01-274824.pdf) Мин/макс магнитометра составляют +- 512 по каждой оси (с небольшими различиями, но все оси обнулены). Аппаратное обеспечение Sam3X8e (Cortex M3), все написано на C.
Вычисления с плавающей запятой и тригонометрия выполняются довольно быстро, так что это не проблема. Микросхема BMX055 выровнена таким образом, чтобы контакты 20, 19 и 18 были направлены вперед.
На страницах технического описания 159–161 указана ориентация. Высота тона увеличивается, когда я поднимаю переднюю часть.
Крен увеличивается, когда я поднимаю левую сторону.
Примеры значений:
Указание направления, которое мой алгоритм называет 305 градусов при выравнивании по горизонтали.
Pitch: 0 , Roll 0 : MAG cal: x 132 y 93 z -364
Pitch: +24, Roll 0 : MAG cal: x -109 y 93 z -397
Pitch: +46, Roll 0 : MAG cal: x -303 y 89 z -351
Pitch: 0 , Roll -44 : MAG cal: x 151 y 352 z -235
Pitch: 0 , Roll +36 : MAG cal: x 130 y -140 z -328
Pitch: 78 , Roll -2 : MAG cal: x -503 y 93 z -199
Pitch: 7 , Roll -53 : MAG cal: x 135 y 424 z -180
Выравнивание всегда должно было быть около 305 градусов (насколько я мог это сделать), может быть +- 5 градусов.
Формула: (одна и та же, используемая везде)
uint16_t compass_tilt_compensation(float roll_radians, float pitch_radians,float mag_y, float mag_x, float mag_z)
{
float tilt_compensated_heading;
float MAG_X;
float MAG_Y;
float cos_roll;
float sin_roll;
float cos_pitch;
float sin_pitch;
int16_t heading;
//pitch_radians =roll_radians;
//roll_radians *= -1;
//mag_x *= -1;
//roll_radians=0.0f;
//pitch_radians=0;v
//pitch_radians *=-1;
cos_roll = cos(roll_radians);
sin_roll = sin(roll_radians);
cos_pitch = cos(pitch_radians);
sin_pitch = sin(pitch_radians);
#if 0
MAG_X = mag_x*cos_pitch+mag_y*sin_roll*sin_pitch+mag_z*cos_roll*sin_pitch;
MAG_Y = mag_y*cos_roll-mag_z*sin_roll;
tilt_compensated_heading = atan2f(-MAG_Y,MAG_X);
#else
MAG_X = mag_x * cos_pitch + mag_z * sin_pitch;
MAG_Y = mag_x * sin_roll * sin_pitch + mag_y * cos_roll - mag_z * sin_roll * cos_pitch;
tilt_compensated_heading = atan2f(-MAG_Y,MAG_X);
//tilt_compensated_heading = atan2f(-mag_y,mag_x); // works fine when leveled
#endif
//convert to degree from 0-3599
heading = tilt_compensated_heading * RAD_TO_DEG * 10;
if ( heading < 0 )
{
heading += 3600;
}
return heading;
}
Я пробовал различные комбинации, я пытался зафиксировать только одну ось и всегда оставлять одну на 0, меняя местами X/Y шаг/рулон, *-1 на любом из входов.
Результаты всегда совершенно неверны. Иногда (в зависимости от того, где я пытаюсь инвертировать или не инвертировать значение методом проб/ошибок) значения кажутся почти линейными. Иногда одна ось компенсируется в положительных областях.
Однако качка всегда вызывает «случайные» прыжки и изменения курса.
Математика никогда не была моим фаворитом, теперь я жалею об этом.
Мое личное предположение, что формула в принципе правильная, маг в принципе работает (ведь я получаю надлежащие степени при прокачке), но я как-то не так кормлю. (Например, Y и X нужно поменять местами, z*-1, шаг должен быть *-1)
Может ли кто-нибудь, кто хорошо разбирается в этой теме, посмотреть и подсказать, как выбрать правильный заголовок?
Было бы здорово поспать несколько часов этой ночью, и вам больше не придется мечтать о неисправном алгоритме :)
Обновление: компенсация наклона здесь работает для отрицательного крена при наведении на курс 305 градусов. Он также используется здесь: http://www.emcu.it/MEMS/Compass/MEMS_Compass_A_RTM1v3.pdf
