Имеется тройка http://wiki.amperka.ru/продукты:troyka-imu-10-dof Использую код из примера Код (C++): // библиотека для работы I²C #include <Wire.h> // библиотека для работы с модулями IMU #include <TroykaIMU.h> // множитель фильтра #define BETA 0.22 // создаём объект для фильтра Madgwick Madgwick filter; // создаём объект для работы с акселерометром Accelerometer accel; // создаём объект для работы с гироскопом Gyroscope gyro; // создаём объект для работы с компасом Compass compass; // переменные для данных с гироскопа, акселерометра и компаса float gx, gy, gz, ax, ay, az, mx, my, mz; // получаемые углы ориентации (Эйлера) float yaw, pitch, roll; // переменная для хранения частоты выборок фильтра float fps = 100; // калибровочные значения компаса // полученные в калибровочной матрице из примера «compassCalibrateMatrixx» const double compassCalibrationBias[3] = { 524.21, 3352.214, -1402.236 }; const double compassCalibrationMatrix[3][3] = { {1.757, 0.04, -0.028}, {0.008, 1.767, -0.016}, {-0.018, 0.077, 1.782} }; void setup() { // открываем последовательный порт Serial.begin(115200); Serial.println("Begin init..."); // инициализация акселерометра accel.begin(); // инициализация гироскопа gyro.begin(); // инициализация компаса compass.begin(); // калибровка компаса compass.calibrateMatrix(compassCalibrationMatrix, compassCalibrationBias); // выводим сообщение об удачной инициализации Serial.println("Initialization completed"); } void loop() { // запоминаем текущее время unsigned long startMillis = millis(); // считываем данные с акселерометра в единицах G accel.readGXYZ(&ax, &ay, &az); // считываем данные с гироскопа в радианах в секунду gyro.readRadPerSecXYZ(&gx, &gy, &gz); // считываем данные с компаса в Гауссах compass.readCalibrateGaussXYZ(&mx, &my, &mz); // устанавливаем коэффициенты фильтра filter.setKoeff(fps, BETA); // обновляем входные данные в фильтр filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); // получение углов yaw, pitch и roll из фильтра yaw = filter.getYawDeg(); pitch = filter.getPitchDeg(); roll = filter.getRollDeg(); // выводим полученные углы в serial-порт Serial.print("yaw: "); Serial.print(yaw); Serial.print("\t\t"); Serial.print("pitch: "); Serial.print(pitch); Serial.print("\t\t"); Serial.print("roll: "); Serial.println(roll); // вычисляем затраченное время на обработку данных unsigned long deltaMillis = millis() - startMillis; // вычисляем частоту обработки фильтра fps = 1000 / deltaMillis; } Датчик откалиброван. Пример работает, но как-то странно: Датчик повернут на угол 128 градусов Код (C++): Begin init... Initialization completed -0.10 -0.19 -0.28 -0.36 -0.45 -0.55 -0.65 -0.74 -0.83 -0.92 ... -128.0 т.е. идет изменение данных с мелким шагом до получения реального направления. Время этого процесса всегда разное, т.к. всегда начинается с 0, а датчик может находиться в момент включения с большим отклонением от севера. Это правильное поведение или что-то все же не так?
Ну вы же пользуетесь фильтрованными данными, а любой фильтр вносит задержку. Хотите хорошее сглаживание - ждите дольше. Как то так.
Я видимо не четко описал ситуацию. Датчик зафиксирован в положении 359гр, а при подключении, я в консоли в течении нескольких секунд наблюдаю рост показаний от 0 до 359, а не колебания вокруг значения 359. Хотелось бы понять, это с датчика такие данные приходят с нарастанием или кривость обработки показаний.
Повторю еще раз. Датчик не знает при старте куда он повернут, поэтому у него это 0. Затем постепенно (задержка фильтра) датчик приходит к пониманию своего положения. Так и должно быть.
дело в том что фильтр меджвика показывает данные с гироскопа компенсированные данными с магнитометра. По сути каждая новая итерация содержит прибавку от магнитометра с коэффициентом пропорциональным BETA. Установите этот коэффициент побольше и значения будут быстрее сходится к реальной величине. В 0 момент времени азимут равен конечно 0, т.к. программа при включении не знает куда повернут датчик. Но потом магнитометр начинает "поворачивать" показания в нужную сторону. Так работает фильтр
