Полезные ссылки
Сайт с материалами по фильтру Мажвика:
http://x-io.co.uk/open-source-imu-and-ahrs-algorithms
Библиотека для Ардуино:
https://github.com/robotclass/RobotClass-MadgwickAHRS
Изображение на титуле страницы — торпедный гироскоп конца 19 века. Источник: http://www.muzej-rijeka.hr
Благодарю Вас !
Добрый день, спасибо Вам за статью.
Но я вот не понял о сырых данных акселерометра. Разве не нужно учитывать настройки акселерометра (2G, 4G…) и делить на соответствующий коэффициент (4096 и т.п.).
И ещё вопрос, у меня почему-то ось Х и Y гироскопа и акселерометра не совпадают, я был уверен что так и есть, но Вы негде их не меняете местами для функции Маджвика…
Спасибо.
Для данных акселерометра нет нужды делать нормировку, она уже есть внутри библиотеки. Насчет несовпадения осей, тут главное не запутаться. К примеру если мы вращаем датчик вокруг оси X, то акселерометр естественно не будет ничего показывать по этой оси. Ведь он измеряет наклон этой оси, а она неподвижна. Это еще не значит, что оси не совпадают.
Спасибо большое, а для каких настроек (±2g, ±4g, ±8g, ±16g) нормировка в библиотеке, и как ее изменить?
в коде есть такая строка:
quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
Откуда взялись q0, q1, q2, q3 ???????
Это переменные из файла MadgwickAHRS.h
А как быть, если используешь два модуля MPU6050?
Я попробовал так:
#include «I2Cdev.h»
#include «MPU6050.h»
#include «MadgwickAHRS.h»
#define TO_RAD 0.01745329252f
MPU6050 accelgyro(0x68); // ADO low
MPU6050 accelgyro2(0x69); // ADO high
unsigned long tm, imu_t, prn_t;
const unsigned int imu_to = 20;
const unsigned int prn_to = 100;
float tdelta;
int16_t ax, ay, az, ax2, ay2, az2;
int16_t gx_raw, gy_raw, gz_raw, gx_raw2, gy_raw2, gz_raw2;
float imu[3], imu2[3];
float quat[4], quat2[4];
float e[3], e2[3];
void setup() {
Serial.begin(57600);
Serial.println(«Initializing I2C devices…»);
accelgyro.initialize(); // инициализация датчика accelgyro(0x68)
accelgyro2.initialize(); // инициализация датчика accelgyro2(0x69)
Serial.println(«Testing device connections…»);
Serial.println(accelgyro.testConnection() ? «MPU6050(0x68) connection successful» : «MPU6050(0x68) connection failed»);
Serial.println(accelgyro2.testConnection() ? «MPU6050(0x69) connection successful» : «MPU6050(0x69) connection failed»);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx_raw, &gy_raw, &gz_raw);
accelgyro2.getMotion6(&ax2, &ay2, &az2, &gx_raw2, &gy_raw2, &gz_raw2);
// преобразуем сырые данные гироскопа в рад/с
float gx = gx_raw * TO_RAD / 131.0;
float gy = gy_raw * TO_RAD / 131.0;
float gz = gz_raw * TO_RAD / 131.0;
float gx2 = gx_raw2 * TO_RAD / 131.0;
float gy2 = gy_raw2 * TO_RAD / 131.0;
float gz2 = gz_raw2 * TO_RAD / 131.0;
// каждые 20мс вычисляем углы наклона
tm = millis();
if (imu_t + imu_to < tm) {
tdelta = tm — imu_t; // вычисляем дельту времени в миллисекундах
imu_t = tm;
// вызываем алгоритм фильтра и передаем туда:
// — дельту времени в секундах
// — данные гироскопа в рад/с
// — сырые данные акселерометра
MadgwickAHRSupdateIMU(tdelta/1000.0, gx, gy, gz, (float)ax, (float)ay, (float)az);
MadgwickAHRSupdateIMU(tdelta/1000.0, gx2, gy2, gz2, (float)ax2, (float)ay2, (float)az2);
quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
quat2Euler(&quat[0], &imu[0]); // преобразуем кватернион в углы Эйлера
quat2Euler(&quat2[0], &imu2[0]); // преобразуем кватернион в углы Эйлера
}
// каждые 100мс добавляем новую точку графика
tm = millis();
if (prn_t + prn_to < tm) {
prn_t = tm;
Serial.print("Yaw "); Serial.print(imu[2]/TO_RAD); Serial.print("\t");
Serial.print("Pitch "); Serial.print(imu[1]/TO_RAD); Serial.print("\t");
Serial.print("Roll "); Serial.println(imu[0]/TO_RAD);
Serial.println();
Serial.print("Yaw_2 "); Serial.print(imu2[2]/TO_RAD); Serial.print("\t");
Serial.print("Pitch_2 "); Serial.print(imu2[1]/TO_RAD); Serial.print("\t");
Serial.print("Roll_2 "); Serial.println(imu2[0]/TO_RAD);
Serial.println();
}
}
Данные со второго MPU6050 равны нулю… Почему?
Добрый день, статья очень помогла! спасибо!
Но почему-то перестали работать кнопки перехода на
ПОДКЛЮЧЕНИЕ / ПРОГРАММА/ ПОЛЕЗНЫЕ ССЫЛКИ
Есть какие-то ограничения?
Это все свежее обновление сайта, чиним. Нет никаких ограничений!
Спасибо, заработало!!!
Спасибо, искал какими библиотеками можно реализовать фильтр Мэджвика, нашел в этой статье. К сожалению нету датчика чтобы проверить код, переписываю под аналогичный. Есть непонятный момент, в коде есть строка преобразования сырых данных гироскопа из град/сек в рад/сек, но данные преобразования уже заложены в библиотечную функцию, получается код преобразовывает данные в рад/сек, а затем когда данные попадают в функцию, преобразование делается еще раз. Разве это будет работать? Кто нибудь проверял на практике, как работает код? В любом случае спасибо за статью!
В дополнение к предыдущему комментарию, вот строки кода из библиотеки:
// Convert gyroscope degrees/sec to radians/sec
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;
Спасибо за статью!
Мажвик еще тот шарлатан, но для понимания принципа неплохо 🙂
пробный запуск кода как есть, время выполнения 10мкс на цикл.
все-таки это не времена Doom2, когда FPU в комп покупался отдельно…
На SoC с float FPU быстрее а главное точнее работает
recipNorm = 1/sqrtf(…
при этом если использовать sqrt на таком процессоре, он будет обсчитывать float как double что заметно снизит скорость.
на процессорах с double FPU, можно сразу все определить как double
вынос пред расчетов, все стало не читаемо, но главное слабая попытка спорить с оптимизацией компилятора -O3, где он это так сам пересчитает, что мать родная не узнает 🙂
в сухом остатке код стал выполняться за 3,1мкс, еще и с повышенной точностью.
но вот вопрос как его стабилизировать? в разделении данных акселерометра и гироскопа ведь его суть и якобы ценность.
betaDef большое — ловит ускорения при движении и получается ошибка
betaDef маленькое — ошибки от ускорения меньше, но начинает расти накопленная ошибка
можно конечно следить за размером вектора ускорения и на лету подстраивать betaDef, но это уже что-то совсем не то, нарушающее концепцию этого фильтра.