99а. Полётный контроллер на STM32: разбор Betaflight — гироскоп 8 кГц, фильтры, PID и DShot

В разделе 99 MPU-6050 читали по I²C сто раз в секунду, а угол собирали комплементарным фильтром. Гоночному квадрокоптеру этого мало: гироскоп опрашивают по SPI восемь тысяч раз в секунду, из его сигнала вычищают шум моторов, регулятор держит угловую скорость, а газ уходит на регуляторы оборотов цифровым кадром. Всё это делает Betaflight — открытая прошивка полётных контроллеров на STM32. Здесь она разобрана по коду: какие микроконтроллеры, как читается гироскоп, какие фильтры и в каком порядке, что считает регулятор и как устроен протокол DShot. Ключевые функции собраны на компьютере и прогнаны на модели гироскопа и мотора.

Опоры: исходники Betaflight 2026.12.0-alpha (коммит 744f95f): src/main/common/filter.c, sdft.c, src/main/sensors/gyro.c, gyro_init.c, gyro_filter_impl.c, src/main/flight/dyn_notch_filter.c, rpm_filter.c, pid.c, pid.h, pid_init.c, mixer_init.c, src/main/drivers/dshot.c, accgyro/gyro_sync.c, accgyro/accgyro_mpu.c, src/main/config/config.c, src/main/pg/*.c, src/platform/common/stm32/dshot_dpwm.c, pwm_output_dshot_shared.c, src/platform/STM32/pwm_output_dshot.c. filter.c, maths.c, sdft.c, dyn_notch_filter.c и rpm_filter.c собраны gcc на компьютере без правок; prepareDshotPacket, разбор телеметрии и заполнение буфера DMA вырезаны из файлов дословно. Модель гироскопа, моторов и регулятора оборотов — своя. Прогон — _verify/bf99a/bf99a.py, запись — _verify/bf99a/bf99a_host.txt. Лицензия: Betaflight — GPL-3.0 (файл LICENSE, в заголовках файлов — «либо версии 3, либо любой более поздней»); в разделе — пересказ и короткие цитаты с путями.

Как это называется в документации

Оригинал	Перевод
flight controller (FC)	полётный контроллер
ESC (electronic speed controller)	регулятор оборотов мотора
looptime, pid_process_denom	период цикла, делитель частоты регулятора
PT1, PT2, PT3 lowpass	фильтр нижних частот 1, 2 и 3 порядка
SVF (state variable filter), в CLI — BIQUAD	фильтр на переменных состояния
notch filter	режекторный фильтр
dynamic notch, SDFT	динамический режекторный, скользящее ДПФ
RPM filter	фильтр по оборотам моторов
bidirectional DShot, eRPM	двунаправленный DShot, электрические обороты
GCR (group coded recording)	код 4 бита → 5 бит в ответе регулятора
feedforward, I-term relax, anti-gravity	упреждение, ослабление И, «антигравитация»
burst DMA (DMAR), bitbang	пакетный DMA таймера, выдача через порт GPIO

99а.1 Что внутри и на чём работает

В src/platform/STM32/startup/ лежат стартовые файлы для STM32F405, F411, F446, F722, F745, F746, F765, G474, H562, H563, H723, H730, H735, H743, H757, H7A3, C562, C591, C5A3 и N657. Рядом с STM32 есть каталоги AT32, APM32, ESP32, PICO, X32 и SIMULATOR — ядро прошивки общее.

Один цикл идёт так (fc/core.c, sensors/gyro.c): задача фильтрации разбирает телеметрию DShot и прогоняет гироскоп через фильтры, затем главная задача считает регулятор (subTaskPidController) и выдаёт газ моторам (subTaskMotorUpdate). Обороты, которые регуляторы присылают обратно, настраивают фильтр в следующем цикле.

Прошивка для F405 собрана make STM32F405 в stm-arm:1: 649573 байта флеша из 992 КБ (63,95 %), 107820 байт ОЗУ из 128 КБ и 13388 байт CCM. Самая крупная функция цикла — pidController, 5812 байт; rpmFilterRun — 840, pt1FilterApply — 26. Там gcc 12.2.1, а Betaflight требует 13.3.1: пришлось снять проверку версии и в копии убрать : uint8_t из typedef enum : uint8_t (синтаксис C23 в rx/expresslrs_common.h).

99а.2 Гироскоп: SPI, 8 кГц, прерывание и DMA

Драйверы — в src/main/drivers/accgyro/. Самые частые на платах: accgyro_spi_mpu6000.c (SPI не быстрее 20 МГц), accgyro_spi_icm426xx.c (ICM-42688P и родня, до 24 МГц, частота выдачи 8 кГц — код 3 в таблице odrLUT) и accgyro_spi_bmi270.c (до 10 МГц). Частоту опроса задаёт gyroSetSampleRate в gyro_sync.c: по умолчанию 8000 Гц, у BMI270 и BMI160 — 3200, у ICM-45686 — 6400, у ICM-20649 — 9000.

Опрос не по таймеру. Вывод «данные готовы» гироскопа заведён на EXTI; обработчик mpuIntExtiHandler (accgyro_mpu.c) сразу запускает цепочку передач SPI через DMA — адрес первого регистра с битом чтения | 0x80, затем одним куском ускорение, температура и гироскоп (у MPU-6000 чтение начинается с регистра ускорения). Процессор узнаёт о данных, когда они уже в памяти.

Период цикла — targetLooptime = pid_process_denom × 10⁶ / частота: 125 мкс при 8 кГц и делителе 1 (по умолчанию DEFAULT_PID_PROCESS_DENOM равен 1). Делитель поднимает config.c, если мотор не успевает за циклом. Пересчёт этого правила для трёх случаев:

На F405 (USE_PID_DENOM_CHECK в target.h) при двунаправленном DShot и разгоне ниже уровня 2 протокол молча меняется на DShot300, а при гироскопе быстрее 4 кГц делитель становится не меньше 2. Запас на мотор берётся из config.c: 100 мкс для DShot300 и 31,25 мкс для DShot600, с телеметрией — вдвое больше.

99а.3 Фильтры нижних частот: PT1, PT2, PT3 и SVF

Главный кирпич — PT1, звено первого порядка (common/filter.c):

FAST_CODE_NOINLINE float pt1FilterGain(float f_cut, float dT)
{
    float omega = 2.0f * M_PIf * f_cut * dT;
    return omega / (omega + 1.0f);
}

FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
{
    filter->state = filter->state + filter->k * (input - filter->state);
    return filter->state;
}

При 250 Гц и шаге 125 мкс коэффициент равен 0,164124. PT2 и PT3 — два и три таких звена подряд; чтобы на частоте среза было −3 дБ, частоту умножают на 1,553773974 и 1,961459177. Четвёртый тип — SVF, фильтр второго порядка Баттерворта на переменных состояния. Замер настоящими функциями на 8 кГц, срез 250 Гц:

PT1 даёт на срезе −3,41, а не −3 дБ: формула коэффициента приближённая. SVF режет выше среза сильнее всех, а полосу до среза почти не трогает.

По умолчанию (sensors/gyro.h, gyro.c) гироскоп проходит два PT1. ФНЧ 2 — 500 Гц — работает на частоте гироскопа до прореживания. ФНЧ 1 динамический: срез растёт с газом от 250 до 500 Гц (dynLpfCutoffFreq, изгиб 5): при газе 0,25 — 335,9 Гц, при 0,5 — 406,2 Гц. На D-составляющую идут свои два PT1: динамический от 75 до 150 Гц и постоянный 150 Гц. Цена фильтров — задержка:

Про устройство КИХ и БИХ-фильтров — раздел 81а.

99а.4 Режекторные: RPM-фильтр и динамический

Порядок в gyro_filter_impl.c: прореживание (с ФНЧ 2), RPM-фильтр, два постоянных режекторных (по умолчанию выключены — частота 0), ФНЧ 1, динамический режекторный. Все режекторные — тот же SVF в режекторном включении. Замер на 300 Гц:

RPM-фильтр (flight/rpm_filter.c, настройки в pg/rpm_filter.c) ставит режекторный на каждую из трёх гармоник каждого мотора: 12 на ось у квадрокоптера, добротность 5 (500 / 100), нижняя граница 100 Гц, ниже 150 Гц вес плавно уменьшается (полоса 50 Гц), верх — 0,48 частоты цикла, 3840 Гц. За цикл обновляются 2 режекторных: все 12 — за 1 мс. Частоты берутся из телеметрии DShot (99а.6), без неё фильтр выключен.

Модель: движение 10 Гц размахом 50 °/с, четыре мотора на 330, 335, 340 и 345 об/с с гармониками 8, 4 и 2 °/с, шум ±2 °/с. После настоящего rpmFilterRun у мотора 1 гармоники 330, 660 и 990 Гц упали с 7,993, 4,017 и 2,018 до 0,0008, 0,0003 и 0,0002 (−79,7, −82,9, −78,4 дБ), у мотора 4 — на −81,0, −84,8 и −77,2 дБ. Движение 10 Гц: 49,98 до и после. Мотор на 120 Гц — вес (120 − 100) / 50 = 0,4 — гасится слабо: 8,000 → 0,990.

Динамический режекторный (dyn_notch_filter.c, common/sdft.c) ищет то, что не связано с оборотами, — резонансы рамы. Скользящее ДПФ на 72 отсчётах даёт 36 полос. При 8 кГц и верхней границе 600 Гц усредняются по 6 отсчётов, ДПФ идёт на 1333,3 Гц, шаг полосы 18,52 Гц, рабочие полосы 5…32. Расчёт разбит на 4 шага на ось — полный круг по трём осям за 12 циклов. Пики уточняются параболой по трём соседним полосам. По умолчанию (pg/dyn_notch.c) — 3 режекторных, 100…600 Гц, добротность 3, начальные центры 183,3, 350,0 и 516,7 Гц. Ниже 2 кГц цикла фильтр выключается.

Модель: движение 10 Гц и резонанс 180 Гц размахом 10 °/с. Через 0,5 с ближайший центр стоял на 181,6 Гц; резонанс ослаблен с 9,989 до 0,032, движение 10 Гц — 50,00 → 49,99.

99а.5 Регулятор: P, I, D и упреждение

Betaflight держит угловую скорость (в режиме «акро» стик задаёт °/с), а не угол; основа ПИД-регулятора — раздел 47. Коэффициенты из конфигуратора переводятся множителями из pid.h: P × 0,032029, I × 0,244381, D × 0,000529, F × 0,013754 / 100. Значения по умолчанию:

Для крена это Kp = 1,441, Ki = 19,55, Kd от 0,01587 до 0,02116; предел суммы — 500. Особенности:

• D — по гироскопу, а не по ошибке. В pid.c: c const float delta = - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency; Рывок стика не бьёт по D; реакцию на стик даёт упреждение F.
• D max: D растёт от 30 к 40, когда гироскоп или стик меняются быстро.
• I-term relax: уставка проходит PT1 15 Гц, разность с ней — «быстрая часть». Накопление И умножается на max(0, 1 − быстрая / 40): при 20 °/с — вдвое медленнее, от 40 °/с — стоит. Так И не набегает на флипах.
• Anti-gravity: при резком газе производная газа (PT2 5 Гц, усиление 80) временно ускоряет И и поднимает P — нос не клюёт при наборе.

Микшер (mixer_init.c, mixerQuadX) раздаёт газ, крен, тангаж и рыскание на четыре мотора коэффициентами ±1; холостой газ motor_idle — 550, то есть 5,5 %.

99а.6 DShot: кадр, таймер с DMA и ответ регулятора

Кадр DShot — 16 бит: 11 бит значения, бит запроса телеметрии и 4 бита контрольной суммы. Значение 0 — стоп, 1…47 — команды, 48…2047 — газ. Сумма — XOR трёх полубайтов (drivers/dshot.c, prepareDshotPacket):

    // compute checksum
    unsigned csum = 0;
    unsigned csum_data = packet;
    for (int i = 0; i < 3; i++) {
        csum ^=  csum_data;   // xor data by nibbles
        csum_data >>= 4;
    }
    // append checksum
#ifdef USE_DSHOT_TELEMETRY
    if (useDshotTelemetry) {
        csum = ~csum;
    }
#endif
    csum &= 0xf;
    packet = (packet << 4) | csum;

С запросом телеметрии 1046 даёт 0x82D7, а флаг запроса после вызова сбрасывается. У двунаправленного DShot сумма инвертирована, и линия тоже (output ^= TIMER_OUTPUT_INVERTED в pwm_output_dshot.c).

Выдача. Канал таймера в режиме ШИМ, DMA на каждом переполнении пишет в CCR длительность следующего бита (dshot_dpwm.c):

FAST_CODE_NOINLINE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
{
    int i;
    for (i = 0; i < 16; i++) {
        dmaBuffer[i * stride] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0;  // MSB first
        packet <<= 1;
    }
    dmaBuffer[i++ * stride] = 0;
    dmaBuffer[i++ * stride] = 0;

    return DSHOT_DMA_BUFFER_SIZE;
}

Для 1046 — 18 слов: 14 7 7 7 7 7 14 7 14 14 7 7 7 14 14 7 0 0. Таймер тикает на 3, 6 или 12 МГц (dshot_dpwm.h), бит — 20 тиков, ноль — 7, единица — 14:

Ноль — 35 % бита, единица — 70 %. Предделитель — частота таймера / такт − 1: у F405 на TIM1 и TIM8 (168 МГц) для DShot600 это 13, на TIM2–TIM5 (84 МГц) — 6, у F722 (216 МГц) — 17. Если моторы сидят на одном таймере, пакетный DMA (DMAR, 4 записи CCR1…CCR4 за переполнение) обходится одним потоком; в режиме «авто» он включается только без телеметрии. Второй путь — bitbang: DMA пишет в BSRR порта, 3 состояния на бит, то есть 33 и 67 %. На F4 он в режиме «авто» включается только с телеметрией, на остальных — всегда, кроме ProShot (dshot_bitbang_shared.c).

Ответ регулятора. После кадра вывод переключается на захват обоих фронтов тем же таймером (ARR = 0xffffffff), DMA складывает метки фронтов. Бит ответа — 16 тиков, в 5/4 раза быстрее кадра. 16 бит ответа (12 бит периода вида eee mmmmmmmmm — период m << e мкс — и 4 бита суммы) кодируются GCR по 5 бит на полубайт и передаются перепадами: 21 бит. decodeTelemetryPacket (pwm_output_dshot_shared.c) делит интервал между фронтами на 16 с округлением, обратной таблицей собирает полубайты и проверяет, что XOR всех четырёх равен 0xF.

Модель регулятора (своя) — прогон через настоящий разбор: 20000 об/мин, 14 полюсов, eRPM 140000, период 428,57 мкс → 12 бит 0x1AD (e = 0, m = 429), кадр 0x1AD9, GCR 0xDA9A9, 12 фронтов. Разбор дал 1399 (сотни eRPM) → 333,10 Гц, 19986 об/мин. Сдвиг одного фронта на ±7 тиков (±0,58 мкс при DShot600) принят, на ±8 — отвергнут; один испорченный бит даёт 0xFFFF, «ошибка». На 581 значении от 2000 до 60000 об/мин ошибок разбора нет, наибольшая ошибка оборотов — 0,446 %: цена округления периода до целых микросекунд.

99а.7 Грабли

В CLI фильтр называется BIQUAD, а биквадратного фильтра в коде нет. Тип 1 в filter.h — FILTER_SVF, строка таблицы lookupTableLowpassType осталась «BIQUAD». Ищите в коде svfLowpassFilterApply.

На F405 включили двунаправленный DShot600, а на осциллографе DShot300 и ПИД 4 кГц. Это config.c, 99а.2: без разгона F405 не тянет оба. Либо смириться, либо разгон уровня 2 (216 МГц).

RPM-фильтр ничего не режет. Нет двунаправленного DShot — фильтр выключен (rpmFilterInit выходит сразу). Или неверно число полюсов motor_poles (по умолчанию 14): частота уедет, а режекторный с Q 5 в полосе ±17 % от центра даёт лишь −1,12 дБ.

Динамический режекторный пропал. Цикл медленнее 2 кГц (DYN_NOTCH_UPDATE_MIN_HZ) — фильтр отключается целиком.

Ответ регулятора то и дело «ошибка». Разбор терпит сдвиг фронта до 7 тиков из 16 на бит; наводки и длинные провода к регулятору съедают этот запас.

Борьба с шумом ФНЧ съела отклик. Каждый низкий срез — задержка: цепочка D по умолчанию уже даёт 3,168 мс на 10 Гц. Сначала режекторные по оборотам, потом снижать ФНЧ.

99а.8 Итоги

• Betaflight 2026.12.0-alpha: стартовые файлы для 20 STM32 от F405 до H7, C5 и N657; ядро общее и для AT32, APM32, ESP32, PICO.
• Гироскоп по SPI с прерыванием и DMA: 8 кГц, цикл 125 мкс; у BMI270 — 3200 Гц. На F405 с двунаправленным DShot — DShot300 и ПИД 4 кГц.
• Фильтры по умолчанию: PT1 500 Гц, RPM-фильтр (3 гармоники, Q 5), динамический PT1 250…500 Гц, 3 динамических режекторных 100…600 Гц; задержка гироскопа 0,954 мс, D — 3,168 мс на 10 Гц.
• RPM-фильтр на модели гасит гармоники моторов на 77…85 дБ, динамический нашёл резонанс 180 Гц на 181,6 Гц за 0,5 с.
• ПИД по угловой скорости: D по гироскопу, D max, упреждение F, ослабление И выше 40 °/с, anti-gravity.
• DShot600: бит 1,6667 мкс, ноль 35 %, единица 70 %, кадр 26,667 мкс; сумма — XOR полубайтов, у двунаправленного инвертирована. Ответ — GCR, 21 бит за 28 мкс, допуск ±7 тиков, обороты с точностью 0,446 %.

Дальше: раздел 99б «Ориентация по IMU: кватернион, фильтр Маджвика и калибровка магнитометра» — все три угла сразу: кватернион вместо углов Эйлера, фильтр Маджвика и компас, откалиброванный из эллипсоида в сферу.