AHRS на MPU6050 + Python

Круглый авиагоризонт, 3D-визуализация, фильтры Madgwick / Mahony, загрузка OBJ/STL моделей

Что такое AHRS?

Как это работает?

Система AHRS решает ключевую проблему инерциальных датчиков:

• Гироскоп — измеряет угловую скорость, но со временем накапливает дрейф (ошибка интегрирования).
• Акселерометр — показывает направление гравитации (даёт абсолютный опорный вектор «вниз»), но чувствителен к ускорениям и вибрациям.
• Фильтр (Madgwick или Mahony) — объединяет эти данные, используя алгоритмы слияния с перекрёстной коррекцией ошибок, и выдаёт стабильный кватернион — математическое представление ориентации без «складок» и сингулярностей.

В нашем проекте: фильтр работает прямо на ESP32, передавая в Python уже готовые кватернионы и углы. Это разгружает компьютер и обеспечивает высокую частоту обновления (до 200 Гц).

Описание интерфейса

Компоненты и стек

Библиотеки для Arduino IDE

В прошивке AHRS-avia.ino используются только встроенные библиотеки — дополнительная установка не требуется.

Библиотеки для Python

Все зависимости перечислены в таблице ниже. Установка выполняется одной командой (см. раздел «Установка»).

Установка и настройка

1️⃣ Прошивка ESP32 (Arduino)

1. Откройте AHRS-avia.ino в Arduino IDE.
2. Выберите плату ESP32S3 Dev Module (или вашу).
3. Убедитесь, что пины I²C соответствуют: #define I2C_SDA 8, #define I2C_SCL 9.
4. Загрузите прошивку. В мониторе порта появится MPU6050_READY.

2️⃣ Установка Python-библиотек

Способ 1 — через pip (рекомендуемый):

pip install vispy pyqt5 pyqtgraph pyserial numpy trimesh pywavefront

Способ 2 — через requirements.txt:

Создайте файл requirements.txt со следующим содержимым:

vispy>=0.14.0
    PyQt5>=5.15.0
    pyqtgraph>=0.13.0
    pyserial>=3.5
    numpy>=1.24.0
    trimesh>=4.0.0
    pywavefront>=1.3.0

Затем выполните:

pip install -r requirements.txt

3️⃣ Проверка зависимостей

Запустите Python и выполните импорт всех библиотек:

python -c "import vispy, PyQt5, pyqtgraph, serial, numpy, trimesh, pywavefront; print('✅ Все библиотеки установлены')"

Если какая-то библиотека отсутствует, вы увидите сообщение ModuleNotFoundError — установите её отдельно.

Также можно использовать скрипт-проверку:

import importlib
    libs = ['vispy', 'PyQt5', 'pyqtgraph', 'serial', 'numpy', 'trimesh', 'pywavefront']
    for lib in libs:
        try:
            importlib.import_module(lib)
            print(f'✅ {lib} — OK')
        except ImportError:
            print(f'❌ {lib} — НЕ УСТАНОВЛЕН')

4️⃣ Запуск Python-клиента

1. Сохраните скрипт AHRS-avia-round.py в рабочую папку.
2. Убедитесь, что все зависимости установлены (см. п. 2–3).
3. Запустите: python AHRS-avia-round.py
4. В интерфейсе выберите COM-порт и нажмите «Подключиться».

После подключения вы увидите:

• 3D-сцена с вращающейся моделью (куб, самолёт или ваша модель).
• Круглый авиагоризонт с дугой крена и шкалой тангажа.
• Графики гироскопа, акселерометра и углов в реальном времени.
• Панель управления фильтрами, калибровкой, темой оформления.

Ключевые возможности

• Два фильтра ориентации: Madgwick (параметры Beta/Zeta) и Mahony (Kp/Ki). Переключение в реальном времени.
• Калибровка гироскопа с анализом сходимости — автоматическое определение смещений и уровня шума.
• Загрузка 3D-моделей через интерфейс (OBJ / STL). Прозрачность регулируется слайдером.
• Темы оформления: Dark, Light, Aviation, Terminal — меняются мгновенно.
• Экспорт данных в CSV с возможностью непрерывной записи.
• Горячие клавиши: Ctrl+W (подключение), Ctrl+K (калибровка), Ctrl+E (экспорт), F11 (полный экран).

Предустановленные профили фильтров

В программе доступно 6 предустановленных профилей для быстрой настройки фильтрации под разные сценарии использования. Каждый профиль оптимизирован для конкретных условий работы датчика.

Расширенная настройка

Изменение пинов I²C (ESP32)

В файле AHRS-avia.ino измените строки:

#define I2C_SDA 8
    #define I2C_SCL 9

Настройка фильтров вручную

Помимо предустановленных профилей, вы можете задать индивидуальные параметры фильтрации через интерфейс:

• Madgwick: Beta (градиентный спуск, влияет на скорость сходимости) и Zeta (подавление дрейфа).
• Mahony: Kp (пропорциональный коэффициент, определяет скорость реакции) и Ki (интегральный коэффициент, влияет на подавление систематической ошибки).

Рекомендуемые диапазоны: Beta: 0.01–0.5, Kp: 0.1–3.0, Ki: 0.0–0.1.

Режимы производительности (на ESP32)

В прошивке доступны три режима, переключаемые по команде:

MODE_BALANCED     # 100 Гц, DLPF 44 Гц (баланс скорости и качества)
    MODE_HIGH_SPEED   # 200 Гц, без DLPF (максимальная частота обновления)
    MODE_HIGH_ACCURACY # 50 Гц, DLPF 5 Гц (минимальный шум)

Экспорт данных

Данные сохраняются в CSV с колонками: time, temp, gx, gy, gz, ax, ay, az, roll, pitch, yaw, q0, q1, q2, q3. Можно экспортировать текущий буфер или записывать непрерывно кнопкой «Начать запись».

Устранение неполадок

• Нет данных в Python: проверьте скорость порта (115200) и правильность пинов.
• Модель не загружается: установите trimesh (pip install trimesh).
• Авиагоризонт не обновляется: убедитесь, что подключение активно и приходят пакеты с углами.
• Сбой калибровки: положите датчик неподвижно и повторите.

Дальнейшее развитие

• Добавление поддержки магнитометра (HMC5883L) для абсолютного курса.
• Сохранение видео с 3D-сценой и авиагоризонтом.
• Поддержка нескольких датчиков (резервирование).
• Управление сервоприводами или дроном по данным AHRS.

Исходный код (фрагменты)

Arduino — обработка кватернионов (Mahony)

void MahonyFilter::update(float gx, float gy, float gz, float ax, float ay, float az, float dt) {
        // ... нормализация акселерометра, вычисление ошибки, 
        // интегральная и пропорциональная составляющие, 
        // интегрирование кватерниона, нормализация
    }

Python — круглый авиагоризонт (отрисовка)

def paintEvent(self, event):
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)
        # Создаём круглую область отсечения
        path = QPainterPath()
        path.addEllipse(center_x - radius, center_y - radius, size, size)
        painter.setClipPath(path)
        # Рисуем небо, землю, линию горизонта, шкалы, символ самолёта
        ...