# 🔬 Профессиональная теория: MPU6050 и Инерциальная Навигация (v2026)

## 1. Архитектура MEMS-сенсора
MPU6050 — это шестиосевой измерительный модуль (6-DOF IMU), объединяющий трехосевой акселерометр и трехосевой гироскоп на одном кремниевом кристалле.

- **MEMS (Micro-Electro-Mechanical Systems):** Внутри чипа находятся микроскопические кремниевые подрессоренные структуры (консоли). Под действием сил инерции они деформируются, изменяя расстояние между обкладками дифференциальных измерительных конденсаторов. Изменение емкости преобразуется встроенным АЦП в цифровой код.
- **DLPF (Digital Low Pass Filter):** Встроенный аппаратный фильтр нижних частот 2-го порядка. Он осуществляет предварительную аналого-цифровую фильтрацию сигналов, отсекая высокочастотные механические вибрации (например, гул бесколлекторных двигателей робота) еще до того, как данные попадут в регистры I2C. В нашей реализации DLPF настроен на полосу ~42 Гц, что является золотым стандартом для стабилизации робототехники.

## 2. Физическая математика и калибровка

### Преобразование и Чувствительность
Для перевода сырых 16-битных данных из регистров датчика ($LSB$) в физические величины используются коэффициенты масштабирования (Sensitivity Scale Factor):
- **Акселерометр (диапазон $\pm2g$):** Масштаб составляет $16384\,\text{LSB}/g$. Формула преобразования:  
  $$ a = \frac{\text{Raw}_{\text{acc}}}{16384} $$
- **Гироскоп (диапазон $\pm250^\circ/\text{s}$):** Масштаб составляет $131\,\text{LSB}/(^\circ/\text{s})$. Формула преобразования с учетом статического смещения нуля:  
  $$ \omega = \frac{\text{Raw}_{\text{gyro}} - \text{Offset}_{\text{gyro}}}{131} $$

### Проблема дрейфа (Bias Drift) и интегрирование
Гироскоп измеряет угловую скорость, а не абсолютный угол. Для вычисления текущего угла наклона применяется численное интегрирование угловой скорости по времени методом прямоугольников:
$$ \theta_{\text{gyro}}(t) = \theta_{\text{gyro}}(t-\Delta t) + \omega \cdot \Delta t $$
Поскольку сырые данные гироскопа всегда содержат белый шум и случайное смещение нуля (Bias), в процессе интегрирования эта микроскопическая ошибка накапливается с каждым шагом времени $\Delta t$. Без внешней коррекции это приводит к необратимому линейному дрейфу — виртуальный горизонт «уплывает» даже у неподвижного датчика. Поэтому **высокоточная калибровка офсетов при старте системы строго обязательна**.

## 3. Алгоритмы слияния сенсоров (Sensor Fusion)

### Почему нельзя доверять одному датчику?
1. **Акселерометр (Низкочастотный фильтр ориентации):** Идеален в статических условиях, так как всегда указывает точное направление вектора истинной гравитации Земли ($g$). Однако в динамике (при толчках, линейных ускорениях робота) акселерометр фиксирует сумму вектора гравитации и вектора динамического ускорения, генерируя сильнейший высокочастотный шум.
2. **Гироскоп (Высокочастотный фильтр ориентации):** Идеален в быстрой динамике, так как мгновенно реагирует на вращение без задержек и нечувствителен к линейным ударам. Но он абсолютно бесполезен на длинных временных интервалах из-за накопления ошибки интеграции (дрейфа).

### Комплементарный фильтр (Complementary Filter)
Комплементарный фильтр осуществляет частотное разделение сигналов. Он объединяет кратковременную точность гироскопа и долговременную стабильность акселерометра, являясь математически легкой заменой фильтра Калмана:
$$ \theta_{\text{fusion}} = \alpha \cdot (\theta_{\text{old}} + \omega \cdot \Delta t) + (1 - \alpha) \cdot \theta_{\text{acc}} $$
Где $\alpha$ — весовой коэффициент фильтра (по умолчанию $0.96$).
- **Постоянная времени фильтра ($\tau$):** Математически определяет временную границу, на которой происходит передача «доверия» от одного датчика к другому. Рассчитывается как:  
  $$ \tau = \frac{\alpha \cdot \Delta t}{1 - \alpha} $$
  При $\alpha = 0.96$ и частоте квантования FreeRTOS в $200\,\text{Гц}$ ($\Delta t = 0.005\,\text{с}$), постоянная времени $\tau \approx 0.12\,\text{секунды}$. Это значит, что на интервалах времени менее 0.12 с фильтр полностью доверяет гироскопу, сглаживая удары, а на интервалах более 0.12 с — опирается на стабильный вектор гравитации акселерометра.

## 4. Математика ориентации (Pitch, Roll, Yaw)

### Расчет углов наклона (Эйлеровы углы)
Определение пространственного положения выполняется через тригонометрические функции на основе проекций вектора ускорения свободного падения на связанные оси датчика:
$$ \text{Roll (Крен)} = \operatorname{atan2}(a_y, a_z) \cdot \frac{180}{\pi} $$
$$ \text{Pitch (Тангаж)} = \operatorname{atan2}(-a_x, \sqrt{a_y^2 + a_z^2}) \cdot \frac{180}{\pi} $$
*Важно:* Для защиты от деления на ноль в сингулярных точках (когда датчик направлен строго вертикально), в подкоренное выражение знаменателя введен программный ограничитель (эпсилон-порог `0.001f`).

### Расчет угла курса (Yaw)
Поскольку акселерометр не способен зафиксировать вращение вокруг вертикальной оси вектора гравитации (Z), угол **Yaw** рассчитывается исключительно путем чистого интегрирования угловой скорости:
$$ \text{Yaw}(t) = \text{Yaw}(t-\Delta t) + \omega_z \cdot \Delta t $$
*Примечание:* Из-за отсутствия в MPU6050 встроенного магнитометра (компаса), угол Yaw не имеет абсолютной привязки к сторонам света и подвержен медленному накоплению дрейфа. Однако он идеально отражает относительные повороты устройства в динамике.

### Модуль полной перегрузки (TotalG)
Для инвариантного анализа внешних механических воздействий и ударов рассчитывается норма (длина) трехмерного вектора ускорения:
$$ \text{TotalG} = \sqrt{a_x^2 + a_y^2 + a_z^2} $$
В состоянии покоя $\text{TotalG} \equiv 1.00\,g$. Любое отклонение от единицы интерпретируется верхним софтом как наличие внешних динамических сил (удары, вибрация, свободное падение).

### Ограничение модели: Шарнирный замок (Gimbal Lock)
Данная реализация использует классические углы Эйлера (последовательные повороты). Главное ограничение этой математической модели — возникновение «шарнирного замка» (Gimbal Lock) при достижении угла Pitch значения $\pm90^\circ$. В этой точке оси Крена и Рыскания сливаются, система теряет одну степень свободы, а тригонометрические уравнения уходят в сингулярность. Для построения систем со свободным вращением на $360^\circ$ (например, пилотажные беспилотники) требуется глубокая модернизация математического ядра и переход на пространственные **кватернионы Hamilton**.

## 5. Особенности реализации в экосистеме Sensor Hub

- **Асинхронная двухъядерная архитектура:** Код адаптирован под ESP32 и FreeRTOS. Тяжелые системные вызовы вывода данных в порт (`Serial.print`), занимающие до нескольких миллисекунд процессорного времени, физически изолированы на **Ядре 1**. Опрос регистров I2C и математика слияния жестко зафиксированы на **Ядре 0**.
- **Детерминированный шаг времени ($\Delta t$):** Комплементарный фильтр работает не по плавающему времени `micros()`, а внутри тайм-слота FreeRTOS с фиксированным периодом $\Delta t = 0.005\,\text{с}$. Это гарантирует идеальную линейность интегрирования угловых скоростей и устраняет вычислительный шум (джиттер времени).
- **Межъядерная потокобезопасность:** Передача вычисленной структуры данных между ядрами защищена легковесной критической секцией на уровне аппаратных мьютексов процессора (`portENTER_CRITICAL`), что предотвращает чтение разорванных («битых») байт между потоками.