MPU-6500 – это 6-осевой инерциальный измерительный модуль (Inertial Measurement Unit – IMU), который объединяет в одном корпусе микросхемы трёхосевой акселерометр и трёхосевой гироскоп. Этот чип, разработанный компанией InvenSense (часть TDK Corporation), является прямым развитием популярной модели MPU-6050. Обе модели почти программно совместимы, но 6500-я модель имеет улучшенные характеристики и повышенную энергоэффективность, а также более высокую скорость передачи данных (до 20 МГц на чтение по SPI-интерфейсу).
В рамках данной статьи мы рассмотрим ключевые особенности 6500-й модели, а для закрепления материала в качестве практики соберём макет измерителя угла поворота (ИУП) с индикацией на круглом дисплее GC9A01.
Содержание
- Общее описание
- Параметры и характеристики
- Режимы работы и настройка
- MPU-6500 в составе платы GY-9250/6500
- Схема подключения MPU-6500 к Arduino UNO
- Программный код (скетч)
- Эксперимент
- Заключение
Общее описание
MPU-6500 – это микросхема датчика линейного ускорения и угловой скорости вращения, построенная по технологии MEMS (Micro-Electro-Mechanical Systems, микроэлектромеханическая система).
Принцип работы акселерометра подробно описывается в статье про шагомер на базе ADXL345, а гироскопа – про инклинометр на базе MPU-6050.
Как уже было сказано выше, 6500 во многом схож с предыдущей моделью 6050. Для начала рассмотрим в общих чертах устройство данного инерциального модуля, используя для наглядности представленную ниже блок-схему микросхемы из официальной документации:
Микросхема включает в себя следующие основные блоки:
– основные трёхосевые чувствительные элементы (Accel, Gyro). Оба имеют 16-битное разрешение. Акселерометр регистрирует в широком диапазоне ускорений до ±16g с частотой опроса до 4 кГц, а гироскоп способен измерять угловую скорость до ±2000 °/с при частоте дискретизации до 8 кГц;
– персональные аппаратные блоки для самотестирования чувствительных элементов (Self test);
– датчик температуры (Temp Sensor) для термокомпенсации основных сенсоров. Чувствительность 333,87 LSB/°C. Аналого-цифровой преобразователь 16-битный, и за точку калибровки (0 LSB) принята температура +21 °C. Важно понимать, что измерять температуру воздуха посредством микросхемы нецелесообразно, поскольку абсолютная погрешность может быть значительной, и производитель не указывает конкретную точность;
– встроенный повышающий преобразователь напряжения (Charge Pump);
– набор 16-разрядных аналого-цифровых преобразователей, отдельно для каждого чувствительного элемента (ADC);
– блоки цифровой обработки сигналов (Signal Conditioning), выполняющие фильтрацию, компенсацию смещения, настройку чувствительности и децимацию (прореживание выборки под нужную частоту опроса);
– регистр флагов прерываний (Interrupt Status Register), который определяет причины и регулирует прерывания;
– набор пользовательских регистров (User & Config Registers) для настройки режимов работы;
– регистр хранения результатов оцифровки данных (Sensor Registers) перед дальнейшей обработкой;
– аппаратный буфер памяти (FIFO) на 512 байт для накопления результатов измерений, позволяющий считывать данные пакетами, а не делать запрос каждый раз по отдельности;
– встроенный аппаратный сопроцессор обработки движения (Digital Motion Processor – DMP) для самостоятельного анализа данных по заранее заложенным алгоритмам;
– блоки передачи данных по SPI- и I2C-интерфейсам; – блоки питания и опорных напряжений (Bias & LDOs).
Модель 6500 по сравнению с предшествующей моделью 6050 имеет следующие отличия:
– поддержка SPI-интерфейса;
– другой объём буферной памяти FIFO (512 против 1024 байт, но это не является недостатком);
– чуть более повышенная энергоэффективность (минимальное напряжение питания 1,71 В);
– небольшие отличия в регистрах смещения, а также LP_ACCEL_ODR и PWR_MGMT_2. Но стандартные библиотеки их не используют либо обрабатывают корректно. Поэтому программно рассматриваемые сенсоры практически полностью совместимы;
– улучшенная стабильность, а также меньший уровень шумов и дрейф ноля.
На сегодняшний день 6500, как и его предшественник 6050, официально отнесен производителем к категории «Not Recommended for New Designs» (NRND – не рекомендуется для новых разработок). Однако эти устройства по-прежнему широко доступны и популярны среди DIY-разработчиков. Для более требовательных проектов можно рассмотреть современные 9-осевые модели, такие как ICM-42688-P или ICM-20948.
Параметры и характеристики
6500 является немного улучшенной версией 6050. Поэтому базовые характеристики практически одинаковые:
Режимы работы и настройка
После подачи питания или сброса микросхема проходит инициализацию, а потом по умолчанию переходит в состояние «сна» (Sleep Mode), где все основные блоки отключены, что обеспечивает минимальное потребление (до 6 мкА). После подачи питания необходимо вывести чип из этого состояния, настроив нужные параметры.
В документации выделяются следующие пользовательские режимы, которые являются комбинациями состояний отдельных сенсоров:
Также стоит отдельно упомянуть применение программируемых прерываний как специфические режимы работы:
1. Прерывание по движению (Wake-on-Motion)
Это прерывание генерируется, когда массив отсчётов с акселерометра (после прохождения через фильтр) превышает заданный пользователем порог. Подходит для пробуждения основного процессора из спящего режима при каком-либо движении;
2. Прерывание по готовности данных (Data Ready)
Генерируется каждый раз, когда новые показания от сенсоров (или из FIFO) готовы для чтения. Это основной способ информирования внешнего контроллера, что данные можно забирать.
Управление осуществляется через регистры. Ниже представлен список ключевых регистров, посредство которых осуществляется настройка и управление чипом:
Чтобы не усложнять знакомство с инерциальным модулем и не путаться в сложном многообразии регистров, мы будем пользоваться библиотекой MPU6050_light от rfetic. Она с небольшими оговорками (только базовое применение) более-менее подходит и для 6050, и для 6500.
Ниже коротко представлены примеры базовых операций с инерциальным модулем.
Быстрый старт (для проверки)
#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
void setup() {
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000);
// Инициализация (0 = ±250°/с, 0 = ±2g)
mpu.begin(0, 0);
// Калибровка (5 секунд)
mpu.calcOffsets(true, true);
Serial.println("MPU-6500 Ready!");
Serial.println(" Accel (g) \t Gyro (°/s)");
}
void loop() {
mpu.update();
Serial.print("A: ");
Serial.print(mpu.getAccX(), 2); Serial.print(", ");
Serial.print(mpu.getAccY(), 2); Serial.print(", ");
Serial.print(mpu.getAccZ(), 2);
Serial.print(" | G: ");
Serial.print(mpu.getGyroX(), 2); Serial.print(", ");
Serial.print(mpu.getGyroY(), 2); Serial.print(", ");
Serial.println(mpu.getGyroZ(), 2);
delay(100);
}
Настройка диапазонов
// настройка гироскопа
void mpu_set_gyro_range(uint8_t range) {
// 0 = ±250°/с, 1 = ±500°/с, 2 = ±1000°/с, 3 = ±2000°/с
mpu.setGyroConfig(range);
}
// настройка акселерометра
void mpu_set_accel_range(uint8_t range) {
// 0 = ±2g, 1 = ±4g, 2 = ±8g, 3 = ±16g
mpu.setAccConfig(range);
}
// примеры использования
// mpu.setGyroConfig(0); // ±250°/с
// mpu.setAccConfig(3); // ±16g
Настройка фильтров
В библиотеке MPU6500_light нет прямого доступа к параметрам DLPFCFG, так как используется программный комплементарный фильтр.
// настройка коэффициента фильтра
void mpu_set_filter(float gyro_coeff) {
// gyro_coeff: 0.0 ... 1.0 (0.98 = сильная фильтрация гироскопа)
mpu.setFilterGyroCoef(gyro_coeff);
}
// примеры использования
// mpu.setFilterGyroCoef(0.98); // Комплементарный фильтр 98% гироскоп
// mpu.setFilterGyroCoef(0.92); // 92% гироскоп, 8% акселерометрЧтение сырых данных
// чтение сырых данных акселерометра
void read_accel() {
mpu.update(); // Обновляем данные
float ax = mpu.getAccX(); // Уже в g
float ay = mpu.getAccY();
float az = mpu.getAccZ();
Serial.print("A(g): ");
Serial.print(ax); Serial.print(", ");
Serial.print(ay); Serial.print(", ");
Serial.println(az);
}
// чтение сырых данных гироскопа
void read_gyro() {
mpu.update(); // Обновляем данные
float gx = mpu.getGyroX(); // Уже в °/с
float gy = mpu.getGyroY();
float gz = mpu.getGyroZ();
Serial.print("G(°/s): ");
Serial.print(gx); Serial.print(", ");
Serial.print(gy); Serial.print(", ");
Serial.println(gz);
}
// чтение сырых данных датчика температуры
float read_temp() {
mpu.update();
return mpu.getTemp(); // Уже в градусах Цельсия
}
// чтение углов
void read_angles() {
mpu.update();
float angleX = mpu.getAngleX(); // Угол по X (крен)
float angleY = mpu.getAngleY(); // Угол по Y (тангаж)
float angleZ = mpu.getAngleZ(); // Угол по Z (рыскание)
Serial.print("Angles (deg): ");
Serial.print(angleX); Serial.print(", ");
Serial.print(angleY); Serial.print(", ");
Serial.println(angleZ);
}
Полный тестовый пример
#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
uint8_t accel_range = 0; // ±2g
uint8_t gyro_range = 0; // ±250°/с
void setup() {
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000);
// 1. Инициализация с диапазонами
mpu.begin(gyro_range, accel_range);
// 2. Калибровка
Serial.println("Калибровка...");
mpu.calcOffsets(true, true);
// 3. Настройка фильтра (опционально)
mpu.setFilterGyroCoef(0.98);
Serial.println("MPU-6500 готов!");
}
void loop() {
// 4. Обновление данных
mpu.update();
// 5. Чтение всех данных
float ax = mpu.getAccX();
float ay = mpu.getAccY();
float az = mpu.getAccZ();
float gx = mpu.getGyroX();
float gy = mpu.getGyroY();
float gz = mpu.getGyroZ();
float temp = mpu.getTemp();
float angleX = mpu.getAngleX();
float angleY = mpu.getAngleY();
float angleZ = mpu.getAngleZ();
// Вывод
Serial.print("A(g): ");
Serial.print(ax); Serial.print(", ");
Serial.print(ay); Serial.print(", ");
Serial.print(az);
Serial.print(" | G(°/s): ");
Serial.print(gx); Serial.print(", ");
Serial.print(gy); Serial.print(", ");
Serial.print(gz);
Serial.print(" | T: ");
Serial.print(temp); Serial.print(" °C");
Serial.print(" | Углы: ");
Serial.print(angleX); Serial.print(", ");
Serial.print(angleY); Serial.print(", ");
Serial.println(angleZ);
delay(100);
}
MPU-6500 в составе платы GY-9250/6500
Я купил плату GY-9250/6500, полагая, что это именно MPU-9250, ведь так было заявлено на сайте продавца. Вот такие изображения были приведены на сайте магазина:
Мне было принципиально важно иметь 9-осевой модуль, поскольку для проектирования ИУП крайне важен магнитометр. В состав микросхемы MPU-9250 входит акселерометр и гироскоп на базе кристалла 6500, а также магнитометр на базе кристалла AK8963.
Но, как это часто бывает, не всё так, как хотелось бы. Попавшая мне в руки плата содержит конкретно только 6500, без кристалла AK8963:
Чтобы твёрдо и чётко удостовериться в истинной природе чипа, нужно его подключить к отладочной плате микроконтроллера (схема подключения представлена в следующем разделе) и прошить представленный ниже код. Он определяет, какой конкретно чип перед Вами. В мониторе порта отобразится уникальный идентификатор модели сенсора и сообщение о том, есть ли на борту регистратор магнитного поля:
#include <Wire.h>
#define MPU_ADDR 0x68
#define MAG_ADDR 0x0C
void setup() {
Serial.begin(115200);
Wire.begin();
Serial.println(F("Проверка MPU-6500 / MPU-9250"));
// 1. Проверка наличия MPU по адресу 0x68
Wire.beginTransmission(MPU_ADDR);
if (Wire.endTransmission() != 0) {
Serial.println(F("Датчик по адресу 0x68 не найден!"));
Serial.println(F("Проверьте подключение."));
while (true);
}
// 2. Чтение WHO_AM_I
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x75);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDR, 1);
uint8_t mpuWhoAmI = Wire.read();
Serial.print(F("MPU WHO_AM_I: 0x"));
Serial.println(mpuWhoAmI, HEX);
// 3. Интерпретация ID
if (mpuWhoAmI == 0x70) {
Serial.println(F(" --> MPU-6500 (основной чип)"));
} else if (mpuWhoAmI == 0x68) {
Serial.println(F(" --> MPU-6050 (предыдущая модель)"));
} else {
Serial.print(F(" --> Неизвестный чип: 0x"));
Serial.println(mpuWhoAmI, HEX);
}
// 4. Включение bypass-режима для доступа к магнитометру
Serial.println(F("\nВключение bypass-режима..."));
// Выход из сна (PWR_MGMT_1)
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission();
delay(100);
// Bypass mode (INT_PIN_CFG)
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x37);
Wire.write(0x02); // I2C_BYPASS_EN = 1
Wire.endTransmission();
delay(50);
// 5. Проверка магнитометра AK8963
Wire.beginTransmission(MAG_ADDR);
if (Wire.endTransmission() != 0) {
Serial.println(F("Магнитометр по адресу 0x0C не отвечает"));
Serial.println(F("Это MPU-6500 (6 осей, без магнитометра)"));
while (true);
}
Wire.beginTransmission(MAG_ADDR);
Wire.write(0x00); // WHO_AM_I регистр AK8963
Wire.endTransmission(false);
Wire.requestFrom(MAG_ADDR, 1);
if (Wire.available()) {
uint8_t magWhoAmI = Wire.read();
Serial.print(F("AK8963 WHO_AM_I: 0x"));
Serial.println(magWhoAmI, HEX);
if (magWhoAmI == 0x48) {
Serial.println(F("\n Это MPU-9250 (9 осей: MPU-6500 + AK8963)!"));
} else {
Serial.print(F("Неизвестный ID магнитометра: 0x"));
Serial.println(magWhoAmI, HEX);
}
}
Serial.println(F("\n Проверка завершена"));
}
void loop() {}
Как видим, сканер показал нам ID = 0x70, что соответствует чипу модели 6500. При этом кристалл AK8963 по адресу 0x0C отсутствует.
Плата GY-9250/6500 содержит всю необходимую обвязку, включая подтягивающие резисторы, конденсаторы фильтрации питания, а также понижающий (с низким падением напряжения) линейный стабилизатор напряжения на 3,3 В.
Ниже представлены таблица распиновки и принципиальная схема платы (конкретно в нашем случае – GY-6500):
Схема подключения MPU-6500 к Arduino UNO
Итак, переходим к сборке схемы. Целью нашего урока является создание прототипа ИУП. Как я уже говорил ранее, для максимальной точности нужна использовать мощь всех 9 осей (акселерометр/гироскоп/магнитометр). Именно поэтому я намеревался использовать 9250. Но на руках у меня оказалась 6500-я модель. Недостающий чип для регистрации магнитного поля я решил компенсировать отдельным модулем – QMC5883L, о котором можно подробно прочитать в статье про компас на ESP32.
В качестве базовой платформы я выбрал Arduino UNO на базе микроконтроллера ATmega328P-PU в корпусе DIP-28, поскольку мне потребуется собрать опытный образец устройства в корпусе, используя именно такую микросхему. При этом программный код будет совместим с ESP32, нужно будет только поменять назначение пинов.
Для отображения степени поворота в градусах я решил применить круглый TFT-дисплей на базе контроллера GC9A01, который в прототипе отлично подойдет для индикации уровня заряда аккумулятора (шкала сегментов, размещенная по длине окружности).
Базовым вариантом коммуникации с инерциальным модулем является подключение по I2C-интерфейсу. QMC5883L – также на I2C. Дисплей использует SPI-протокол. Ниже представлены таблица и схема подключений, а также собранный макет устройства:
Программный код (скетч)
Проект разрабатывался в среде программирования Arduino IDE 2. Иногда могут возникнуть трудности с доступом к официальному сайту. В качестве альтернативного варианта можно воспользоваться официальным репозиторием на GitHub.
Для взаимодействия с TFT-дисплеем воспользуемся простой и надёжной библиотекой Adafruit_GC9A01A.
Как упоминалось ранее, мы будем использовать библиотеку MPU6050_light от rfetic для инерциального модуля. А для взаимодействия с регистратором магнитного поля применим популярную библиотеку QMC5883LCompass.
Ниже представлен очень подробно прокомментированный код для ИУП:
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_GC9A01A.h>
#include <MPU6050_light.h>
#include <QMC5883LCompass.h>
// Настройкапинов для дисплея
#define TFT_CS 10
#define TFT_DC 9
#define TFT_RST 8
// Создание объекта дисплея с указанием пинов CS, DC, RST
Adafruit_GC9A01A tft(TFT_CS, TFT_DC, TFT_RST);
MPU6050 mpu(Wire); // Объект MPU6050, использующий шину Wire (I2C) для обмена данными
QMC5883LCompass compass; // Объект магнитометра QMC5883L (по умолчанию адрес 0x0D)
// Константы настроек
#define DECLINATION_ANGLE 10.0 // Магнитное склонение для Москвы (в градусах) – поправка на истинный север
#define GYRO_LPF_ALPHA 0.1 // коэффициент сглаживания, нормальными значениями являются 0,05...0,2
#define CALIBRATION_TIME 10000 // Время калибровки магнитометра в миллисекундах (10 секунд)
#define CENTER_X 120 // Координата X центра экрана (пиксели) – ширина 240, центр в 120
#define CENTER_Y 120 // Координата Y центра экрана (пиксели) – высота 240, центр в 120
#define FILTER_COEFF 0.96 // Коэффициент дополнительного фильтра (0.96 = 96% доверия гироскопу, 4% – магнитометру)
unsigned long lastMicros = 0; // Время предыдущего измерения (в микросекундах) – используется для расчёта dt
float gyroAngle = 0; // Абсолютный угол, накопленный интегрированием гироскопа (в градусах)
float zeroOffset = 0; // Угол в момент установки нуля – вычитается из абсолютного для получения относительного угла
bool zeroSet = false; // Флаг: установлен ли ноль? (если false, основной цикл loop() не выполняется)
float lastDisplayedAngle = 999; // Последний угол, выведенный на дисплей – для предотвращения частых перерисовок
unsigned long lastDispUpdate = 0;// Время последнего обновления дисплея (в миллисекундах) – для ограничения частоты
// Переменные для хранения калибровочных данных магнитометра
int calXmin = 0, calXmax = 0; // Минимальное и максимальное значения по оси X (сырые показания АЦП)
int calYmin = 0, calYmax = 0; // Минимальное и максимальное значения по оси Y
int calZmin = 0, calZmax = 0; // Минимальное и максимальное значения по оси Z
// Функция для вывода текста по центру экрана
void centerText(int y, const char* text, uint16_t color) {
// y – вертикальная координата (отсчёт сверху)
// text – строка для вывода
// color – цвет текста
tft.setTextSize(2); // Размер шрифта (2 – увеличенный, базовый размер 1)
tft.setTextColor(color); // Устанавливаем цвет текста
int16_t x1, y1; // Переменные для получения размеров текста (не используются, но обязательны для getTextBounds)
uint16_t w, h; // Ширина и высота текста в пикселях
// Получаем размеры текста (без учёта текущей позиции курсора)
tft.getTextBounds(text, 0, 0, &x1, &y1, &w, &h);
// Устанавливаем курсор так, чтобы текст оказался по центру по горизонтали:
// CENTER_X – половина ширины текста, по вертикали – переданная координата y
tft.setCursor(CENTER_X - w/2, y);
tft.print(text); // Выводим текст на дисплей
}
// Функция отображения числового значения угла
void displayAngle(float angle) {
// Очищаем центральную область экрана (квадрат 240x240 с центром в CENTER_X, CENTER_Y)
// Заливаем чёрным цветом, чтобы стереть предыдущее значение
tft.fillRect(CENTER_X - 100, CENTER_Y - 100, 240, 240, GC9A01A_BLACK);
// Устанавливаем цвет текста – белый на чёрном фоне
tft.setTextColor(GC9A01A_WHITE, GC9A01A_BLACK);
tft.setTextSize(5); // Крупный размер шрифта (5) для отображения угла
char buf[10]; // Буфер для преобразования числа в строку
// Преобразуем угол (float) в строку: 5 символов всего, 1 знак после запятой
dtostrf(angle, 5, 1, buf);
int16_t x1, y1; // Для получения размеров текста
uint16_t w, h; // Ширина и высота текста
tft.getTextBounds(buf, 0, 0, &x1, &y1, &w, &h); // Получаем размеры текста
// Устанавливаем курсор в центр экрана (по горизонтали и вертикали) с учётом размеров текста
tft.setCursor(CENTER_X - w/2, CENTER_Y - h/2);
tft.print(buf); // Выводим число
// Рисуем символ градуса в виде маленького кружка справа от числа
// Координаты кружка: правее текста на 8 пикселей, немного выше центра (коррекция по высоте)
tft.drawCircle(CENTER_X + w/2 + 8, CENTER_Y - h/2 + 5, 4, GC9A01A_WHITE); // Контур круга радиусом 4
tft.fillCircle(CENTER_X + w/2 + 8, CENTER_Y - h/2 + 5, 3, GC9A01A_WHITE); // Заливка круга радиусом 3 (создаёт толстый кружок)
}
// Калибровка магнитометра
void calibrateMagnetometer() {
// Инициализируем минимальные и максимальные значения для каждой оси:
// минимумы – максимальное положительное число, максимумы – минимальное отрицательное
int xmin = 32767, xmax = -32768;
int ymin = 32767, ymax = -32768;
int zmin = 32767, zmax = -32768;
unsigned long start = millis(); // Запоминаем время начала калибровки
int samples = 0; // Счётчик прочитанных измерений (используется для обновления экрана)
// Цикл длится пока не пройдёт CALIBRATION_TIME миллисекунд (10 секунд)
while (millis() - start < CALIBRATION_TIME) {
compass.read(); // Читаем данные с магнитометра (все три оси)
// Получаем сырые значения по осям
int x = compass.getX(), y = compass.getY(), z = compass.getZ();
// Обновляем минимумы и максимумы
if (x < xmin) xmin = x;
if (x > xmax) xmax = x;
if (y < ymin) ymin = y;
if (y > ymax) ymax = y;
if (z < zmin) zmin = z;
if (z > zmax) zmax = z;
samples++; // Увеличиваем счётчик
// Каждые 20 измерений (примерно 200 мс) обновляем информацию на экране
if (samples % 20 == 0) {
char buf[20];
// Вычисляем оставшееся время (в секундах)
int remaining = (CALIBRATION_TIME - (millis() - start)) / 1000;
sprintf(buf, "Remain: %d sec", remaining);
// Очищаем область для текста (прямоугольник 190x30)
tft.fillRect(CENTER_X - 100, 110, 190, 30, GC9A01A_BLACK);
// Выводим текст по центру экрана по вертикали y=120
centerText(120, buf, GC9A01A_WHITE);
}
delay(10);
}
// По окончании калибровки сохраняем полученные экстремумы в глобальные переменные
calXmin = xmin; calXmax = xmax;
calYmin = ymin; calYmax = ymax;
calZmin = zmin; calZmax = zmax;
// Выводим результаты калибровки в Serial (для отладки)
Serial.println(F("\n Калибровка QMC5883L завершена"));
Serial.print(F(" X: min=")); Serial.print(xmin); Serial.print(F(" max=")); Serial.println(xmax);
Serial.print(F(" Y: min=")); Serial.print(ymin); Serial.print(F(" max=")); Serial.println(ymax);
Serial.print(F(" Z: min=")); Serial.print(zmin); Serial.print(F(" max=")); Serial.println(zmax);
}
// Функция ручной калибровки магнитных данных для коррекции сырых значений
void calibrateMagneticData(float &mx, float &my, float &mz) {
// Эта функция получает ссылки на три переменные (сырые данные) и модифицирует их,
// приводя к калиброванному виду (центрирование и масштабирование).
// Вычисляем смещение (offset) как среднее между минимумом и максимумом
float xOffset = (calXmin + calXmax) / 2.0;
float yOffset = (calYmin + calYmax) / 2.0;
float zOffset = (calZmin + calZmax) / 2.0;
// Вычитаем смещение – теперь данные центрированы относительно ноля
mx -= xOffset;
my -= yOffset;
mz -= zOffset;
// Вычисляем полуразмах (диапазон/2) по каждой оси
float xRange = (calXmax - calXmin) / 2.0;
float yRange = (calYmax - calYmin) / 2.0;
float zRange = (calZmax - calZmin) / 2.0;
// Защита от деления на ноль: если диапазон меньше 1, считаем его равным 1
if (xRange < 1) xRange = 1;
if (yRange < 1) yRange = 1;
if (zRange < 1) zRange = 1;
// Вычисляем средний полуразмах по всем трём осям
float avgRange = (xRange + yRange + zRange) / 3.0;
// Масштабируем каждую ось так, чтобы её полуразмах стал равен среднему
// Это выравнивает чувствительность по осям
mx *= (avgRange / xRange);
my *= (avgRange / yRange);
mz *= (avgRange / zRange);
}
// Функция получения угла с компенсацией наклона
float getTiltCompensatedAngle() {
// Эта функция возвращает угол азимута (0-360°) с учётом наклона устройства (крен и тангаж)
// и с применением магнитного склонения.
compass.read(); // Читаем свежие данные с магнитометра
// Получаем углы наклона из акселерометра MPU6050 (в градусах)
float roll = mpu.getAccAngleX(); // Крен (поворот вокруг оси X)
float pitch = mpu.getAccAngleY(); // Тангаж (поворот вокруг оси Y)
// Берём сырые данные с магнитометра
float mx = compass.getX();
float my = compass.getY();
float mz = compass.getZ();
// Применяем калибровку (центрирование + масштабирование) к сырым данным
calibrateMagneticData(mx, my, mz);
// Переводим углы наклона из градусов в радианы (для тригонометрических функций)
float rollRad = roll * DEG_TO_RAD;
float pitchRad = pitch * DEG_TO_RAD;
// Компенсация наклона: проекция вектора магнитного поля на горизонтальную плоскость
// Формулы взяты из стандартных алгоритмов для магнитометра с акселерометром
float mxh = mx * cos(pitchRad) + mz * sin(pitchRad); // Горизонтальная составляющая X
float myh = mx * sin(rollRad) * sin(pitchRad) + my * cos(rollRad) - mz * sin(rollRad) * cos(pitchRad);
// Горизонтальная составляющая Y
// Вычисляем угол азимута через арктангенс (atan2) и переводим в градусы
float angle = atan2(myh, mxh) * RAD_TO_DEG;
// Добавляем магнитное склонение
angle += DECLINATION_ANGLE;
// Нормализуем угол в диапазон [0, 360)
if (angle < 0) angle += 360;
if (angle >= 360) angle -= 360;
return angle;
}
// Функция установки нулевого положения
void setZero() {
// Очищаем экран
tft.fillScreen(GC9A01A_BLACK);
// Выводим инструкции для пользователя
centerText(60, "SET ZERO", GC9A01A_YELLOW);
centerText(90, "Place at 0 position", GC9A01A_WHITE);
centerText(120, "DO NOT MOVE!", GC9A01A_RED);
// Обратный отсчёт 5 секунд, чтобы пользователь успел установить устройство в нужное положение
for (int i = 5; i > 0; i--) {
char buf[20];
sprintf(buf, "Set in: %d sec", i);
// Очищаем область для текста
tft.fillRect(CENTER_X - 80, 135, 160, 30, GC9A01A_BLACK);
centerText(150, buf, GC9A01A_WHITE);
delay(1000); // Ждём 1 секунду
}
// По истечении отсчёта фиксируем текущий угол как нулевой
zeroOffset = getTiltCompensatedAngle(); // Сохраняем смещение
gyroAngle = zeroOffset; // Синхронизируем гироскоп с этим углом
zeroSet = true; // Устанавливаем флаг, что ноль задан
// Очищаем экран и показываем 0°
tft.fillScreen(GC9A01A_BLACK);
displayAngle(0);
// Сообщаем в Serial
Serial.println(F("\n Ноль установлен"));
}
// Инициализация микроконтроллера
void setup() {
// Инициализация последовательного порта для отладки (скорость 115200 бод)
Serial.begin(115200);
// Инициализация шины I2C (общение с датчиками)
Wire.begin();
Wire.setClock(400000); // Устанавливаем частоту 400 кГц (Fast Mode)
delay(1000); // Даём питанию и датчикам стабилизироваться
// Инициализация дисплея
tft.begin();
tft.fillScreen(GC9A01A_BLACK); // Заливаем экран чёрным
tft.setRotation(2); // Поворот на 180 градусов, чтобы верх экрана был со стороны разъёма
// Вывод информации о проекте в Serial
Serial.println(F(" SPRYTRON.RU "));
Serial.println(F("Измеритель угла поворота"));
Serial.println(F("MPU-6500 QMC5883L GC9A01"));
// 1. Инициализация и калибровка MPU6500
centerText(50, "Init MPU6500...", GC9A01A_WHITE);
// Пытаемся инициализировать MPU6050 (если вернёт не 0 – ошибка)
if (mpu.begin() != 0) {
centerText(70, "ERROR!", GC9A01A_RED);
while (1); // Бесконечный цикл – остановка выполнения
}
// Сообщаем о начале калибровки MPU (5 секунд)
centerText(70, "Calibrating 5s...", GC9A01A_YELLOW);
centerText(160, "DO NOT MOVE!", GC9A01A_RED);
// Обратный отсчёт 5 секунд перед калибровкой
for (int i = 5; i > 0; i--) {
char buf[10];
sprintf(buf, "%d sec", i);
tft.fillRect(CENTER_X - 50, 90, 100, 25, GC9A01A_BLACK);
centerText(90, buf, GC9A01A_WHITE);
delay(1000);
}
// Запускаем калибровку гироскопа и акселерометра (вычисляем смещения)
mpu.calcOffsets(true, true);
// Выводим сообщение об успехе
Serial.println(F("MPU6500 калибровка завершена"));
// 2. Инициализация QMC5883L
centerText(110, "Init QMC5883L...", GC9A01A_WHITE);
compass.init(); // Инициализация магнитометра (настройка регистров по умолчанию)
// Устанавливаем режим: Continuous (01), ODR=200Hz (0x0D), RNG=8G (0x01), OSR=512 (0x01)
// Согласно даташиту: режим непрерывных измерений, высокая частота обновления, полный диапазон ±8Гс, высокое передискретизация
compass.setMode(0x01, 0x0D, 0x01, 0x01);
delay(100); // Небольшая задержка для стабилизации
// Проверяем, отвечает ли устройство по I2C адресу 0x0D
Wire.beginTransmission(0x0D);
if (Wire.endTransmission() != 0) { // Если передача не удалась – устройство не найдено
centerText(130, "ERROR!", GC9A01A_RED);
while (1);
}
centerText(130, "QMC5883L OK", GC9A01A_GREEN);
delay(1000); // Даём пользователю прочитать сообщение
// 3. Калибровка магнитометра
tft.fillScreen(GC9A01A_BLACK);
centerText(40, "CALIBRATE MAG", GC9A01A_YELLOW);
centerText(70, "Rotate device", GC9A01A_WHITE);
centerText(90, "10 seconds", GC9A01A_WHITE);
// Запускаем калибровку магнитометра (сбор экстремумов)
calibrateMagnetometer();
// 4. Установка нулевого положения
setZero();
// Запоминаем текущее время в микросекундах для расчёта dt в основном цикле
lastMicros = micros();
}
// Основной бесконечный цикл программы
void loop() {
// Если ноль ещё не установлен – выходим (ничего не делаем)
if (!zeroSet) return;
// Обновляем данные с MPU6050 (читаем новые значения акселерометра и гироскопа)
mpu.update();
// 1. Вычисление времени dt
unsigned long now = micros(); // Текущее время в микросекундах
float dt = (now - lastMicros) / 1000000.0; // Разница в секундах (переводим микросекунды в секунды)
lastMicros = now; // Запоминаем время для следующего шага
// Защита от аномально больших dt (например, при зависании) – ограничиваем 0.05 сек
if (dt > 0.05) dt = 0.05;
// 2. Получение скорости вращения с гироскопа
float rawRate = mpu.getGyroZ(); // Сырая угловая скорость по оси Z (градусы/сек)
// используем экспоненциальный фильтр нижних частот, который сглаживает шум
gyroRateFiltered = GYRO_LPF_ALPHA * rawRate + (1.0 - GYRO_LPF_ALPHA) * gyroRateFiltered;
float gyroRate = gyroRateFiltered;
// 3. Получение стабильного угла с магнитометра (с компенсацией наклона)
float magAngle = getTiltCompensatedAngle(); // Угол от 0 до 360
// 4. Дополнительный фильтр (слияние гироскопа и магнитометра)
// Интегрируем гироскоп: прибавляем приращение угла за время dt
gyroAngle += gyroRate * dt;
// Вычисляем разницу между показаниями магнитометра и гироскопа
float diff = magAngle - gyroAngle;
// Нормализуем разницу в диапазон [-180, 180] (чтобы корректно обрабатывать переход через 0/360)
if (diff > 180.0) diff -= 360.0;
if (diff < -180.0) diff += 360.0;
// Корректируем угол гироскопа на часть разницы (FILTER_COEFF определяет скорость коррекции)
gyroAngle += FILTER_COEFF * diff;
// 5. Вычисление относительного угла (относительно нуля)
float relativeAngle = gyroAngle - zeroOffset;
// Нормализуем относительный угол в диапазон [-180, 180] для отображения
if (relativeAngle > 180.0) relativeAngle -= 360.0;
if (relativeAngle < -180.0) relativeAngle += 360.0;
// 6. Обновление дисплея (не чаще чем раз в 50 мс)
if (millis() - lastDispUpdate >= 50) {
lastDispUpdate = millis();
// Если угол изменился более чем на 0.1 градуса – обновляем экран (экономия ресурсов)
if (abs(relativeAngle - lastDisplayedAngle) > 0.1) {
displayAngle(relativeAngle);
lastDisplayedAngle = relativeAngle;
}
}
// 7. Вывод данных в монитор порта (для отладки) раз в 200 мс
static unsigned long lastPrint = 0; // Статическая переменная – сохраняет значение между вызовами loop
if (millis() - lastPrint >= 200) {
lastPrint = millis();
Serial.print("Угол: ");
Serial.println(relativeAngle, 1); // Выводим угол с одним знаком после запятой
}
}
Алгоритм работы
В рамках инициализации системы выполняется 3 последовательных этапа настройки:
1) MPU-6500
Для устранения систематических ошибок чувствительных элементов (смещений по осям) выполняется калибровка посредством выдержки устройства в неподвижном состоянии в течение 5 секунд (метод mpu.calcOffsets(true, true)). При помощи внутренних функций библиотеки вычисляются средние уровни смещений. Полученные результаты сохраняются для последующей компенсации.
2) QMC5883L
В рамках этой операции определяются диапазоны значений магнитного поля для компенсации внешних помех. Устройство необходимо вращать по всем по осям (X, Y, Z) в течение 10 секунд, собирая данные. При этом фиксируются минимальные и максимальные значения для каждой оси. Эти данные используются для настройки чувствительности.
Длительность подготовки определяется переменной CALIBRATION_TIME. За 10 секунд датчик опрашивается 1000 раз, и программа запоминает только крайние значения (минимумы и максимумы) по каждой оси. Чем дольше собираем точки, тем выше шанс, что эти экстремумы будут реальными, а не случайными, что напрямую влияет на точность центрирования и масштабирования данных. Если времени мало, то Вы можете попросту не успеть повернуть датчик во все стороны, и настройка будет неполной.
В представленном коде используется упрощенная калибровка магнитометра, которая компенсирует только hard-iron искажения (смещение нуля). Она не учитывает soft-iron искажения (влияние металла и токов на форму поля), что может давать ошибку примерно до 5 градусов. Такая упрощённая подготовка, реализованная в коде, подходит только для демонстрации концепции (игрушки, прототипы). Для измерительных приборов и навигации она неприменима.
3) Установка нулевого положения
Как только будут завершены процессы подготовки основных сенсоров, далее будет выполняться фиксация начального угла (zeroOffset), с которого будет вестись отсчёт. Для этого перед началом измерений следует установить устройство в исходное положение, которое будет считаться отправной точкой. На эту процедуру система выделяет 5 секунд (ZEROING_DELAY), чтобы пользователь успел установить прибор на позицию. По истечению времени текущий угол принимается за нулевой (zeroOffset = getTiltCompensatedAngle()), то есть это исходное положение.
В основном цикле программы первым делом обновляются данные с гироскопа и акселерометра:
mpu.update();Затем вычисляется время, прошедшее с предыдущего измерения. Это нужно для корректного интегрирования скорости вращения:
unsigned long now = micros();
float dt = (now - lastMicros) / 1000000.0;
lastMicros = now;
if (dt > 0.05) dt = 0.05;Полученная скорость вращения по оси Z проходит через экспоненциальный фильтр нижних частот, который сглаживает шум, не обнуляя полезный сигнал:
float rawRate = mpu.getGyroZ();
gyroRateFiltered = GYRO_LPF_ALPHA * rawRate + (1.0 - GYRO_LPF_ALPHA) * gyroRateFiltered;
float gyroRate = gyroRateFiltered;Параллельно с этим считывается угол с магнитометра. Внутри функции getTiltCompensatedAngle() происходят несколько важных операций: сначала берутся сырые значения с магнитометра, затем к ним применяется настройка (центрирование и масштабирование), после чего на основе данных с акселерометра вычисляются углы наклона устройства (крен и тангаж). Эти углы позволяют спроецировать вектор магнитного поля на горизонтальную плоскость. Так мы получаем азимут, который не зависит от того, как наклонено устройство в пространстве:
float mxh = mx * cos(pitchRad) + mz * sin(pitchRad);
float myh = mx * sin(rollRad) * sin(pitchRad) + my * cos(rollRad) - mz * sin(rollRad) * cos(pitchRad);
float angle = atan2(myh, mxh) * RAD_TO_DEG + DECLINATION_ANGLE;Полученный угол нормализуется в диапазон 0…360 градусов.
Теперь вступает в работу дополнительный фильтр, который объединяет данные от гироскопа и магнитометра. Гироскоп интегрируется (накапливается), и к нему прибавляется текущая скорость, умноженная на время:
gyroAngle += gyroRate * dt;Затем вычисляется разница между показаниями магнитометра и гироскопа. Чтобы эта разница корректно вычислялась даже при переходе через ноль (например, с 359° на 1°), она нормализуется в диапазон от -180 до +180 градусов. После этого гироскопический угол корректируется на часть этой разницы. Таким способом гироскоп сохраняет быструю динамику (96% доверия), а магнитометр медленно, но уверенно корректирует накапливающийся дрейф (4% доверия):
float diff = magAngle - gyroAngle;
if (diff > 180.0) diff -= 360.0;
if (diff < -180.0) diff += 360.0;
gyroAngle += FILTER_COEFF * diff;На выходе мы получаем абсолютный угол, но пользователю нужен угол относительно того положения, которое он выбрал как нулевое. Поэтому из текущего угла вычитается сохранённое смещение zeroOffset, и результат нормализуется в диапазон [-180, 180]. Это и есть тот самый угол, который отображается на экране:
float relativeAngle = gyroAngle - zeroOffset;
if (relativeAngle > 180.0) relativeAngle -= 360.0;
if (relativeAngle < -180.0) relativeAngle += 360.0;С точки зрения математики в алгоритме используется комплементарная фильтрация, где гироскоп обеспечивает высокую частоту обновления и точность при движениях, магнитометр и акселерометр обеспечивают точность в состоянии покоя, а функция компенсации наклона позволяет корректно производить измерения при отклонениях от горизонтали.
Такой подход значительно снижает чувствительность к вибрациям, быструю реакцию на движения, а также высокую точность в статичном состоянии.
Основным недостатком является зависимость от внешних магнитных полей (металлические предметы, динамики, моторы), которые искажают показания. В таких условиях комплементарный фильтр может давать погрешность, так как он опирается на абсолютные показания компаса.
Эксперимент
После подачи питания на устройство микроконтроллер производит инициализацию. Сперва – для MPU-6500, при этом устройство должно быть неподвижно в течение 5 секунд:
После подготовки инерциального модуля производится инициализация магнитометра, за которой следует его калибровка. В течение 10 секунд производится сбор точек, в это время нужно вращать устройство по всем трёх осям, чтобы учесть как можно больше различных ориентаций чувствительного элемента в пространстве. Время Вы можете в коде устанавливать на своё усмотрение.
Далее будет выделено 5 секунд (Вы также можете увеличить время, если потребуется), чтобы расположить устройство в исходное состояние, относительно которого начнётся измерение:
По окончанию всех процедур можно приступать к измерениям:
Попробуем провести эксперимент. Перезагрузим микроконтроллер, откалибруем датчики и разместим макет относительно прямого угла стола:
Повернём на 90 градусов:
Результат: примерно 91,5 градуса. Поскольку из-за дрейфа показания немного «скачут», сложно чётко зафиксировать конкретную величину. При этом надо иметь ввиду, что отклонение может быть обусловлено дополнительными факторами: небольшое отклонение плат инерциального модуля и компаса при размещении на макетной плате, неидеальное позиционирование макетки на столе, недостаточное качество настройки магнитометра.
Повернём ещё на 90 градусов (суммарно будет 180 относительно исходного состояния):
Получили приблизительно 178,7 градуса.
Разумеется, такая методика тестирования не совсем корректная и не позволяет достоверно оценить погрешность. Однако целью данного эксперимент не ставилось создание прецизионного измерительного прибора. Данный эксперимент показывает, что применение 9-осевого инерциального модуля позволяет создать систему беспроводного контроллера, которая с приемлемой точностью определяет полную пространственную ориентацию тела.
Заключение
В данной статье мы познакомились с инерциальным модулем MPU-6500 от компании TDK InvenSense. Этот 6-осевой датчик является прямым наследником популярной 6050-й модели и по-прежнему остается одним из самых популярных выборов для DIY-проектов и профессиональных разработок благодаря своему богатому набору функций и проверенной временем архитектуре.
Основными достоинствами рассмотренного инерциального модуля являются:
– наличие цифрового процессора движений (DMP): дополнительные вычислительные мощности, которые позволяют получать готовые, отфильтрованные величины углов без дополнительных затрат на алгоритмы на стороне управляющего контроллера. Это дает значительную экономию вычислительных ресурсов и упрощает разработку;
– гибкость и точность настроек: доступен широкий выбор программируемых диапазонов измерений для основных чувствительных элементов. Это позволяет адаптировать датчик под конкретную задачу: от регистрации плавных движений до регистрации высоких перегрузок;
– развитая периферия: наличие 512-байтового FIFO-буфера позволяет организовать эффективный сбор данных пакетами, уменьшая нагрузку на шину и экономя энергию. Также присутствует вспомогательный I2C-интерфейс для подключения внешних датчиков, например, магнитометра, что позволяет создать полноценную 9-осевую систему ориентации;
– надежность и распространенность: огромная база готовых библиотек, кода и документации делает MPU-6500 доступным даже для начинающих разработчиков.
На практике мы убедились, что прибор на базе 9-осевой инерциальной системы (акселерометр/гироскоп + магнитометр) подходит для создания устройств определения угла поворота, но не как измерительный прибор.
Тем не менее, если комплексно использовать данные со всех 9 осей, а также применив алгоритм комплементарной фильтрации, можно добиться плавности и отзывчивости, что позволит определить полную пространственную ориентацию тела. А это отлично подходит для беспроводных контроллеров управления. Если же Ваша задача выходит за рамки классической ориентации и требует, например, сверхнизкого энергопотребления для носимой электроники, то стоит обратить внимание на более современные решения, такие как BMI270 или ICM-20948, которые предлагают расширенные алгоритмы и улучшенное энергосбережение.