ESP32 и телеметрия

ESP32, датчики и телеметрия

Цель занятия: обучающиеся собирают стенд с ESP32, снимают показания датчиков, фильтруют их и по телеметрии находят ошибку в алгоритме до вылета.

Прежде чем алгоритм окажется в воздухе, его поведение нужно увидеть. ESP32 опрашивает датчики и отдаёт телеметрию на стенд, где значения проверяются в реальном времени: правильно ли считывается ориентация, что происходит при вибрации, как ведут себя данные на границах диапазона.

Датчики аппарата

Датчик	Что измеряет	Проблема
Гироскоп	Угловую скорость, °/с	Дрейф: медленно «уплывает» со временем
Акселерометр	Ускорение, в т.ч. силу тяжести	Шум: любая вибрация винтов ломает показания
Барометр	Давление → высоту	Реагирует на ветер, потоки от винтов, открытую дверь
Магнитометр	Направление на магнитный север	Врёт рядом с моторами и силовыми проводами
GPS	Координаты	Не работает в помещении, точность единицы метров

Ни один датчик не даёт правды. Гироскоп точен мгновенно, но накапливает ошибку. Акселерометр не копит ошибку, но зашумлён. Это не дефект конкретных деталей, а фундаментальное свойство измерений — подробнее в разделе Робототехника → Сигналы и данные. Задача инженера не в поиске идеального датчика, а в грамотном объединении неидеальных.

Почему один датчик не даёт угол

Казалось бы, угол наклона можно получить двумя способами.

Через гироскоп — интегрируя угловую скорость:

\theta_t = \theta_{t-1} + \omega \cdot \Delta t

Мгновенно и гладко. Но каждый шаг добавляет крошечную ошибку, и за минуту угол «уплывает» на десятки градусов, даже если аппарат неподвижен.

Через акселерометр — по направлению силы тяжести:

\theta = \arctan\!\left(\frac{a_x}{a_z}\right)

Не уплывает никогда. Но при вибрации винтов показания скачут так, что угол прыгает на десятки градусов за такт.

Комплементарный фильтр

Объединяем сильные стороны: у гироскопа берём быстрое поведение, у акселерометра — долговременную правду.

\theta_t = \alpha\,(\theta_{t-1} + \omega\,\Delta t) + (1-\alpha)\,\theta_{\text{акс}}

const float ALPHA = 0.98f;      // доверие гироскопу на коротком интервале
float angle = 0.0f;

void updateAngle(float gyroRate, float accX, float accZ, float dt) {
    float accAngle = atan2f(accX, accZ) * 180.0f / PI;
    angle = ALPHA * (angle + gyroRate * dt) + (1.0f - ALPHA) * accAngle;
}

Смысл $\alpha = 0{,}98$ : на каждом такте угол на 98 % определяется гироскопом (быстро и гладко) и на 2 % подтягивается к показаниям акселерометра (медленно убирает дрейф). Шум акселерометра при этом ослабляется в 50 раз.

Аналогия. Гироскоп — это спидометр: точно говорит, как быстро вы едете, но чтобы узнать координату, показания надо складывать, и ошибка копится. Акселерометр — редкий взгляд на дорожный указатель: сам по себе неточен и случаен, но не даёт уехать в никуда. Комплементарный фильтр — привычка «еду по спидометру, но сверяюсь с указателями».

Заглянуть глубже: фильтр Калмана

Комплементарный фильтр — упрощённый частный случай фильтра Калмана, который делает то же самое, но вычисляет коэффициент доверия $\alpha$ динамически, исходя из оценки текущей неопределённости каждого источника.

Идея: фильтр хранит не только оценку величины, но и насколько он в ней уверен (ковариацию). На каждом шаге он предсказывает новое состояние по модели, затем корректирует предсказание измерением — причём вес коррекции тем больше, чем менее фильтр уверен в предсказании и чем точнее измерение.

В полётных контроллерах применяется расширенный вариант (EKF), оценивающий одновременно положение, скорость, ориентацию и смещения нулей датчиков. Для учебных задач комплементарного фильтра достаточно, а понимание Калмана — это уже уровень университетской теории оценивания.

Борьба с вибрацией

Вибрация винтов — главный враг данных на квадрокоптере. Она портит акселерометр, а через него — работу регулятора.

Лечится в таком порядке (от самого действенного к последнему средству):

Механика — балансировка

Отбалансированные винты и целые моторы дают на порядок меньше вибрации, чем любой фильтр способен убрать. Кривой винт не лечится программно — он лечится заменой.

Механика — развязка

Мягкие демпферы под платой контроллера гасят то, что осталось от механики.

Только потом — фильтрация

Программный фильтр нижних частот убирает остаток. Но каждый фильтр добавляет задержку, а задержка в контуре управления — прямой путь к раскачке.

Правило приоритета. Сначала механика, потом фильтры. Обучающийся, который пытается победить вибрацию агрессивной фильтрацией, получает вялый аппарат с запаздывающим регулятором — и не понимает почему. Это одна из самых частых ошибок при первой самостоятельной настройке.

Простой фильтр нижних частот

float filtered = 0.0f;
const float BETA = 0.2f;      // 0..1: меньше — сильнее сглаживание

float lowPass(float raw) {
    filtered = BETA * raw + (1.0f - BETA) * filtered;
    return filtered;
}

Каждое новое значение лишь слегка сдвигает результат. Чем меньше BETA, тем глаже выход — и тем больше запаздывание. Компромисс настраивается замером, а не на глаз.

Стенд телеметрии

Задача стенда — увидеть числа, пока аппарат стоит на столе.

void sendTelemetry() {
    static uint32_t last = 0;
    if (millis() - last < 50) return;      // 20 раз в секунду
    last = millis();

    Serial.printf("%lu,%.2f,%.2f,%.2f,%.2f\n",
                  millis(), angleRaw, angleFiltered, gyroRate, vbat);
}

Формат CSV выбран намеренно: такой вывод открывается в любой таблице и строится в график без дополнительных инструментов.

Снять данные в покое

Аппарат неподвижен на столе, запись 30 секунд. Смотрим уровень шума и наличие дрейфа. Это база для сравнения.

Снять данные с работающими моторами

Винты сняты, моторы раскручены. Разница с предыдущим замером — это и есть вклад вибрации.

Наклонить на известный угол

Кладём аппарат на угольник или на край стола под 45°. Проверяем, что фильтр показывает именно 45°, а не 30 или 60. Это калибровка доверия к своим данным.

Проверить возврат

Вернули в горизонт — угол должен вернуться к нулю и остаться там. Уползание означает, что дрейф гироскопа не компенсируется: проверяем $\alpha$ .

Главная мысль занятия. Отладка «на глаз, в воздухе» не работает: аппарат летит быстро, а обучающийся видит только результат. Отладка по числам на столе превращает загадку в наблюдение. Именно так устроена инженерная работа — сначала измерение, потом вывод.

Диагностика

Симптом	Причина	Проверка
Угол медленно уползает в покое	Дрейф гироскопа не компенсируется	Уменьшить $\alpha$ (0,98 → 0,95)
Угол дёргается при работе моторов	Вибрация на акселерометре	Балансировка винтов, демпферы
Угол реагирует с запаздыванием	Слишком сильная фильтрация	Увеличить `BETA`, ослабить фильтр
Показывает 45° вместо 30°	Не откалиброван или неверные оси	Калибровка на ровной поверхности
Данные приходят рывками	Плавающий период цикла	Фиксировать шаг, замерять `dt` фактически
Всё «плывёт» после разгона	Просадка питания влияет на датчик	Проверить питание и развязку

Приёмка занятия

Стенд собран, телеметрия идёт, график строится.
Обучающийся объясняет, почему нельзя взять угол только с гироскопа или только с акселерометра.
Показывает разницу замеров с моторами и без — и называет вывод.
При наклоне на известный угол фильтр даёт правильное значение и возвращается в ноль.

Каналы и CRSF Настройка ПИД