Jump to content

    
Guest vlanaz

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

Recommended Posts

Guest vlanaz

Очень нужна помощь. Вопрос к величине получаемых расчетных углов от Маджвика.
Использую 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

Share this post


Link to post
Share on other sites
Guest vlanaz
6 hours ago, vlanaz said:

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

 

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

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

Share this post


Link to post
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.