Алгоритм Маджвика. Углы Эйлера намного меньше реальных

[vlanaz 0]

Очень нужна помощь. Вопрос к величине получаемых расчетных углов от Маджвика.
Использую 2 библиотеки установленных в Arduino. Одна для работы с MPU9250, вторая для Madgwick. Плата Arduino MEGA2560.
Визуально, для контроля движения, объект отображается на экране через Processing.
Если использовать собственные функции расчета углов Эйлера из библиотеки MPU9250, углы адекватны реальному повороту платы сенсоров. Конечно дрожит и плывет, но в общем визуально поворот реальной платы совпадает с направлениями и величиной углов объекта в Processing.

Из "сырых" данных MPU9250 получаю три угла Эйлера с помощью алгоритма Маджвика. Посылаю в фильтр Маджвика те же данные и возвращаются углы ориентировочно в 16 раз меньше. То есть 4 полных оборота платы сенсоров вокруг одной оси дает поворот примерно на 90 градусов.

Чувствительность акселерометра +-2g, гироскопа +-250 градусов/сек. По умолчанию и они не меняются. Реальная частота вызова Маджвика каждые 10-12ms.

В вызове фильтра Маджвика оси магнетометра X и Y переставлены, так как в datasheet на MPU9250 указано, что по сравнению с акселерометром и гироскопом у чипа магнитометра оси не совпадают. А ось Z магнетометра по отношению к акселерометру и гироскопу направлена в другую сторону. Поэтому для данных по оси Z стоит знак минус.

Вызов функции из библиотеки MPU9250 и получение углов:

ahrs.update(avgyrx, avgyry, avgyrz, avaccx, avaccy, avaccz, avmagy, avmagx, -avmagz);
yaw   = ahrs.getYaw();
pitch = ahrs.getPitch();
roll  = ahrs.getRoll();

Вызов функции из библиотеки MPU9250 для получения углов Эйлера практически идентичен

yaw = mpu.getYaw();
pitch = mpu.getPitch();
roll = mpu.getRoll();

Кто нибудь может подсказать, что так может влиять на чувствительность фильтра при вычислении углов поворота.  В фильтре регулируются только два параметра. У меня они определены так:

#define sampleFreqDef   100.0f       // sample frequency in Hz
#define betaDef         0.1f                  // 2 * proportional gain

MadgwickAHRS.cpp MadgwickAHRS.h MPU9250.h MPU9250RegisterMap.h QuaternionFilter.h

[vlanaz 0]

6 hours ago, vlanaz said:

Очень нужна помощь. Вопрос к величине получаемых расчетных углов от Маджвика.

 

Все, разобрался. В цикле вызова фильтра вместо микросекунд попали миллисекунды. Поэтому была не чувствительность низкая, а затороможенность реакций.

Хотя никто и не откликнулся, все равно спасибо. В аттаче реакция платы сенсоров, полностью адекватная реальным углам ее вращений.

[Aner 0]

С малыми углами там проблема. Еще набегают углы за н-ное время в покое.