MPU-6050 — это популярный 6-осевой инерциальный измерительный модуль, объединяющий в одном корпусе трёхосевой акселерометр и трёхосевой гироскоп. Благодаря такой конфигурации сенсор позволяет точно отслеживать ориентацию и движение объектов в пространстве, что делает его незаменимым для систем навигации, анализаторов физической активности, устройств распознавания жестов, систем стабилизации роботов, а также стабилизации видео- и фотокамер.
В статье мы подробно рассмотрим устройство и принцип работы данного сенсора, а также в качестве примера соберём макет цифрового угломера (инклинометр) на базе ESP32 с индикацией на OLED-дисплее, который покажет степень наклона с точностью до долей градуса.
Содержание
- Устройство и принцип работы
- Параметры и характеристики
- MPU-6050 в составе модуля GY-521
- Схема подключения
- Программный код (скетч)
- Эксперимент
- Алгоритм работы
- Заключение
MPU-6050 — это микросхема датчика линейного ускорения и угловой скорости вращения, построенная по технологии MEMS (Micro-Electro-Mechanical Systems, микроэлектромеханическая система) от компании InvenSense (ныне часть TDK). Также данный датчик называют инерциальным измерительным модулем (Inertial Measurement Unit — IMU) с шестью степенями свободы.
Чип включает в себя:
— акселерометр: линейное ускорение по осям X, Y, Z (м/с² или g).
— гироскоп: угловую скорость вращения вокруг тех же осей (градусы/секунду — °/с).
Этот тандем позволяет точно определять ориентацию в пространстве: крен (roll), тангаж (pitch) и даже рыскание (yaw) — при использовании фильтров.
Ключевым блоком этого модуля является встроенный DMP (Digital Motion Processor — цифровой процессор движения). Это как «мозг» микросхемы: он может сам обрабатывать данные, фильтровать шумы и выдавать готовые углы, не нагружая основной микроконтроллер.
В состав микросхемы входит датчик температуры (термистор). Вот основные характеристики и особенности:
— диапазон измерения: от -40 °C до +85 °C;
— разрешение: около 0,06 °C (при использовании 16-битного АЦП);
— точность: ±1 °C;
— интерфейс: данные с датчика температуры передаются через I2C-шину.
Термистор применяется для компенсация температурных изменений, а также для мониторинга температуры устройства.
Существует иная модификация микросхемы: MPU-6000. В целом, обе модели (6000 и 6050) по параметрам и характеристикам одинаковы. Но отличие модели 6000 заключается в следующем:
— возможность подключать к микроконтроллеру по интерфейсам I2C или SPI (по SPI более высокоскоростная передача данных);
— общее питание для цифровой и аналоговой части микросхемы. То есть у 6000-й версии нет отдельного контакта VLOGIC для настройки логических уровней;
— некоторое отличие в программной части работы с сенсором (более сложное).
Таким образом, если важнее обработка данных на борту устройства и простота настройки — выбирайте 6050.
Для более сложных приложений можно рассмотреть следующие аналоги:
— MPU-9250: 6050 + магнитометр AK8963 (9 осей);
— BMI160: более современный и точный аналог;
— ICM-20948: 9 осей, низкое энергопотребление.
Для реализации функционала угломера вполне может быть достаточно и данных с акселерометра. Но мы также будем использовать показания с гироскопа, чтобы улучшить точность измерений.
Устройство и принцип работы акселерометра и гироскопа
В статье про шагомер на базе цифрового акселерометра ADXL345 описывается принцип работы акселерометра в виде MEMS-системы. Чтобы не повторяться, здесь мы уделим внимание другому, но не менее интересному вопросу — принцип работы регистратора угловой скорости в составе MEMS-сенсора.
Принцип работы гироскопа основан на эффекте Кориолиса: когда тело с некой массой движется в определённом направлении со некоторой скоростью, и к этому телу прикладывается внешнее угловое движение, создаётся сила (сила Кориолиса), которая вызывает перпендикулярное смещение этого движущегося тела. Скорость движения напрямую связана с приложенным угловым перемещением.
Если говорить совсем простыми словами, эффект Кориолиса — это отклонение движущихся объектов от прямой траектории из-за вращения системы, в которой они находятся. Представьте себе карусель, и если Вы просто стоите на ней — то всё нормально. Но если вы идёте прямо к центру или от центра — ваше движение кажется изогнутым. Это происходит потому, что разные точки карусели движутся с разной скоростью.
И ещё один очень простой пример. Представьте, что Вы бросаете мяч на вращающейся платформе — он полетит не прямо, а по кривой.
Конкретно в MPU-6050 эффект Кориолиса меняет ёмкость между движущимися массами в зависимости от направления вращения. Этот сигнал считывается и превращается в показания угловой скорости.
Но колебания внутри гироскопа могут возникать не только из-за эффекта Кориолиса, но и из-за линейных ускорений вдоль оси Y. Чтобы избавиться от этой помехи, на одном чипе делают две рамки с массами, которые колеблются в противоположных направлениях. Благодаря этому силы Кориолиса на эти массы тоже направлены в разные стороны и взаимно компенсируют влияние линейных ускорений.
На изображении ниже представлена конструкция MEMS-гироскопа:
Основные элементы:
— Proof mass (2DOF) — инерционная масса с двумя степенями свободы, которая вибрирует в одной плоскости. При вращении гироскопа возникает сила Кориолиса, вызывающая колебания массы в перпендикулярной плоскости;
— Stationary Drive и Stationary Sense — стационарные элементы, обеспечивающие возбуждение и измерение колебаний массы;
— Movable Drive (1DOF) и Movable Sense (1DOF) — подвижные элементы с одной степенью свободы, которые помогают фиксировать и усиливать сигналы;
— Anchor — крепление, удерживающее конструкцию на месте;
— Z, Rate input — ось вращения, вдоль которой измеряется угловая скорость;
— X, Sense и Y, Drive — оси, вдоль которых происходят измерения и возбуждение колебаний.
Такая конструкция обеспечивает высокую точность измерений и минимизацию перекрестных помех между осями.
В качестве простой аналогии, которая поясняет принцип работы регистратора угловой скорости внутри микроэлектромеханической системы, можно привести такой пример.
Представьте себе две небольшие тонкие металлические пластины, подвешенные на ниточках внутри маленького прозрачного аквариума, наполненного густым сиропом (это придаёт плавности движениям пластин, имитируя эффект пружины). Эти пластины висят очень близко друг к другу, но не касаются, образуя некое подобие объёмного конденсатора. Этот объёмный конденсатор подключен к высокочувствительной измерительной системе, которая с невероятной точностью регистрирует изменение электрической ёмкости.
Когда вся описанная система находится в состоянии покоя (неподвижна), пластины висят ровно друг напротив друга, расстояние между ними одинаково, и, соответственно, ёмкость остаётся стабильной — электроника показывает базовое значение. Но как только аквариум с сиропом начинают вращать, внутри возникает эффект Кориолиса, который заставляет две пластины слегка смещаться в сторону. При отклонении изменяется расстояние между ними изменяется и, соответственно, изменяется ёмкость конденсатора. Эта перемена фиксируется электроникой, которая переводит её в данные о скорости или угле вращения.
В реальном же MEMS-сенсоре вместо аквариума — герметичная микроскопическая камера, а вместо сиропа — воздух или вакуум. Вместо металлических пластин — крошечные кремниевые структуры, которые двигаются и изменяют ёмкость. Благодаря такой конструкции электроника может с высокой точностью определить, как именно вращается устройство, используя изменения ёмкости, аналогичные колебаниям нашего миниатюрного объёмного конденсатора в сиропе.
Параметры и характеристики MPU-6050
* Чувствительность гироскопа
Данный параметр влияет на точность определения угловой скорости ω (°/с — градусы в секунду), для расчёта которой используется следующая формула:
ω = Sensitivity × Raw_value,
где Raw_value — сырые данные (числовые отсчёты, для биполярных измерений при 16 бит это -32768 … +32768);
Sensitivity — чувствительность для выбранного диапазона.
Чувствительность представляет собой количество LSB (Least Significant Bit — младший значащий бит) на градус в секунду. Допустим, при диапазоне ±500 °/с мы получили значение 1310 LSB. Соответственно, угловая скорость будет равна:
ω = 65,5 × 1310 = 20 °/с.
Разрешение — минимальное изменение измеряемой величины, которое может зафиксировать датчик. То есть, например, при диапазоне ±250 °/с один градус в секунду соответствует 131 единице на выходе сенсора.
** Чувствительность акселерометра
Указанный параметр определяет реальное ускорение сенсора. Для расчёта ускорения применяется следующая формула:
α = Sensitivity × Raw_value,
где α — ускорение в единице измерения g;
Sensitivity — чувствительность для выбранного диапазона;
Raw_value — сырые данные.
К примеру, при диапазоне ±4g чувствительность составляет половину от базовой (1,024 LSB/g). Если получено значение 1024 LSB, то ускорение будет равно:
a = 1,024 × 1024 = 1 g.
Единица g — это единица измерения ускорения, равная ускорению свободного падения на поверхности Земли. Это ускорение, которое получает тело при свободном падении под действием силы тяжести:
1 g = 9,80665 м/с² (метров в секунду за секунду)
MPU-6050 в составе платы (модуля) GY-521
Для экспериментов часто применяют популярную отладочную плату GY-521, на которой распаяна микросхема датчика со всей необходимой обвязкой:
Данная плата содержит всю необходимую обвязку для работы с микросхемой сенсора, включая понижающий (с низким падением напряжения) линейный стабилизатор напряжения на 3,3 В. Однако, для полноценной работы модуля можно подавать на вход VCC и напряжение +3,3 В.
Ниже представлены таблица распиновки и принципиальная схема платы модуля GY-521:
В документации к микросхеме (datasheet) представлено изображение, которое иллюстрирует как соотносятся направления осей и корпус самой микросхемы:
Для использования модуля 6050 в качестве угломера нужно учитывать, как оси датчика соотносятся с его корпусом.
Ориентация осей:
— X направлена слева направо, как читается надписи на корпусе микросхемы. Другими словами: в правую сторону, если смотреть на корпус сверху, а первый пин расположен в верхнем левом углу корпуса;
— Y перпендикулярна оси X. Если смотреть на корпус сверху, то направление оси Y направлена вверх относительно первого пина микросхемы в верхнем левом углу;
— Z направлена перпендикулярно плоскости корпуса вверх.
Именно в таком положении, когда корпус микросхемы сенсора расположен параллельно плоскости земли, регистрируются.
Схема подключения MPU-6050 к ESP32
Для построения угломера воспользуемся отладочной платой NodeMCU-32S (38 pin), которая построена на базе модуля ESP-WROOM-32. Для отображения отклонение уровня от ноля (наклон) в градусах применим OLED-дисплей на базе контроллера SSD1306. Интерфейс подключения для обоих модулей — I2C. Ниже представлена таблица подключений сенсора и дисплея к отладочной плате.
Обратите внимание на позиционирование модуля GY-521, представленного на фотография экспериментального макета: плата расположена вертикально так, что ось X направлена вверх. При этом Z будет направлена уже не перпендикулярно плоскости земли, а параллельно. Это значит, что в исходном состоянии показания осей X и Z будут смещены на 90 градусов. Конкретно для оси X исходным положением с нулевым углом будет именно вертикальное позиционирование, что учтено в программе.
Программный код (скетч)
Разработка прошивки выполнялась в среде программирования Arduino IDE. Чтобы программировать ESP32, для среды необходимы дополнительные настройки. Подробная инструкция приведена в статье про микрофон INMP441.
Для работы с OLED-дисплеем воспользуемся популярной библиотекой Adafruit_SSD1306 (дополнительно можно подключить Adafruit_GFX для отображения графики).
Что касается работы с датчиком, то применим популярную и простую библиотеку Adafruit_MPU6050. Она обеспечивает оптимальный набор функций и достаточно проста для понимания новичкам. Также для успешной компиляции может понадобиться библиотека Adafruit_Sensor. Ниже представлен очень подробно прокомментированный листинг кода:
// ==================== ПОДКЛЮЧЕНИЕ БИБЛИОТЕК ====================
#include <Wire.h> // Библиотека для I2C коммуникации
#include <Adafruit_GFX.h> // Графическая библиотека для дисплеев
#include <Adafruit_SSD1306.h> // Специфичная библиотека для OLED дисплеев SSD1306
#include <Adafruit_MPU6050.h> // Библиотека для работы с датчиком MPU-6050
#include <Adafruit_Sensor.h> // Общая библиотека для работы с датчиками Adafruit
// ==================== НАСТРОЙКИ ДИСПЛЕЯ ====================
#define SCREEN_WIDTH 128 // Ширина дисплея в пикселях (128 для большинства OLED)
#define SCREEN_HEIGHT 64 // Высота дисплея в пикселях (64 для большинства OLED)
#define OLED_RESET -1 // Номер пина сброса (-1 означает, что пин не используется)
// Создание объекта дисплея с указанными параметрами
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// ==================== СОЗДАНИЕ ОБЪЕКТА ДАТЧИКА ====================
Adafruit_MPU6050 mpu; // Создание объекта для работы с MPU-6050
// ==================== ОПТИМИЗИРОВАННЫЕ ПАРАМЕТРЫ ====================
#define UPDATE_RATE 500 // Желаемая частота обновления данных: 500 Гц (500 раз в секунду)
#define SAMPLE_INTERVAL 2000 // Интервал между измерениями в микросекундах: 2000 мкс = 500 Гц
// Параметры комплементарного фильтра:
float alpha = 0.98; // Коэффициент фильтра: 0.98 = 98% гироскоп, 2% акселерометр
float dt = 1.0 / UPDATE_RATE; // Фиксированный временной интервал для вычислений
// ==================== ПЕРЕМЕННЫЕ ДЛЯ ВЫСОКОСКОРОСТНОЙ РАБОТЫ ====================
unsigned long lastMicros = 0; // Время последнего измерения в микросекундах (точное время)
unsigned long lastDisplayUpdate = 0; // Время последнего обновления дисплея в миллисекундах
const int DISPLAY_UPDATE_INTERVAL = 50; // Интервал обновления дисплея: 50 мс = 20 Гц
// ==================== ПЕРЕМЕННЫЕ ДЛЯ ФИЛЬТРАЦИИ ДАННЫХ ====================
float roll = 0, pitch = 0; // Финальные углы после комплементарного фильтра
float gyroRoll = 0, gyroPitch = 0; // Углы, рассчитанные интегрированием данных гироскопа
float accelRoll = 0, accelPitch = 0; // Углы, рассчитанные из данных акселерометра
// ==================== СТРУКТУРА ДЛЯ КАЛИБРОВОЧНЫХ ДАННЫХ ====================
struct CalibrationData {
float gyroX, gyroY, gyroZ; // Смещения нуля для гироскопа по осям X, Y, Z
float accelX, accelY, accelZ; // Смещения для акселерометра по осям X, Y, Z
};
CalibrationData calData; // Создание экземпляра структуры для хранения калибровочных данных
const int calibrationSamples = 1000; // Количество измерений для калибровки: 1000 samples
// ==================== КОНФИГУРАЦИЯ ОРИЕНТАЦИИ ДАТЧИКА ====================
#define SENSOR_VERTICAL true // true: датчик установлен вертикально, false: горизонтально
// ==================== БУФЕРЫ ДЛЯ БЫСТРОГО ВЫВОДА НА ДИСПЛЕЙ ====================
char pitchBuffer[10]; // Буфер для хранения форматированного значения угла pitch
char rollBuffer[10]; // Буфер для хранения форматированного значения угла roll
// ==================== ФУНКЦИЯ НАСТРОЙКИ (ВЫПОЛНЯЕТСЯ ОДИН РАЗ ПРИ ЗАПУСКЕ) ====================
void setup() {
// Инициализация последовательного порта для отладки (115200 бод)
Serial.begin(115200);
// УСКОРЕННАЯ ИНИЦИАЛИЗАЦИЯ I2C ШИНЫ
Wire.begin(); // Инициализация I2C шины
Wire.setClock(400000); // Установка высокой скорости I2C: 400 кГц (стандарт: 100 кГц)
// БЫСТРАЯ ИНИЦИАЛИЗАЦИЯ ДИСПЛЕЯ
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
// Если дисплей не инициализировался, войти в бесконечный цикл
while(1);
}
// Настройка параметров отображения
display.clearDisplay(); // Очистка дисплея
display.setTextSize(1); // Установка размера текста (1 = 6x8 пикселей)
display.setTextColor(SSD1306_WHITE); // Установка цвета текста (белый на черном)
display.setCursor(0, 0); // Установка позиции курсора в начало
display.println("Fast Init..."); // Вывод сообщения о инициализации
display.display(); // Обновление дисплея (вывод на экран)
// УСКОРЕННАЯ ИНИЦИАЛИЗАЦИЯ ДАТЧИКА MPU-6050
if (!mpu.begin()) {
// Если датчик не найден, вывести сообщение об ошибке
display.clearDisplay();
display.setCursor(0, 0);
display.println("MPU6050 Error!");
display.display();
while (1); // Остановка программы при ошибке
}
// ОПТИМИЗИРОВАННЫЕ НАСТРОЙКИ ДАТЧИКА ДЛЯ ВЫСОКОЙ СКОРОСТИ
mpu.setAccelerometerRange(MPU6050_RANGE_4_G); // Диапазон акселерометра: ±4g (больше разрешение)
mpu.setGyroRange(MPU6050_RANGE_500_DEG); // Диапазон гироскопа: ±500°/с (лучшее разрешение)
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); // Полоса пропускания фильтра: 21 Гц (быстрее отклик)
// БЫСТРАЯ КАЛИБРОВКА ДАТЧИКОВ
fastCalibrateSensors();
// ИНИЦИАЛИЗАЦИЯ ПЕРЕМЕННЫХ ВРЕМЕНИ
lastMicros = micros(); // Запоминаем текущее время в микросекундах
lastDisplayUpdate = millis(); // Запоминаем текущее время в миллисекундах
}
// ==================== ОСНОВНОЙ ЦИКЛ ПРОГРАММЫ (ВЫПОЛНЯЕТСЯ БЕСКОНЕЧНО) ====================
void loop() {
unsigned long currentMicros = micros(); // Текущее время в микросекундах
// ВЫСОКОЧАСТОТНОЕ ОБНОВЛЕНИЕ ДАННЫХ ДАТЧИКА (500 Гц)
if (currentMicros - lastMicros >= SAMPLE_INTERVAL) {
lastMicros = currentMicros; // Обновляем время последнего измерения
// ЧТЕНИЕ ДАННЫХ С ДАТЧИКА С МИНИМАЛЬНЫМИ ЗАДЕРЖКАМИ
sensors_event_t a, g, temp; // Структуры для данных акселерометра, гироскопа и температуры
mpu.getEvent(&a, &g, &temp); // Получение данных от датчика
// БЫСТРАЯ ОБРАБОТКА ПОЛУЧЕННЫХ ДАННЫХ
processSensorData(a, g);
}
// НИЗКОЧАСТОТНОЕ ОБНОВЛЕНИЕ ДИСПЛЕЯ (20 Гц) - ЧТОБЫ НЕ ЗАМЕДЛЯТЬ ОСНОВНОЙ ЦИКЛ
unsigned long currentMillis = millis(); // Текущее время в миллисекундах
if (currentMillis - lastDisplayUpdate >= DISPLAY_UPDATE_INTERVAL) {
lastDisplayUpdate = currentMillis; // Обновляем время последнего обновления дисплея
updateDisplay(roll, pitch); // Обновляем показания на дисплее
}
}
// ==================== БЫСТРАЯ ОБРАБОТКА ДАННЫХ ДАТЧИКА ====================
void processSensorData(sensors_event_t &a, sensors_event_t &g) {
// ПРИМЕНЕНИЕ КАЛИБРОВОЧНЫХ СМЕЩЕНИЙ К СЫРЫМ ДАННЫМ
float accX = a.acceleration.x - calData.accelX; // Акселерометр X с коррекцией
float accY = a.acceleration.y - calData.accelY; // Акселерометр Y с коррекцией
float accZ = a.acceleration.z - calData.accelZ; // Акселерометр Z с коррекцией
float gyroX = g.gyro.x - calData.gyroX; // Гироскоп X с коррекцией дрейфа
float gyroY = g.gyro.y - calData.gyroY; // Гироскоп Y с коррекцией дрейфа
float gyroZ = g.gyro.z - calData.gyroZ; // Гироскоп Z с коррекцией дрейфа
// БЫСТРОЕ ВЫЧИСЛЕНИЕ УГЛОВ ОТ АКСЕЛЕРОМЕТРА
if (SENSOR_VERTICAL) {
// Для вертикального датчика используем оптимизированные вычисления
float sqrtYZ = sqrt(accY * accY + accZ * accZ); // Вычисляем √(Y² + Z²) один раз
float sqrtXZ = sqrt(accX * accX + accZ * accZ); // Вычисляем √(X² + Z²) один раз
// Вычисление угла roll (крен) через арктангенс
accelRoll = atan2(-accX, sqrtYZ) * 57.2958f; // 57.2958 = 180/π (предвычислено для более быстрой работы программы)
// Вычисление угла pitch (тангаж) через арктангенс
accelPitch = atan2(accY, sqrtXZ) * 57.2958f;
}
// ИНТЕГРИРОВАНИЕ ДАННЫХ ГИРОСКОПА ДЛЯ ПОЛУЧЕНИЯ УГЛОВ
gyroRoll += gyroY * dt * 57.2958f; // Интегрируем угловую скорость вокруг оси Y → roll
gyroPitch += gyroX * dt * 57.2958f; // Интегрируем угловую скорость вокруг оси X → pitch
// КОМПЛЕМЕНТАРНЫЙ ФИЛЬТР - ОБЪЕДИНЕНИЕ ДАННЫХ АКСЕЛЕРОМЕТРА И ГИРОСКОПА
// Формула: angle = α*(angle + gyro*dt) + (1-α)*accelAngle
roll = alpha * (roll + gyroY * dt * 57.2958f) + (1 - alpha) * accelRoll;
pitch = alpha * (pitch + gyroX * dt * 57.2958f) + (1 - alpha) * accelPitch;
// ОГРАНИЧЕНИЕ УГЛОВ В ДИАПАЗОНЕ -180° ДО 180°
roll = constrainAngleFast(roll);
pitch = constrainAngleFast(pitch);
}
// ==================== ОПТИМИЗИРОВАННАЯ ФУНКЦИЯ ОГРАНИЧЕНИЯ УГЛА ====================
float constrainAngleFast(float angle) {
// Преобразуем угол в диапазон 0-360°: angle + 180, затем берем modulo 360
angle = fmod(angle + 180.0f, 360.0f);
// Корректируем отрицательные значения (fmod может возвращать отрицательные)
if (angle < 0) angle += 360.0f;
// Возвращаем в диапазон -180° до 180°
return angle - 180.0f;
}
// ==================== БЫСТРАЯ КАЛИБРОВКА ДАТЧИКОВ ====================
void fastCalibrateSensors() {
// Вывод инструкции для пользователя
display.clearDisplay();
display.setCursor(30, 0);
display.println("CALIBRATION");
display.setCursor(30, 20);
display.println("Keep device");
display.setCursor(20, 35);
display.println("flat and still!");
display.display();
delay(1000); // Пауза 1 секунда для чтения инструкции
// Переменные для накопления сумм измерений
float sumAccX = 0, sumAccY = 0, sumAccZ = 0;
float sumGyroX = 0, sumGyroY = 0, sumGyroZ = 0;
// БЫСТРАЯ КАЛИБРОВКА БЕЗ ЧАСТОГО ОБНОВЛЕНИЯ ДИСПЛЕЯ
for (int i = 0; i < calibrationSamples; i++) {
// Чтение данных с датчика
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
// Накопление сумм для акселерометра
sumAccX += a.acceleration.x;
sumAccY += a.acceleration.y;
sumAccZ += a.acceleration.z;
// Накопление сумм для гироскопа
sumGyroX += g.gyro.x;
sumGyroY += g.gyro.y;
sumGyroZ += g.gyro.z;
delayMicroseconds(500); // Минимальная задержка между измерениями
}
// ВЫЧИСЛЕНИЕ СРЕДНИХ ЗНАЧЕНИЙ (КАЛИБРОВОЧНЫХ СМЕЩЕНИЙ)
calData.accelX = sumAccX / calibrationSamples; // Смещение акселерометра по X
calData.accelY = sumAccY / calibrationSamples; // Смещение акселерометра по Y
calData.accelZ = sumAccZ / calibrationSamples; // Смещение акселерометра по Z
calData.gyroX = sumGyroX / calibrationSamples; // Дрейф гироскопа по X
calData.gyroY = sumGyroY / calibrationSamples; // Дрейф гироскопа по Y
calData.gyroZ = sumGyroZ / calibrationSamples; // Дрейф гироскопа по Z
// КОРРЕКЦИЯ ДЛЯ ВЕРТИКАЛЬНОГО ДАТЧИКА (УЧЕТ СИЛЫ ТЯЖЕСТИ)
if (SENSOR_VERTICAL) {
// Определяем, какая ось ближе к вертикали (имеет наибольшее ускорение)
if (fabs(calData.accelX) > fabs(calData.accelY)) {
// Ось X ближе к вертикали - корректируем с учетом гравитации (9.81 м/с²)
calData.accelX -= 9.81f * (calData.accelX > 0 ? 1.0f : -1.0f);
} else {
// Ось Y ближе к вертикали
calData.accelY -= 9.81f * (calData.accelY > 0 ? 1.0f : -1.0f);
}
}
}
// ==================== ОПТИМИЗИРОВАННОЕ ОБНОВЛЕНИЕ ДИСПЛЕЯ ====================
void updateDisplay(float roll, float pitch) {
display.clearDisplay(); // Очистка дисплея
// ПРЕДВАРИТЕЛЬНОЕ ФОРМАТИРОВАНИЕ СТРОК ДЛЯ БЫСТРОГО ВЫВОДА
dtostrf(pitch, 5, 1, pitchBuffer); // Преобразование числа в строку: 5 символов, 1 десятичный знак
dtostrf(roll, 5, 1, rollBuffer); // Преобразование числа в строку: 5 символов, 1 десятичный знак
// БЫСТРЫЙ ВЫВОД ТЕКСТА НА ДИСПЛЕЙ
display.setTextSize(2); // Установка крупного размера текста для углов
// Вывод угла Pitch (тангаж - наклон вперед/назад)
display.setCursor(0, 5); // Позиция: X=0, Y=5 пикселей
display.print("X:"); // Метка оси
display.print(pitchBuffer); // Значение угла
display.print((char)247); // Символ градуса °
// Вывод угла Roll (крен - наклон влево/вправо)
display.setCursor(0, 25); // Позиция: X=0, Y=25 пикселей
display.print("Y:"); // Метка оси
display.print(rollBuffer); // Значение угла
display.print((char)247); // Символ градуса °
// ГРАФИЧЕСКАЯ ИНДИКАЦИЯ УГЛОВ (ПОЛОСА ПРОКРУТКИ)
// Преобразование угла pitch в позицию на дисплее (от -90° до +90° → от 5 до 123 пикселей)
int pitchPos = map(constrain(pitch, -90, 90), -90, 90, 5, SCREEN_WIDTH - 5);
// Рисование горизонтальной оси
display.drawLine(5, 55, SCREEN_WIDTH - 5, 55, SSD1306_WHITE);
// Рисование вертикальной метки в центре (ноль)
display.drawLine(SCREEN_WIDTH/2, 50, SCREEN_WIDTH/2, 60, SSD1306_WHITE);
// Рисование бегунка (кружок в текущей позиции)
display.fillCircle(pitchPos, 55, 3, SSD1306_WHITE);
// ВЫВОД МЕТОК ШКАЛЫ МЕЛКИМ ШРИФТОМ
display.setTextSize(1); // Установка мелкого размера текста для меток
display.setCursor(SCREEN_WIDTH/2 - 5, 45); // Позиция для метки "0"
display.print("0");
display.setCursor(5, 45); // Позиция для метки "-90"
display.print("-90");
display.setCursor(SCREEN_WIDTH - 20, 45); // Позиция для метки "90"
display.print("90");
display.display(); // Обновление дисплея (вывод всего нарисованного)
}Эксперимент
Итак, кладём макетную плату на горизонтальную поверхность (стол) и подаём питание на контроллер. После калибровки мы видим, что по оси X у нас примерно 0:
Протестируем прибор, измерив наклон по оси X. Разумеется, школьный транспортир в качестве “эталонного измерительного канала” использовать не совсем корректно. Но для общего понимания того, что прибор показывает близкие к реальности адекватные значения, нам будет достаточно:
— 27 градусов
— 52 градуса
Алгоритм работы
Общий алгоритм работы программы следующий:
1) инициализация: настройка оборудования (I2C, дисплей, сенсор);
2) калибровка: замер и сохранение «нулевых» показаний датчика для компенсации погрешностей.
3) бесконечный цикл (основная работа):
— высокочастотный опрос сенсора (500 Гц);
— обработка данных: применение калибровки, расчет углов по акселерометру, интегрирование гироскопа и объединение данных комплементарным фильтром;
— данные выводятся на дисплей.
Каждый рез перед тем, как подать питание или выполнить перезагрузку, устройство необходимо положить на ровную горизонтальную поверхность, чтобы произвести калибровку. Этот процесс необходим, чтобы система оценила уровень смещения показаний по каждой из осей, и значение смещения будет вычитаться для компенсации погрешностей при получении данных с датчика.
Калибровка описывается в функции fastCalibrateSensors. Датчик должен лежать неподвижно на ровной поверхности. Программа считывает 1000 измерений.
Для гироскопа рассчитывается среднее значение смещения нуля (дрейф). Если сенсор неподвижен, то он должен показывать 0 °/с. Но на практике он показывает небольшие значения. Эти значения запоминаются как calData.gyroX, Y, Z и в дальнейшем вычитаются из каждого нового показания.
Для акселерометра рассчитывается смещение нуля. Когда микросхема сенсора лежит ровно (горизонтально, параллельно плоскости земли), ось Z должна показывать ~9.81 м/с² (сила тяжести), а X и Y ~0. Если это не так, вычисляются поправки. В нашем случае плата GY-521 с сенсором установлена вертикально, поэтому дополнительно корректируется учет силы тяжести.
В рамках бесконечного главного цикла loop программа работает с двумя разными частотами для максимальной эффективности:
— с высокой частотой (500 Гц) выполняется обработка данных;
— с низкой частотой (20 Гц) производится обновление дисплея. Редко обновляемый дисплей не занимает временные ресурсы для выполнения быстрых вычислений.
Ядром программы является обработка данных сенсора, описанная в функции processSensorData. Из сырых данных вычитаются калибровочные смещения:
float accX = a.acceleration.x - calData.accelX;
float gyroX = g.gyro.x - calData.gyroX;Для расчёта угла по оси X (roll, крен, наклон по оси X) по данным акселерометра используется функция арктангенса atan2, которая возвращает угол (в радианах) между положительным направлением оси X и точкой (x, y) на плоскости:
accelRoll = atan2(-accX, sqrt(accY * accY + accZ * accZ)) * (180 / π).Здесь мы смотрим на проекцию вектора ускорения на плоскость Y-Z. Когда сенсора наклонен, сила тяжести распределяется между осями Y и Z. Функция atan2 вычисляет угол этого наклона.
Отношение 180/π можно представить как 57,2958 (чтобы сэкономить ресурсы контроллера на расчёты). Оно переводит радианы в градусы.
Расчёт угла по оси Y (pitch, тангаж, наклон вокруг оси Y)
accelPitch = atan2(accY, sqrt(accX * accX + accZ * accZ)) * (180 / π).Чтобы выполнить расчет углов по гироскопу, используется следующая формула:
Новый_угол = Старый_угол + (Угловая_скорость × Время);
gyroRoll += gyroY × dt × 57.2958fгде gyroY — это как быстро происходит вращения вокруг оси Y (в радианах/секунду).
Умножение на время dt (0,002 секунды для 500 Гц) дает изменение угла в радианах.
Умножение на 57,2958 переводит в градусы (180/π).
Ошибка в gyroY накапливается (интегрируется) с каждым шагом, вызывая дрейф. Поэтому необходимо использовать комплементарный фильтр:
roll = alpha * (roll + gyroY * dt * 57.2958f) + (1 - alpha) * accelRoll;
pitch = alpha * (pitch + gyroX * dt * 57.2958f) + (1 - alpha) * accelPitch;где roll и pitch — это предыдущие значения углов после фильтрации (состояние фильтра);
gyroY и gyroX — угловые скорости с гироскопа по соответствующим осям;
dt — время между измерениями;
57.2958f — перевод из радиан в градусы (180/π);
accelRoll и accelPitch — углы, вычисленные по акселерометру.
Таким образом, к текущему углу roll добавляется приращение от гироскопа:
gyroY * dt * 57.2958f — это интегрирование скорости в угол.
Затем результат умножается на alpha (0.98), чтобы сохранить большую часть информации. Добавляется небольшая часть (2%) измеренной величины от акселерометра (1 – alpha) * accelRoll, чтобы компенсировать дрейф регистратора угловой скорости.
После фильтрации осуществляется нормализация значений при помощи функции constrainAngleFast, которая гарантирует, что угол остается в диапазоне от -180° до 180°, что удобно для отображения.
Функция updateDisplay выводит на дисплей рассчитанные углы по осям X и Y вместе с графической шкалой, которая наглядно показывает текущий уровень наклона.
Заключение
В данной статье мы подробно рассмотрели устройство, принцип работы и возможности комбинированного цифрового датчика MPU-6050 — трёхосевого акселерометра и гироскопа, объединённых в одной микросхеме. Этот инерциальный измерительный блок открывает широкие перспективы для создания систем контроля движения: от стабилизации роботов и камер до управления жестами и анализа физических активностей.
В качестве примера мы на базе отладочной платы ESP32 построили простой, но достаточно точный угломер. Подробно рассмотренный алгоритм работы легко адаптируется под различные задачи, что делает рассмотренный датчик отличным выбором для начинающих и опытных разработчиков, желающих создавать интерактивные и интеллектуальные устройства.