Датчики и приводы: считывание, фильтрация, калибровка, моторы

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

Это главный сдвиг статьи. В бэкенде источник истины — база; во встраиваемой системе источника истины нет вообще, есть физический мир и ваши модели того, как он отображается в числа. Дальше — механика этого отображения в обе стороны: от физики к числу (датчики, калибровка, фильтры) и от числа к физике (драйверы, ШИМ, моторы, обратная связь). АЦП, таймеры и ШИМ берутся как данность из статьи про периферию, шины к цифровым датчикам — из статьи про протоколы.

Тракт измерения: где именно рождается ошибка

Между физической величиной и переменной float temperature_c стоит цепочка из пяти-шести ступеней, и каждая портит сигнал по-своему. Отладка «датчик врёт» становится осмысленной работой ровно тогда, когда вы умеете сказать, на какой ступени врёт.

Главное правило звучит жёстко: систематическую ошибку убирает калибровка, случайную — усреднение, а алиасинг не убирает ничто. Если на входе АЦП есть помеха 1005 Гц, а вы оцифровываете с частотой 1000 Гц, она придёт в данные как медленное колебание с периодом 200 мс — неотличимое от настоящего сигнала. Цифровой фильтр её уже не отделит: информации о том, что это была за частота, в отсчётах физически нет. Лечится только аналоговым RC-фильтром до входа АЦП. Это первое, что надо проверить на плате, и первое, что забывают программисты.

Что бывает на выходе датчика

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

Аналоговые датчики: отношение вместо напряжения

Классика — терморезистор NTC в делителе: $V_{in} = V_{ref} \cdot \frac{R_{ntc}}{R_{ntc} + R_{fix}}$. Если делитель и опора АЦП питаются от одного напряжения, $V_{ref}$ сокращается:

$$\frac{raw}{2^N - 1} = \frac{R_{ntc}}{R_{ntc} + R_{fix}} \quad \Rightarrow \quad R_{ntc} = R_{fix} \cdot \frac{raw}{2^N - 1 - raw}$$

Такое измерение называют ratiometric, и оно нечувствительно к дрейфу питания: батарея села с 3,30 до 3,05 В — результат не изменился ни на градус. Так подключают потенциометры, тензомосты и термисторы. Ошибка — питать делитель от 5 В, а мерить относительно внутренней опоры 1,2 В, получая зависимость показаний от заряда батареи.

#define ADC_MAX 4095u
#define R_FIX   10000L
// Сопротивление NTC 10k B=3950 (Ом) для температур от -20 до +100 °C с шагом 10 °C.
static const uint16_t ntc_r[13] = {97100,55300,32600,19900,12500,8060,5330,
                                    3600, 2490, 1750, 1250,  911, 674};

// Температура в децицельсиях (251 = 25,1 °C). Только целые: на Cortex-M0 без FPU
// логарифм для уравнения Стейнхарта-Харта стоит ~2000 тактов, таблица — ~40.
int16_t ntc_temp_dc(uint16_t raw)
{
    if (raw == 0u || raw >= ADC_MAX) return INT16_MIN;      // обрыв или замыкание
    int32_t r = (R_FIX * (int32_t)raw) / (int32_t)(ADC_MAX - raw);
    for (unsigned i = 0; i + 1u < 13u; i++) {
        int32_t hi = ntc_r[i], lo = ntc_r[i + 1u];
        // Линейная интерполяция внутри интервала: ошибка < 0,3 °C при шаге 10 °C.
        if (r <= hi && r >= lo)
            return (int16_t)(-200 + (int32_t)i * 100 + ((hi - r) * 100) / (hi - lo));
    }
    return (r > (int32_t)ntc_r[0]) ? (int16_t)-200 : (int16_t)1000;
}

Сложность: $O(n)$ перебором или $O(\log n)$ бинарным поиском, но при $n = 13$ линейный проход быстрее из-за предсказуемости ветвлений; память — 26 байт во flash. Точное уравнение $\frac{1}{T} = A + B\ln R + C(\ln R)^3$ уместно на STM32F4 с FPU, на AVR таблица выигрывает на два порядка.

Вторая тонкость аналогового входа — время выборки. Внутри АЦП последовательного приближения стоит конденсатор хранения в единицы пикофарад, заряжающийся через ключ от источника сигнала. Если выходное сопротивление источника велико, конденсатор не успевает зарядиться, и код получается заниженным и зависящим от того, что мерили на предыдущем канале — характерный симптом «соседние каналы протекают друг в друга». Правило из даташитов ST: при 12 битах источник должен иметь сопротивление порядка килоом при коротком времени выборки; для делителей в сотни килоом ставьте время выборки максимальным (480 тактов на STM32F4) либо буферизуйте повторителем на ОУ.

Цифровые датчики: карта регистров и право не опрашивать

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

Три правила, экономящие дни отладки. Читать пачкой, а не по регистру: шесть транзакций I2C по 12 байт не просто медленнее — между чтением старшего и младшего байта датчик может обновить данные, и вы получите значение, которого никогда не было (классический разрыв, дающий редкие выбросы в 32000 единиц). Автоинкремент адреса и одно чтение на 14 байт закрывают вопрос. Проверять WHO_AM_I при старте: одна строка отличает «датчик не припаян» от «датчик врёт». Доверять регистру статуса, а не своему таймеру: если ODR датчика 1000 Гц и вы читаете по своему таймеру на 1000 Гц, кварцы разъедутся, и вы начнёте то читать одно значение дважды, то пропускать.

Времяимпульсные датчики: считает таймер, а не процессор

Квадратурный энкодер выдаёт два меандра со сдвигом 90°; направление определяется тем, какой канал опережает. Считать это программно — гарантированный проигрыш: на 3000 об/мин и 1000 импульсах на оборот получается 200 тысяч фронтов в секунду. Таймеры STM32 и ESP32 имеют аппаратный режим энкодера, где счётчик меняется самим железом, без единого такта процессора.

void encoder_init(void)   // TIM3 в режиме энкодера: канал A -> PA6, канал B -> PA7
{
    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
    TIM3->SMCR = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1;    // SMS=011: счёт по обоим каналам, режим x4
    TIM3->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0
                | (0x5u << TIM_CCMR1_IC1F_Pos)       // входной фильтр: сигнал стабилен 8 тактов —
                | (0x5u << TIM_CCMR1_IC2F_Pos);      // режет дребезг и наводки от мотора
    TIM3->ARR = 0xFFFFu;                             // считаем по кругу, переполнение штатно
    TIM3->CNT = 0x8000u;
    TIM3->CR1 = TIM_CR1_CEN;
}

// Вызывается строго периодически из задачи регулятора (например, раз в 5 мс).
int16_t encoder_delta(void)
{
    static uint16_t prev = 0x8000u;
    uint16_t now = (uint16_t)TIM3->CNT;
    int16_t d = (int16_t)(now - prev);   // намеренное переполнение uint16 — верно в обе стороны
    prev = now;
    return d;
}

Разрешение — $4 \cdot PPR$ отсчётов на оборот вала; при редукторе 1:30 и энкодере 11 PPR это 1320 отсчётов на оборот выходного вала. Скорость считают не производной от позиции, а именно так: количество отсчётов за фиксированный интервал. Это встроенное усреднение, и шума в нём заметно меньше, чем в численном дифференцировании.

Ультразвуковой дальномер HC-SR04 меряется входным захватом таймера: длительность эха в микросекундах делить на 58 — сантиметры. Функция pulseIn() в блокирующем цикле — типичная ошибка Arduino-кода: до 25 мс на измерение, и любой контур управления мёртв. Правильный способ — input capture с прерыванием, разобранный в статье про прерывания.

Метрология: разрешение — это не точность

Датчик может иметь 24 бита разрешения и точность 5 % — так и бывает: HX711 отдаёт 24 бита, из которых свободны от шума 15–18, а абсолютная точность тензомоста без калибровки вообще не определена. Полезная величина — ENOB (эффективное число бит): $SNR_{dB} = 6{,}02 \cdot ENOB + 1{,}76$. Двенадцатибитный АЦП STM32 в схеме с шумной цифровой землёй легко даёт ENOB 9,5 — значит, младшие два-три бита это генератор случайных чисел, и показывать их пользователю нельзя.

Отсюда приём: передискретизация с усреднением. Усреднение $4^k$ независимых выборок уменьшает СКО шума в $2^k$ раз и добавляет $k$ эффективных бит: из 12 бит сделать 14 — усреднить 16 выборок, сделать 16 — усреднить 256. Условия: шум должен быть больше половины кванта (иначе усреднять нечего, все выборки одинаковы), а сигнал — медленнее частоты выборки. На STM32 это бесплатно: таймер запускает АЦП, DMA складывает в буфер, задача просыпается на готовом массиве.

Калибровка: модель ошибки и как её обратить

Рабочая модель для 90 % датчиков — аффинная плюс кривизна: $y = a \cdot x + b + f(x) + n(t)$, где $a$ — ошибка усиления, $b$ — смещение, $f(x)$ — нелинейность, $n(t)$ — шум. Калибровка убирает первые три, фильтрация — четвёртый.

АЛГОРИТМ two_point_calibrate(ref_lo, ref_hi, raw_lo, raw_hi)
  требовать |raw_hi - raw_lo| > 10 * шум        # иначе точки слишком близко
  gain   := (ref_hi - ref_lo) / (raw_hi - raw_lo)
  offset := ref_lo - gain * raw_lo
  вернуть (gain, offset)
  # O(1) по времени и памяти. Точки берут ближе к краям рабочего диапазона:
  # внутри ошибка интерполируется, снаружи — экстраполируется и растёт неограниченно.

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

"""Подбор калибровки по эталонным измерениям. Запускается на ноутбуке, не на МК."""
import numpy as np

raw = np.array([812, 1024, 1530, 2048, 2570, 3100, 3480], dtype=float)  # коды АЦП
ref = np.array([-10.0, 0.0, 20.2, 40.1, 60.3, 80.0, 95.4])              # эталон, °C

for deg in (1, 2, 3):
    coef = np.polyfit(raw, ref, deg)      # МНК: O(n * deg^2), n мало — считается мгновенно
    print(f"степень {deg}: {np.abs(ref - np.polyval(coef, raw)).max():.3f} °C")
# 1: 1.184 °C | 2: 0.212 °C | 3: 0.198 °C — третья степень прироста уже не даёт

# Считаем в double, но в прошивку кладём float32 — и ПЕРЕСЧИТЫВАЕМ ошибку после округления,
# иначе сюрприз на железе. Константы копируются прямо в исходник прошивки.
q = np.polyfit(raw, ref, 2).astype(np.float32)
print("после float32:", np.abs(ref - np.polyval(q.astype(float), raw)).max())
print("static const float CAL[3] = {%.9gf, %.9gf, %.9gf};" % tuple(q))

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

Калибровка IMU: три уровня работы

Гироскоп — смещение нуля. Неподвижный датчик показывает не ноль, а 1–5 °/с. Интеграл этого смещения уводит угол на градус в секунду: за минуту робот «повернётся» на 60°, не двигаясь. Лечится усреднением 500–2000 отсчётов в покое при старте — с обязательной проверкой, что устройство действительно неподвижно, иначе смещение впитает реальное движение. Оно дрейфует с температурой, поэтому в серьёзных системах его перекалибровывают при каждой остановке.

Акселерометр — шесть положений. Модель $a_{true} = S \cdot (a_{raw} - b)$, где $b$ — вектор смещений трёх осей, $S$ — диагональная матрица масштабов. Кладём устройство на шесть граней; в каждом положении одна ось видит $\pm g$, две другие — ноль. Отсюда $b_i = \frac{a_i^{+} + a_i^{-}}{2}$ и $s_i = \frac{2g}{a_i^{+} - a_i^{-}}$. Пять минут работы, точность растёт в разы.

Магнитометр — эллипсоид. Искажения двух видов: hard iron (постоянные магниты и токи на самой плате сдвигают центр сферы измерений) и soft iron (ферромагнитные детали корпуса превращают сферу в эллипсоид). Процедура: повращать устройство во всех ориентациях, набрать облако точек, подогнать эллипсоид, найти преобразование в сферу. Без этого компас робота ошибается на 20–40°, причём ошибка зависит от курса — что особенно коварно.

Где хранить калибровку

typedef struct {
    uint32_t magic;        // 0xCA11B0FF — «здесь лежит валидная структура»
    uint16_t version;      // версия формата: право менять поля в будущих прошивках
    uint16_t reserved;
    float    gain, offset;
    int16_t  gyro_bias[3];
    uint32_t crc32;        // CRC по всем предыдущим байтам
} calib_t;

// Загрузка с деградацией, а не с отказом: битая калибровка не должна кирпичить устройство.
void calib_load(calib_t *out)
{
    const calib_t *s = (const calib_t *)CALIB_FLASH_ADDR;
    if (s->magic == 0xCA11B0FFu && s->version == CALIB_VERSION &&
        crc32_hw((const uint8_t *)s, offsetof(calib_t, crc32)) == s->crc32)
        *out = *s;
    else {
        *out = calib_defaults;           // заводские константы: хуже, но устройство работает
        fault_set(FAULT_CALIB_INVALID);  // и обязательно сообщаем наружу, что калибровки нет
    }
}

Три вещи, за которые платят возвратами из поля: CRC (сектор flash портится при отключении питания во время записи), версия формата (иначе новая прошивка прочитает старую структуру как мусор) и поведение по умолчанию (устройство работает на заводских константах, а не уходит в HardFault). Про сектора, износ и атомарность записи — в статье про отладку и производство.

Фильтрация: четыре инструмента и когда какой

Фильтр всегда покупается за задержку. Это не побочный эффект, а закон: чтобы отличить сигнал от шума, надо посмотреть на несколько отсчётов, то есть подождать. Если результат идёт в контур управления, задержка съедает запас устойчивости; если на экран человеку — она бесплатна. Поэтому выбор фильтра начинается с вопроса «куда пойдёт результат».

Медиана против выбросов

Медиана из трёх последних отсчётов полностью удаляет одиночный выброс любой амплитуды и почти не искажает фронты. Среднее в той же ситуации размажет выброс по всему окну — испортит три отсчёта вместо одного.

int16_t adc_median3(int16_t x)
{
    static int16_t h[3];
    h[0] = h[1]; h[1] = h[2]; h[2] = x;          // сдвиг окна: 6 байт состояния
    int16_t a = h[0], b = h[1], c = h[2], t;     // сеть сортировки: ровно 3 сравнения,
    if (a > b) { t = a; a = b; b = t; }          // без циклов и без промахов предсказателя
    if (b > c) { t = b; b = c; c = t; }
    if (a > b) { b = a; }
    return b;
}

Для окна $n$ медиана честно стоит $O(n \log n)$ при сортировке, но при $n \le 7$ сети сортировки дают фиксированное число сравнений и выигрывают у любой сортировки — про них есть разбор в треке алгоритмов. Задержка медианного фильтра — примерно $\frac{n-1}{2}$ отсчётов, память — $O(n)$.

Скользящее среднее и EMA

Скользящее среднее по $n$ отсчётам подавляет белый шум в $\sqrt{n}$ раз и вносит групповую задержку ровно $\frac{n-1}{2}$ отсчётов. Реализуется рекурсивно за $O(1)$ — прибавили новый, вычли выпавший, — но требует кольцевого буфера на $n$ элементов. Экспоненциальное среднее (EMA, однополюсный БИХ-фильтр) убирает и эту цену:

$$y_k = \alpha \cdot x_k + (1 - \alpha) \cdot y_{k-1}, \qquad \alpha = \frac{\Delta t}{\tau + \Delta t}, \qquad f_c \approx \frac{1}{2\pi\tau}$$

Одна переменная состояния, одно умножение, никакого буфера — поэтому EMA и есть рабочая лошадь встраиваемых систем. Не подбирайте $\alpha$ на глаз, считайте от частоты среза: нужен срез 5 Гц при выборке 1000 Гц — значит $\tau = \frac{1}{2\pi \cdot 5} = 31{,}8$ мс и $\alpha = \frac{1}{31{,}8 + 1} = 0{,}0305$.

// Целочисленный EMA: эквивалент alpha = 1/2^k. На Cortex-M0 без FPU это 3 такта против ~40
// на float. Состояние — 4 байта на канал.
typedef struct { int32_t acc; uint8_t k; } ema_i_t;   // acc хранит значение, сдвинутое на k

static inline int16_t ema_i_update(ema_i_t *f, int16_t x)
{
    // Аккумулятор масштабирован намеренно: иначе младшие разряды теряются при каждом
    // сдвиге и фильтр «залипает», навсегда останавливаясь в паре единиц от истины.
    f->acc += (int32_t)x - (f->acc >> f->k);
    return (int16_t)(f->acc >> f->k);
}

Соответствие $\alpha = 2^{-k}$: $k = 3$ даёт $0{,}125$, $k = 6$ — $0{,}0156$. Залипание из комментария — самая частая ошибка самодельных целочисленных фильтров, и она не воспроизводится в тестах на синтетических данных, потому что там разница вход-выход всегда велика.

Комплементарный фильтр: два датчика, одна строка

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

$$\theta_k = a \cdot (\theta_{k-1} + \omega_k \Delta t) + (1 - a) \cdot \theta_{acc}$$

// dt в секундах, gyro_y в рад/с. a = tau/(tau+dt): при tau = 0,5 c и dt = 5 мс это 0,990.
float tilt_update(float gyro_y, float acc_x, float acc_z, float dt)
{
    static float theta = 0.0f;
    const float tau = 0.5f, a = tau / (tau + dt);
    float theta_acc = atan2f(-acc_x, acc_z);   // абсолютный угол: медленный, но не уплывает
    theta = a * (theta + gyro_y * dt) + (1.0f - a) * theta_acc;
    return theta;
}

Постоянная времени $\tau$ — это граница раздела: быстрее неё верим гироскопу, медленнее — акселерометру. Полсекунды — разумная отправная точка для наземного робота; для квадрокоптера берут 1–2 с, потому что там ускорения корпуса велики и акселерометру можно верить только в среднем.

Скалярный Калман: когда нужна ещё и уверенность

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

АЛГОРИТМ kalman_1d_step(x, P, z, Q, R)
  # x — оценка, P — её дисперсия, z — измерение
  # Q — дисперсия шума процесса (насколько величина успевает измениться за шаг)
  # R — дисперсия шума измерения (насколько врёт датчик)
  P_pred := P + Q                     # предсказание: оценка та же, доверие падает
  K      := P_pred / (P_pred + R)     # усиление: доля, которую берём от невязки
  x      := x + K * (z - x)           # коррекция
  P      := (1 - K) * P_pred          # доверие выросло
  вернуть (x, P)
  # O(1) по времени и памяти: два float состояния.

"""Подбор Q и R по записи с реального датчика. Прототип на ноутбуке — потом константы в C."""
import numpy as np

def kalman_1d(z, Q, R):
    x, P, out = float(z[0]), 1.0, np.empty(len(z))
    for i, zi in enumerate(z):
        P = P + Q                 # предсказание
        K = P / (P + R)           # усиление Калмана
        x, P = x + K * (zi - x), (1.0 - K) * P   # коррекция
        out[i] = x
    return out

R = float(np.load("rest.npy").var())   # R ИЗМЕРЯЮТ: датчик в покое, дисперсия шума
for Q in (1e-4, 1e-3, 1e-2, 1e-1):
    # Установившееся усиление — решение P = (1-K)(P+Q): K = (-Q + sqrt(Q^2 + 4QR)) / 2R.
    K_ss = (-Q + np.sqrt(Q * Q + 4 * Q * R)) / (2 * R)
    print(f"Q={Q:<7g} K_ss={K_ss:.4f} — то же, что EMA с alpha={K_ss:.4f}")

Практический вывод, экономящий много времени: для одномерной величины без модели динамики Калман в установившемся режиме — это EMA. Брать его стоит ради двух вещей: корректной работы при нерегулярных и пропущенных измерениях (в EMA нечем выразить «прошло 300 мс вместо 5») и ради самого $P$, по которому видно, можно ли доверять оценке прямо сейчас. Настоящие матрицы — оценка позиции и скорости вместе, слияние одометрии, IMU и лидара — это уже расширенный фильтр Калмана из статьи про робототехнику.

Сравнение по двум осям, которые важны на практике

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

Приводы: от числа обратно к физике

Здесь программиста подстерегает другой набор сюрпризов: ток, индуктивность, тепло и механика.

Щёточный мотор и H-мост

Первое, что надо принять: мотор нельзя подключить к GPIO. Ножка отдаёт 20 мА, мотор при пуске берёт 2 А — между ними всегда стоит ключ. Второе: мотор индуктивен, и ток через индуктивность не прерывается мгновенно. В момент выключения ключа ток продолжает течь и создаёт выброс напряжения, пробивающий транзистор, если ему некуда деться. Отсюда обязательный обратный диод у одиночного ключа и встроенные диоды в любой микросхеме моста.

void motor_pwm_init(void)     // TIM1, комплементарные каналы, драйвер DRV8871 / TB6612
{
    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
    TIM1->PSC = 0;
    TIM1->ARR = 2099;                       // 84 МГц / 2100 = 40 кГц — выше слышимого диапазона
    TIM1->CCMR1 = (0x6u << 4) | TIM_CCMR1_OC1PE | (0x6u << 12) | TIM_CCMR1_OC2PE;
    TIM1->CCER  = TIM_CCER_CC1E | TIM_CCER_CC2E;
    // Мёртвое время задаётся АППАРАТНО: ~100 нс между закрытием одного ключа и открытием
    // другого. Задержки в коде этого не заменят — они не синхронны с ШИМ.
    TIM1->BDTR  = TIM_BDTR_MOE | (0x0Au << TIM_BDTR_DTG_Pos);
    TIM1->CR1   = TIM_CR1_ARPE | TIM_CR1_CEN;
}

void motor_set(int16_t speed)  // -1000..+1000
{
    if (speed >  1000) speed =  1000;
    if (speed < -1000) speed = -1000;
    if (speed > -50 && speed < 50) {        // мёртвая зона: ниже ~5 % мотор стоит и греется
        TIM1->CCR1 = 0; TIM1->CCR2 = 0;     // выбег
    } else {
        uint16_t duty = (uint16_t)((speed < 0 ? -speed : speed) * (TIM1->ARR + 1u) / 1000u);
        TIM1->CCR1 = (speed > 0) ? duty : 0;
        TIM1->CCR2 = (speed > 0) ? 0 : duty;
    }
}

Про частоту ШИМ. Диапазон 1–5 кГц попадает точно в слышимый — мотор пищит, и заказчик замечает это первым. Уходить надо выше 20 кГц, но не сильно: каждое переключение стоит энергии в транзисторе, и на 100 кГц драйвер греется сильнее мотора. 20–40 кГц — практический оптимум. Отдельная тонкость: разрешение ШИМ обратно пропорционально частоте. На 84 МГц и 40 кГц остаётся 2100 шагов, около 11 бит — достаточно; на ESP32 через LEDC на 40 кГц тоже 11 бит, а 16 бит доступны только до 1,2 кГц — и это уже слышно.

Шаговый двигатель: открытый контур и его цена

Шаговик держит положение без датчика: подали последовательность фаз — ротор встал куда надо. Это огромное удобство и главная ловушка одновременно: если момент нагрузки превысит удерживающий, ротор пропустит шаг, и система об этом не узнает — координата в программе разъедется с реальностью навсегда.

Современные драйверы (A4988, DRV8825, TMC2209) принимают всего два сигнала, STEP и DIR, и сами занимаются токовыми петлями и микрошагом. От программы требуется темп импульсов — именно темп: подать сразу целевую частоту нельзя, ротор с нагрузкой не успеет разогнаться и сорвётся. Нужна рампа.

typedef struct {
    int32_t  pos, target;      // текущая и целевая позиция в шагах
    uint32_t c, c_min;         // текущий интервал между шагами и предел скорости, в тактах
    int32_t  n;                // номер шага в фазе разгона — параметр рекуррентности
} stepper_t;

void TIM2_IRQHandler(void)     // срабатывает по достижению ARR
{
    TIM2->SR &= ~TIM_SR_UIF;
    stepper_t *s = &g_step;
    if (s->pos == s->target) { TIM2->CR1 &= ~TIM_CR1_CEN; return; }

    STEP_PORT->BSRR = STEP_PIN;                 // фронт STEP: драйвер ловит именно фронт
    s->pos += (s->target > s->pos) ? 1 : -1;
    int32_t left = (s->target > s->pos) ? (s->target - s->pos) : (s->pos - s->target);

    if (left <= s->n) {                         // тормозим: осталось столько же, сколько разгонялись
        if (--s->n > 0) s->c += (2 * s->c) / (4 * s->n + 1);
    } else if (s->c > s->c_min) {               // разгон: c[n+1] = c[n] - 2*c[n]/(4n+1)
        s->n++;
        s->c -= (2 * s->c) / (4 * s->n + 1);
        if (s->c < s->c_min) s->c = s->c_min;
    }
    TIM2->ARR = s->c - 1u;                      // вступит в силу со следующего периода
    STEP_PORT->BSRR = (uint32_t)STEP_PIN << 16; // спад
}

Алгоритм Дэвида Остина (https://www.embedded.com/generate-stepper-motor-speed-profiles-in-real-time/) даёт линейное ускорение без единой операции с плавающей точкой: $O(1)$ на шаг, около 20 байт состояния. Это классика, которая крутится в каждом 3D-принтере.

Микрошаг — разделение полного шага на 16, 32 или 256 положений синусоидальными токами фаз. Он не увеличивает точность позиционирования (удерживающий момент в промежуточных положениях мал, и реальная погрешность остаётся на уровне полного шага), но радикально уменьшает вибрацию и шум. Ставить 256 микрошагов ради точности — распространённое заблуждение; ради тишины — правильно.

RC-серво и BLDC

Хобби-серво управляется импульсом 1000–2000 мкс, повторяющимся каждые 20 мс. Позиция задаётся длительностью импульса, а не скважностью — путать легко, последствия дорогие. Внутри серво уже стоит замкнутый контур с потенциометром, поэтому вам достаточно таймера: предделитель до тика в 1 мкс, ARR = 19999 (период 20 мс), CCR = 1000 + deg * 1000 / 180. Границы у каждой серво свои и калибруются, а не берутся из статей.

Ограничения, о которых узнают на практике: серво не сообщает свою позицию (вы не знаете, дошла она или упёрлась), потребляет 0,5–2 А в пике (питать от вывода 5 В платы Arduino — верный способ ловить перезагрузки) и дрожит на границах диапазона, потому что упор внутрь механики означает постоянную борьбу контура, нагрев и смерть за пару часов. Диапазон ограничивайте программно.

Бесколлекторный мотор не имеет щёток, значит, коммутировать фазы должна электроника. Простой вариант — шестишаговая коммутация по датчикам Холла или по обратной ЭДС: дёшево, но с пульсациями момента. Серьёзный — векторное управление (FOC): меряем токи двух фаз, преобразуем в систему координат ротора (Кларк и Парк), держим два ПИ-регулятора на токи, обратно преобразуем в ШИМ. Нужны АЦП, синхронизированный с ШИМ, петля на 10–20 кГц и математика с плавающей точкой — то есть STM32F4/G4 с FPU и CORDIC, а не ESP32, занятый Wi-Fi. Начинать разумно с открытых проектов SimpleFOC (https://simplefoc.com/) и VESC (https://vesc-project.com/).

Замкнутый контур: ПИД, который переживает продакшн

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

typedef struct {
    float kp, ki, kd;
    float integral;                    // накопитель И-составляющей, уже умноженный на ki
    float prev_meas;                   // предыдущее ИЗМЕРЕНИЕ, а не ошибка
    float d_filt;                      // отфильтрованная производная
    float out_min, out_max, d_alpha;   // d_alpha — коэффициент EMA для Д, 0,1 разумно
} pid_t;

float pid_update(pid_t *p, float setpoint, float meas, float dt)
{
    float err = setpoint - meas;
    // (1) Производная по ИЗМЕРЕНИЮ с обратным знаком, а не по ошибке: иначе скачок уставки
    // даёт бесконечный всплеск ("derivative kick") и рывок мотора.
    float d_raw = -(meas - p->prev_meas) / dt;
    p->prev_meas = meas;
    // (2) Д-составляющая ОБЯЗАТЕЛЬНО фильтруется: она усиливает шум датчика в 1/dt раз.
    // Без фильтра kd больше нуля в реальной системе просто не работает.
    p->d_filt += p->d_alpha * (d_raw - p->d_filt);
    // (3) Интегратор с ограничением ДО насыщения — clamping anti-windup.
    p->integral += p->ki * err * dt;
    if (p->integral > p->out_max) p->integral = p->out_max;
    if (p->integral < p->out_min) p->integral = p->out_min;

    float out = p->kp * err + p->integral + p->kd * p->d_filt;
    // (4) Насыщение выхода — физическая реальность: больше напряжения питания не выдать.
    if (out > p->out_max) out = p->out_max;
    if (out < p->out_min) out = p->out_min;
    return out;
}

Про windup стоит сказать отдельно — это самая частая ошибка самодельных регуляторов. Пока привод в насыщении (мотор на полной мощности, а цель не достигнута — например, робот упёрся в стену), ошибка не уменьшается, а интеграл продолжает расти. Уберите препятствие — и накопленный интеграл погонит мотор на полной мощности далеко за уставку. Внешне это выглядит как «регулятор сошёл с ума через минуту работы».

Второе, что убивает регуляторы, — нерегулярный шаг. Формулы ПИД предполагают постоянный $\Delta t$; если вызывать pid_update из главного цикла, где иногда случается запись во flash на 5 мс, коэффициенты фактически плавают. Правильно — вызывать из задачи с xTaskDelayUntil или прямо из прерывания таймера, как разобрано в статьях про прерывания и RTOS.

Ось привода как машина состояний

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

Три перехода в Fault дают основную часть надёжности. Ошибка слежения (расхождение уставки и измерения больше порога) ловит и заклинившую механику, и оборванный энкодер, и неверно подобранные коэффициенты — один сторож на три класса отказов. Пропадание сигнала энкодера обязано быть отдельной проверкой: при обрыве провода регулятор видит «скорость ноль», честно даёт полную мощность, и мотор разгоняется до разрушения. Автосброса из Fault быть не должно никогда: механизм, оживающий сам после аварии, травмоопасен, и это записано во всех нормах по функциональной безопасности.

Электрические реалии, которые ломают код

Мотор убивает I2C. Всё работает на столе, а с включённым мотором датчик возвращает мусор или шина зависает. Причина — импульсные токи через общую землю и наводки на длинные SDA/SCL. Лечится разделением земель (силовая и сигнальная соединяются в одной точке у источника), витой парой или экраном, керамикой 100 нФ прямо на выводах датчика, в тяжёлых случаях — оптронной или цифровой развязкой драйвера. И обязательно таймаутом в драйвере: зависшую шину надо уметь расклинивать девятью тактами SCL.

Просадка питания при пуске. Пусковой ток в 5–8 раз выше номинального, внутреннее сопротивление батареи и проводов даёт падение напряжения, и контроллер уходит в сброс ровно в момент старта движения. Симптом «робот перезагружается, когда трогается» — почти всегда это. Лечится электролитом 470–1000 мкФ у драйвера, отдельным стабилизатором для логики и рампой разгона вместо ступеньки.

Земля датчика тока. Шунт измеряет ток по падению напряжения на нём, АЦП — относительно своей земли. Если между ними течёт силовой ток, разность потенциалов складывается с измеряемым сигналом. Отсюда правило Kelvin-соединения: измерительные провода идут непосредственно к выводам шунта, а не к силовой дорожке. Всё это часть бюджета питания из статьи про питание и ограничения.

Что даёт каждая плата

Отсюда разделение, определяющее архитектуру почти любого реального робота: микроконтроллер держит контур с гарантированным периодом, одноплатник думает. Между ними — UART, CAN или USB с потоком отфильтрованных величин и команд. Подробно это в следующей статье, а на портале тема продолжена в разделе НИР, где роботизированная система рассматривается целиком как распределённая: определение и классификация роботизированных систем, теоретическое обоснование микросервисной архитектуры для роботизированных систем и программная реализация сервисов. Там видно, во что превращается пара «датчик — привод», когда над ней вырастает архитектура из десятков узлов.

Отладка без единого принта

printf в контуре управления вреден дважды: занимает миллисекунды и меняет тайминг, который вы отлаживаете. Чем пользуются вместо него.

Осциллограф — единственный способ увидеть аналоговый сигнал до АЦП. Если показания скачут, вопрос ровно один: скачет ли само напряжение. Щуп на вход АЦП отвечает за пять секунд и делит задачу пополам — чинить схему или чинить код. Там же видны наводка 50 Гц (синус, синхронный с сетью), звон от мотора (пачки высокочастотных колебаний в моменты переключения ШИМ) и недозаряд конденсатора хранения (ступенька в момент выборки).

Логический анализатор — для цифровых датчиков. PulseView с декодером I2C показывает адрес, регистр и байты ответа. NACK на адрес — датчик не припаян или адрес другой. Всегда 0xFF — питание есть, инициализация не прошла. Минуты вместо часов чтения кода.

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

ЦАП или ШИМ как «принтер» аналоговых величин. Вывести внутреннюю переменную регулятора на ЦАП STM32 и смотреть осциллографом — способ увидеть переходный процесс целиком, а не по точкам. SWD и живые переменные: отладчик Cortex-M читает память через порт доступа, не останавливая ядро, — Ozone, STM32CubeMonitor и live watch строят графики на работающей системе. Для потока телеметрии — SEGGER RTT: запись в кольцевой буфер в ОЗУ стоит около 1 мкс против сотен микросекунд у UART.

"""Настройка регулятора: плата шлёт CSV по UART, скрипт считает показатели.
Правильное разделение — МК только пишет строку, вся аналитика на ноутбуке."""
import serial, numpy as np

rows = []
with serial.Serial("/dev/ttyUSB0", 921600, timeout=1) as ser:
    ser.write(b"STEP 500\n")                       # подаём ступеньку уставки
    for _ in range(4000):                          # 4000 строк по 5 мс = 20 секунд
        p = ser.readline().decode(errors="ignore").strip().split(",")
        if len(p) == 4:                            # t_us, setpoint, measured, output
            rows.append([float(v) for v in p])

d = np.array(rows); dt = np.diff(d[:, 0]); t = (d[:, 0] - d[0, 0]) / 1e6
# Джиттер периода проверяют ПЕРВЫМ: настраивать коэффициенты на плавающем шаге бессмысленно.
print(f"период: медиана {np.median(dt):.0f} мкс, max {dt.max():.0f} мкс, "
      f"джиттер p99 {np.percentile(dt, 99) - np.median(dt):.0f} мкс")
target = d[-1, 1]
band = np.where(np.abs(d[:, 2] - target) > 0.02 * target)[0]
print(f"перерегулирование {(d[:, 2].max() - target) / target * 100:.1f} %, "
      f"время установления {t[band[-1]]:.3f} с")

Инструментальная сторона подробнее — в статье про отладку и производство, а привычка измерять вместо угадывания — в треке производительности.

Типичные ошибки

• analogRead() в цикле управления. На AVR это 110 мкс, на ESP32 — непредсказуемое время из-за конкуренции с Wi-Fi. Правильно: таймер запускает АЦП, DMA складывает результат, задача просыпается на готовых данных.
• Цифровой фильтр вместо антиалиасингового RC. Фильтр не восстановит того, чего нет в отсчётах. RC-цепочка стоит два компонента и решает проблему навсегда.
• Калибровка по одной точке при большой ошибке усиления. Убрали смещение, оставили наклон — и на другом конце диапазона те же проценты ошибки.
• Чтение многобайтового значения по одному регистру. Разрыв между старшим и младшим байтом даёт редкие абсурдные значения — худший класс ошибок, потому что воспроизводится раз в час.
• ПИД с производной по ошибке и без фильтра, плюс интегратор без ограничения. Каждое изменение уставки — рывок, каждый шум датчика — дребезг привода, а после первого упора в препятствие регулятор «сходит с ума».
• ШИМ мотора на 1–2 кГц. Работает, но пищит; заказчик слышит это раньше, чем вы измеряете КПД.
• Задержка в коде вместо аппаратного мёртвого времени. Сквозной ток убивает мост за микросекунды, а __NOP() не синхронизирован с ШИМ.
• Питание серво и логики от одного стабилизатора. Пусковой ток сажает шину, контроллер уходит в сброс.
• Нет сторожа на обрыв датчика обратной связи. Регулятор видит ноль и честно даёт полную мощность — самый разрушительный сценарий из всех.
• Пересчёт в физические единицы на каждом шаге ISR. Держите сырые целые в быстром тракте, переводите в градусы и метры на границе — там, где данные покидают контур.

Мини-итог

Датчик не даёт значение, он даёт свидетельство, испорченное на каждой из пяти ступеней тракта. Работа с ним раскладывается на три независимые задачи: схемотехника (ratiometric-подключение, время выборки, антиалиасинговый RC — то, что нельзя починить программой), калибровка (аффинная модель, полином или таблица, хранение с CRC и версией — убирает систематическую ошибку) и фильтрация (медиана против выбросов, EMA против шума, комплементарный и Калман, когда есть дополнительная информация — убирает случайную ошибку и всегда платит задержкой).

Привод — тот же тракт наоборот, и там ждут ток, индуктивность и тепло. Мотор не подключается к GPIO; ток индуктивности не прерывается мгновенно; сквозной ток убивает мост за микросекунды; ШИМ ниже 20 кГц слышно; шаговик пропускает шаги молча. Между датчиком и приводом стоит регулятор, и в реальной системе он обязан иметь ограниченный интегратор, фильтрованную производную по измерению, строго постоянный шаг и явную машину состояний с аварией, из которой нет автоматического выхода.

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

Источники

• Elecia White. Making Embedded Systems, O’Reilly — https://www.oreilly.com/library/view/making-embedded-systems/9781449308889/
• Steven W. Smith. The Scientist and Engineer’s Guide to Digital Signal Processing (полный текст бесплатно) — https://www.dspguide.com/
• Greg Welch, Gary Bishop. An Introduction to the Kalman Filter — https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf
• Roger Labbe. Kalman and Bayesian Filters in Python — https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
• Analog Devices. MT-003, Understand SINAD, ENOB, SNR, THD — https://www.analog.com/media/en/training-seminars/tutorials/MT-003.pdf
• Silicon Labs. AN118, Improving ADC Resolution by Oversampling and Averaging — https://www.silabs.com/documents/public/application-notes/an118.pdf
• STMicroelectronics. AN2834, How to get the best ADC accuracy in STM32 — https://www.st.com/resource/en/application_note/an2834-how-to-get-the-best-adc-accuracy-in-stm32-microcontrollers-stmicroelectronics.pdf
• David Austin. Generate stepper-motor speed profiles in real time — https://www.embedded.com/generate-stepper-motor-speed-profiles-in-real-time/
• Brett Beauregard. Improving the Beginner’s PID — про kick, windup и переключение режимов — http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
• Karl Åström, Tore Hägglund. PID Controllers: Theory, Design and Tuning — https://www.isa.org/products/pid-controllers-theory-design-and-tuning-2nd-editio
• Bosch Sensortec. BME280 datasheet — образец хорошей карты регистров и компенсации — https://www.bosch-sensortec.com/products/environmental-sensors/humidity-sensors-bme280/
• TDK InvenSense. ICM-42688-P datasheet — https://invensense.tdk.com/products/motion-tracking/6-axis/icm-42688-p/
• SimpleFOC — открытая библиотека векторного управления BLDC — https://simplefoc.com/ ; Trinamic TMC2209, микрошаг и StallGuard — https://www.analog.com/en/products/tmc2209.html
• sigrok и PulseView — https://sigrok.org/wiki/PulseView ; SEGGER RTT — https://www.segger.com/products/debug-probes/j-link/technology/about-real-time-transfer/

Что дальше

Робототехника: кинематика, управление, ROS, обратная связь — соберём датчики и приводы из этой статьи в машину, которая целенаправленно двигается: как из отсчётов энкодеров получается положение робота на плоскости (одометрия и её накапливающаяся ошибка), почему прямая кинематика проста, а обратная имеет несколько решений или ни одного, как контуры тока, скорости и положения вкладываются друг в друга, что даёт ROS 2 поверх всего этого и где проходит граница между микроконтроллером с жёстким периодом и Linux-узлом, который может себе позволить подумать десять миллисекунд.