99д. Гироскоп-акселерометр MPU-6050 по I2C: регистры, FIFO, прерывание и калибровка смещений

В разделе 99 MPU-6050 будили, читали четырнадцатью байтами сто раз в секунду и складывали угол комплементарным фильтром. В 99а тот же класс датчиков работает по SPI на 8 кГц в Betaflight, в 99б из гироскопа, акселерометра и магнитометра собирается ориентация фильтром Маджвика. Здесь — сам драйвер микросхемы: как найти её на шине, разбудить и не усыпить снова, как два регистра задают частоту выборок, что даёт встроенный фильтр, зачем FIFO и как он ломается при переполнении, как читать по прерыванию «данные готовы» через DMA и как убрать смещение нуля гироскопа — программно или записью в его собственные регистры.

Опоры: RM0008 (DocID13902 Rev 17): приём трёх байт и больше ведущим, способ 2, с. 763; одного и двух байт — с. 764; приём через DMA с битом LAST, с. 769; биты LAST и DMAEN в I2C_CR2, с. 775; таблица 78 — I2C1_RX на канале 7 DMA1, с. 281; быстрый режим — с. 783–784; AFIO_EXTICR2, с. 190; EXTI_RTSR, с. 211; таблица режимов выводов, с. 160. DocID13587 Rev 17 (STM32F103x8/xB): PB6 — I2C1_SCL, PB7 — I2C1_SDA, оба FT, PB5 без FT, с. 32. Даташита и карты регистров MPU-6050 в проекте нет. Номера регистров, биты и задержки — по драйверам с открытым кодом: Betaflight 2026.12.0-alpha (коммит 744f95f, accgyro_mpu.h, accgyro_mpu.c, accgyro_mpu6050.c, accgyro_spi_mpu6000.c, GPL-3.0), Zephyr 4.4.2 (mpu6050.h, mpu6050.c, Apache-2.0), VESC (коммит 4fd8279, mpu9150.h — MPU-9150 это MPU-6050 и магнитометр в одном корпусе, GPL-3.0) и Klipper (коммит 7bc4d09, mpu9250.py, sensor_mpu9250.c, GPL-3.0). Объём FIFO 1024 байта, порядок величин в кадре FIFO, что происходит при переполнении, единица регистров XG_OFFS_USR и форма встроенного фильтра — по общим сведениям, не проверено: модель их повторяет, а не проверяет. Все числа — прогон драйвера _verify/imu99f/mpu99f.c, собранного arm-none-eabi-gcc 12.2.1 под Cortex-M3 и запущенного в QEMU 7.2 (машина mps2-an385), со своей моделью MPU-6050 на Python 3.11; прошивка собрана armclang 6.24. Смещение гироскопа, шум и движение платы — допущения: числа ниже говорят о модели, а не о датчике.

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

Оригинал	Перевод
WHO_AM_I	регистр «кто я»: код модели
PWR_MGMT_1, SLEEP, DEVICE_RESET, CLKSEL	управление питанием: сон, полный сброс, источник такта
SIGNAL_PATH_RESET	сброс тракта измерения
DLPF_CFG (digital low pass filter)	встроенный цифровой фильтр нижних частот
SMPLRT_DIV (sample rate divider)	делитель частоты выборок
gyroscope output rate	частота, с которой гироскоп выдаёт отсчёты внутрь
FS_SEL, AFS_SEL (full scale)	диапазон гироскопа и акселерометра
FIFO_EN, USER_CTRL, FIFO_COUNT, FIFO_R_W	что писать в FIFO, его включение и сброс, счётчик байт, окно чтения
FIFO_OFLOW	FIFO переполнен
DATA_RDY, INT_PIN_CFG, LATCH_INT_EN	данные готовы, настройка вывода INT, защёлка
XG_OFFS_USR	пользовательское смещение гироскопа по X
bias, zero-rate offset	смещение нуля

99д.1 Адрес, код модели, сон и такт от гироскопа

Модуль с MPU-6050 подключают так:

Адрес задаёт вывод AD0: на земле — 0x68, на питании — 0x69 (MPU9150_ADDRESS_AD0_LOW и _HIGH в VESC). Код в WHO_AM_I (0x75) от AD0 не зависит: Betaflight, когда ему задан адрес 0x69, всё равно ждёт 0x68. Модули с похожими микросхемами отвечают иначе: MPU-6500 — 0x70, MPU-9250 — 0x71 (так в Betaflight и Zephyr). У них свои драйверы в Betaflight, а в Zephyr у MPU-6500 своя формула температуры; драйвер этого раздела рассчитан только на MPU-6050 и их отвергает:

/* кто на шине: 0x68 (AD0 = 0) или 0x69 (AD0 = 1); WHO_AM_I у MPU-6050 — 0x68 при любом AD0 */
int mpu_probe(mpu_t *d)
{
    static const uint8_t addr[2] = {MPU_ADDR_AD0_LO, MPU_ADDR_AD0_HI};
    uint8_t who;
    d->who = 0;
    for (int i = 0; i < 2; i++) {
        d->addr = addr[i];
        if (xfer_done(d, mpu_port_xfer(d->addr, 0, 0, 0, 0)) != MPU_OK)
            continue;                                           /* нет ответа на этот адрес */
        if (mpu_read_regs(d, MPU_WHO_AM_I, &who, 1) != MPU_OK)
            return MPU_E_BUS;
        d->who = who;
        return who == MPU_WHO_6050 ? MPU_OK : MPU_E_ID;         /* 0x70 — MPU-6500, 0x71 — MPU-9250: другие карты */
    }
    return MPU_E_NACK;
}

В модели на плате с AD0 на земле опознание — 2 обмена, с AD0 на питании — 3: сначала NACK по 0x68. Плата с кодом 0x70 даёт MPU_E_ID, −3; пустая шина — MPU_E_NACK, −1.

Сон. После включения MPU-6050 спит: в PWR_MGMT_1 (0x6B) стоит бит 6, SLEEP (MPU6050_SLEEP_EN в Zephyr; 0x40 — значение SET_PWR_MGMT_1_SLEEP в Klipper). В модели после включения регистр читается как 0x40, четырнадцать байт данных — нули, а ожидание готовности кончается через 395 мс кодом MPU_E_TIMEOUT, −4.

Сброс. Бит 7 того же регистра, DEVICE_RESET, возвращает все регистры к начальным, а SIGNAL_PATH_RESET (0x68), биты 2, 1 и 0, — тракты гироскопа, акселерометра и термометра. После каждого Betaflight ждёт по 100 мс со ссылкой на документацию:

/* полный сброс: все регистры — по умолчанию, и микросхема снова спит */
int mpu_reset(mpu_t *d)
{
    int rc;
    if ((rc = mpu_write_reg(d, MPU_PWR_MGMT_1, MPU_PWR1_RESET)) != MPU_OK)
        return rc;
    mpu_port_delay_us(MPU_RESET_MS * 1000u);
    if ((rc = mpu_write_reg(d, MPU_SIGNAL_PATH_RST, MPU_SPR_ALL)) != MPU_OK)
        return rc;
    mpu_port_delay_us(MPU_RESET_MS * 1000u);
    return MPU_OK;
}

В модели сброс — записи 6B 80 и 68 07 и 200 мс. После него PWR_MGMT_1 — снова 0x40: микросхема опять спит. Сброс после настройки стирает и настройку, и пробуждение — данные пропадают, ожидание снова кончается кодом −4 через 395 мс.

Такт. Разбудить — записать в PWR_MGMT_1 ноль в бит 6. Заодно выбирают источник такта CLKSEL (биты 2:0): 1 — генератор с опорой на гироскоп X, 3 — на гироскоп Z (MPU_CLK_SEL_PLLGYROX и _PLLGYROZ в Betaflight; сам Betaflight для MPU-6050 пишет 0x03). Генератору после смены CLKSEL нужно время — в комментарии Betaflight «не больше 10 мс»:

/* разбудить с тактом от гироскопа X, частота, фильтр, диапазоны, INT по готовности данных (держится до чтения статуса) */
int mpu_init(mpu_t *d, uint8_t dlpf, uint8_t div, uint8_t fs, uint8_t afs)
{
    uint8_t v;
    int rc;
    if (dlpf > 6u || fs > 3u || afs > 3u)
        return MPU_E_ARG;
    if ((rc = mpu_write_reg(d, MPU_PWR_MGMT_1, MPU_CLK_PLL_XGYRO)) != MPU_OK)    /* SLEEP = 0 */
        return rc;
    mpu_port_delay_us(MPU_PLL_MS * 1000u);
    if ((rc = mpu_read_regs(d, MPU_PWR_MGMT_1, &v, 1)) != MPU_OK)
        return rc;
    if (v & MPU_PWR1_SLEEP)
        return MPU_E_SLEEP;                                     /* запись не дошла: микросхема спит и даёт нули */
    const uint8_t cfg[][2] = {{MPU_SMPLRT_DIV, div}, {MPU_CONFIG, dlpf}, {MPU_GYRO_CONFIG, (uint8_t)(fs << 3)},
                              {MPU_ACCEL_CONFIG, (uint8_t)(afs << 3)}, {MPU_INT_PIN_CFG, MPU_INTCFG_LATCH},
                              {MPU_INT_ENABLE, MPU_INT_DATA_RDY}};
    for (uint32_t i = 0; i < sizeof cfg / sizeof cfg[0]; i++)
        if ((rc = mpu_write_reg(d, cfg[i][0], cfg[i][1])) != MPU_OK)
            return rc;
    d->dlpf = dlpf;
    d->div = div;
    d->fs = fs;
    d->afs = afs;
    return MPU_OK;
}

Драйвер читает PWR_MGMT_1 обратно: если бит сна остался, запись не дошла, и он возвращает MPU_E_SLEEP, а не тихие нули. В модели настройка на 200 выборок в секунду — записи 6B 01, 19 04, 1A 03, 1B 00, 1C 00, 37 20, 38 01, чтение 6B → 01, 11 мс вместе с ожиданием генератора.

99д.2 Частота выборок: DLPF_CFG и SMPLRT_DIV

Два регистра решают, как часто обновляются данные. CONFIG (0x1A), биты 2:0 — DLPF_CFG, встроенный фильтр. Имена его кодов в Betaflight называют полосу: 0 — 256 Гц, 1 — 188, 2 — 98, 3 — 42, 4 — 20, 5 — 10, 6 — 5 Гц, 7 — 2100HZ_NOLPF, без фильтра. От фильтра зависит, с какой частотой гироскоп отдаёт отсчёты внутрь: при DLPF_CFG = 0 — 8 кГц (MPU6050_SMPLRT_DIV 0 // 8000Hz в Betaflight), с фильтром — 1 кГц («Gyroscope Output Rate = 1kHz when the DLPF is enabled»). SMPLRT_DIV (0x19) делит её: частота выборок = частота гироскопа / (1 + SMPLRT_DIV).

/* частота выборок в тысячных герца: гироскоп — 8 кГц без фильтра (DLPF_CFG = 0), 1 кГц с ним; делится на 1 + SMPLRT_DIV */
uint32_t mpu_rate_mhz(uint8_t dlpf, uint8_t div)
{
    const uint32_t base = (dlpf == 0u || dlpf == 7u) ? 8000000u : 1000000u;
    return base / (1u + div);
}

Код 7 драйвер не принимает: что он даёт на MPU-6050, по документам на диске не проверить. Модель считает выборки по времени их появления, а драйвер — по формуле:

Акселерометр обновляется 1000 раз в секунду («Accel Sample Rate 1kHz» в Betaflight). При 8 кГц он отстаёт от гироскопа: в модели из 32 кадров FIFO подряд ускорение меняется через каждые 8.

Фильтр нужен не только против шума. Выборки берутся реже, чем гироскоп видит вибрацию, и всё, что выше половины частоты выборок, складывается в низкие частоты. В модели плату трясёт на 205 Гц с размахом ±20 °/с по X, выборок — 100 в секунду; 205 Гц перекладываются в 5 Гц — медленную качку, которой нет. Встроенный фильтр в модели — звено первого порядка с полосой из имени кода (настоящая его форма не проверена), СКО на выходе из 100 выборок при 14,14 °/с на входе:

С кодом 0 (полоса 256 Гц, 8 кГц, делитель 79) вибрация почти вся проходит в выборки. Полоса 42 Гц — уже 3,05 °/с, 10 Гц — 0,74. Полоса фильтра должна быть ниже половины частоты выборок; цена — запаздывание.

99д.3 Диапазоны, цена разряда, температура и пачка из 14 байт

Диапазоны — GYRO_CONFIG (0x1B) и ACCEL_CONFIG (0x1C), биты 4:3; таблицы — в 99.2. Драйвер переводит в целые: тысячные °/с через ту же таблицу 1310, 655, 328, 164 (кодов на °/с, умноженных на 10), что в Zephyr, тысячные g — делением на 16384 >> AFS_SEL:

Плату в модели вращают со скоростью 300 °/с по Z: на ±250 °/с код упирается в 32767, и драйвер видит 250,13 °/с. У акселерометра при ±2 g разряд — 0,061 мg: в целых тысячных g код 1 даёт 0, для наклона это неважно, для вибрации — берут меньшую единицу.

Температура. Код / 340 + 36,53 °C — так в Betaflight (tempScale = 1.0f / 340.0f, tempZero = 36.53f) и в 99.3:

/* температура кристалла: код / 340 + 36,53 °C */
int32_t mpu_temp_cdeg(int32_t raw)
{
    return (int32_t)rdiv((int64_t)raw * 100, 340) + 3653;
}

Zephyr 4.4.2 прибавляет 36, а не 36,53: при кристалле 30,00 °C в модели код −2220, драйвер даёт 30,00 °C, формула Zephyr — 29,47 °C. Разница 0,53 °C — на постоянное смещение; это температура кристалла, а не воздуха (99.8).

Пачка. Ускорение X, Y, Z, температура и гироскоп X, Y, Z лежат подряд с 0x3B по 0x48, каждое значение — старшим байтом вперёд. Читать их надо одной пачкой: номер регистра 0x3B, повторный старт, 14 байт.

/* 14 байт с 0x3B: ускорение X, Y, Z, температура, гироскоп X, Y, Z */
void mpu_parse14(const uint8_t b[14], mpu_raw_t *r)
{
    for (int k = 0; k < 3; k++) {
        r->a[k] = mpu_be16(b + 2 * k);
        r->g[k] = mpu_be16(b + 8 + 2 * k);
    }
    r->t = mpu_be16(b + 6);
}

В модели плата вращается со скоростью 10, −20 и 30 °/с; пачка — 00 3D FF F2 3F F3 F7 54 05 2E F5 CB 0F 69: ускорение 61, −14, 16371, температура −2220, гироскоп 1326, −2613, 3945 — это 10,12, −19,95, 30,11 °/с.

Время на шине: опрос INT_STATUS — 39 тактов SCL, 14 байт — 156. Опрос и данные вместе на 400 кГц — 487,50 мкс, на 100 кГц — 1950,00. Если начинать чтение сразу с INT_STATUS (0x3A) и брать 15 байт, — 165 тактов, 412,50 мкс на 400 кГц и 1650,00 на 100 кГц: по одной выборке за чтение шина пропустит не больше 2424 выборок в секунду на 400 кГц и 606 на 100 кГц. Чаще — только через FIFO.

99д.4 FIFO: включение, счётчик, переполнение и восстановление

FIFO копит выборки внутри микросхемы, и читать можно пачками, реже частоты выборок. Что класть — FIFO_EN (0x23): бит 7 — температура, 6, 5, 4 — гироскоп X, Y, Z, 3 — ускорение (VESC). Включает FIFO бит 6 USER_CTRL (0x6A), сбрасывает — бит 2. Порядок запуска — как в Klipper: остановить запись, сбросить, включить, прочитать INT_STATUS, чтобы снять флаг переполнения, пустить запись:

/* остановить запись, сбросить FIFO, включить его, снять флаг переполнения чтением статуса, пустить запись */
int mpu_fifo_start(mpu_t *d, uint8_t mask)
{
    uint8_t st;
    int rc;
    d->fifo_mask = mask;
    d->frame = frame_bytes(mask);
    if (!d->frame)
        return MPU_E_ARG;
    if ((rc = mpu_write_reg(d, MPU_FIFO_EN, 0u)) != MPU_OK || (rc = mpu_write_reg(d, MPU_USER_CTRL, MPU_USER_FIFO_RST)) != MPU_OK
        || (rc = mpu_write_reg(d, MPU_USER_CTRL, MPU_USER_FIFO_EN)) != MPU_OK || (rc = mpu_read_regs(d, MPU_INT_STATUS, &st, 1)) != MPU_OK)
        return rc;
    return mpu_write_reg(d, MPU_FIFO_EN, mask);
}

Счётчик байт — FIFO_COUNTH и FIFO_COUNTL (0x72, 0x73), от старшего байта Klipper берёт пять бит (& 0x1f). Сами байты читают из FIFO_R_W (0x74) пачкой: указатель регистра на нём стоит, и каждый байт пачки — следующий из FIFO.

Кадр ускорение + гироскоп — 12 байт: величины в модели идут по возрастанию номеров регистров, как в пачке с 0x3B, но без температуры. На 1 кГц через 50 мс в FIFO 600 байт. FIFO на 1024 байта переполняется через 85,62 мс, и 1024 на 12 не делится — остаток 4. Модель выбрасывает самые старые байты, и начало FIFO оказывается посреди кадра. Кто читает кадры, не глядя на флаг, получает такое: ускорение −122, 74, −43, гироскоп 18, 16348, 245. Ускорение Z, около 16384, переехало в гироскоп Y.

Признаков переполнения два: бит 4 INT_STATUS, FIFO_OFLOW (Klipper проверяет его же, FIFO_OVERFLOW_INT 0x10), и счётчик, не кратный кадру. Драйвер проверяет оба и восстанавливается одним способом — сбросом:

/* до max кадров из FIFO. Переполнение или счёт не кратен кадру — граница кадров потеряна: сброс FIFO и ошибка */
int mpu_fifo_read(mpu_t *d, mpu_raw_t *out, uint32_t max, uint32_t *got)
{
    uint8_t st, b[MPU_FIFO_CHUNK * 14u];
    uint16_t cnt;
    uint32_t n;
    int rc;
    *got = 0;
    if ((rc = mpu_read_regs(d, MPU_INT_STATUS, &st, 1)) != MPU_OK || (rc = mpu_fifo_count(d, &cnt)) != MPU_OK)
        return rc;
    if ((st & MPU_INT_FIFO_OFLOW) || cnt % d->frame) {
        d->overflows++;
        d->lost += cnt / d->frame;                              /* то, что лежало, уже со сдвигом — выбросить */
        if ((rc = mpu_fifo_start(d, d->fifo_mask)) != MPU_OK)
            return rc;
        return (st & MPU_INT_FIFO_OFLOW) ? MPU_E_OFLOW : MPU_E_ALIGN;
    }
    n = cnt / d->frame;
    if (n > max)
        n = max;
    while (*got < n) {
        uint32_t k = n - *got > MPU_FIFO_CHUNK ? MPU_FIFO_CHUNK : n - *got;
        if ((rc = mpu_read_regs(d, MPU_FIFO_R_W, b, k * d->frame)) != MPU_OK)
            return rc;
        for (uint32_t i = 0; i < k; i++)
            mpu_fifo_parse(d->fifo_mask, b + i * d->frame, &out[*got + i]);
        *got += k;
    }
    return MPU_OK;
}

В модели после переполнения mpu_fifo_read вернул MPU_E_OFLOW, −6, и сбросил FIFO; через 5 мс — 5 кадров, первый — ускорение 77, −13, 16389, гироскоп 240, −114, 78: снова на своих местах. Выброшено 84 кадра — то, что лежало в FIFO со сдвигом. Кадр из одного гироскопа — 6 байт, и FIFO на 1 кГц переполняется вдвое позже, через 170,74 мс.

Успевает ли шина. На 1 кГц кадр 12 байт драйвер читает по 8 за обмен: 894 такта SCL, то есть 1117,50 мкс на кадр на 100 кГц — больше миллисекунды, за которую приходит следующий. За 3000 кадров FIFO переполнился 5 раз, выброшено 425 кадров. На 400 кГц — 279,38 мкс на кадр и ни одного переполнения. Кадр 6 байт — 462 такта, 577,50 мкс на 100 кГц: тут хватает и стандартного режима.

99д.5 Прерывание «данные готовы» и чтение через DMA

Опрос INT_STATUS занимает шину впустую. Вывод INT говорит о готовности сам: INT_ENABLE (0x38) = 0x01, DATA_RDY_EN. Нули в битах 7 и 6 INT_PIN_CFG — двухтактный выход, активный высоким уровнем (INTMODE_ACTIVEHIGH и INTDRV_PUSHPULL в VESC, INT_LEVEL_HIGH, INT_OPEN_DIS в комментарии Betaflight), и без защёлки INT — импульс 50 мкс (MPU9150_INTLATCH_50USPULSE). С LATCH_INT_EN (бит 5 INT_PIN_CFG, 0x37) INT держится, пока не прочитан INT_STATUS (INTCLEAR_STATUSREAD). Драйвер ставит защёлку, а чтение начинает с 0x3A: 15 байт — статус и данные, и это же чтение отпускает INT. В модели статус в такой пачке — 0x01, а следующее чтение 0x3A даёт 0x00.

На F103 INT — на PB5: вход с подтяжкой к земле (код 1000 в таблице режимов, с. 160, ODR = 0), AFIO_EXTICR2, биты 7:4 = 0001 — порт B (с. 190), фронт включает EXTI_RTSR (с. 211). Приёмник I2C1 обслуживает канал 7 DMA1 (таблица 78, с. 281). По RM0008 (с. 769) при приёме ведущим DMA выдаёт сигнал конца на предпоследнем байте, бит LAST в I2C_CR2 (с. 775) заставляет I2C ответить NACK после последнего, а STOP ставят в прерывании по концу передачи. DMAEN можно поставить и в событии ADDR, до его снятия:

/* INT: регистр 0x3A, повторный старт, адрес чтения — опросом; 15 байт — DMA с LAST: NACK на последнем (с. 769) */
void EXTI9_5_IRQHandler(void)
{
    static const uint8_t reg = MPU_INT_STATUS;
    EXTI->PR = 1u << 5;
    if (busy) {
        missed++;                                       /* прошлое чтение не кончилось */
        return;
    }
    DMA1_Channel7->CNDTR = sizeof dma_buf;
    DMA1_Channel7->CCR = DMA_CCR7_MINC | DMA_CCR7_TCIE | DMA_CCR7_PL_1 | DMA_CCR7_EN;   /* из периферии в память */
    if (addr_phase(dev.addr, &reg, 1, 1, 0) != MPU_OK) {
        DMA1_Channel7->CCR = 0;
        (void)fail(MPU_E_BUS);
        bus_errors++;
        return;
    }
    I2C1->CR2 |= I2C_CR2_DMAEN | I2C_CR2_LAST;          /* в событии ADDR, до его снятия (с. 769) */
    busy = 1;
    (void)I2C1->SR2;                                    /* ADDR снят: дальше байты идут сами */
}

/* конец DMA: STOP, разбор, поправка смещения, курс по гироскопу Z */
void DMA1_Channel7_IRQHandler(void)
{
    mpu_raw_t r;
    DMA1->IFCR = DMA_IFCR_CTCIF7;
    DMA1_Channel7->CCR = 0;
    I2C1->CR1 |= I2C_CR1_STOP;
    I2C1->CR2 &= (uint16_t)~(I2C_CR2_DMAEN | I2C_CR2_LAST);
    mpu_parse14(dma_buf + 1, &r);
    for (int k = 0; k < 3; k++) {
        gyro_mdps[k] = mpu_gyro_corr_mdps(&dev, r.g[k], k);
        acc_mg[k] = mpu_acc_mg(r.a[k], dev.afs);
    }
    temp_cdeg = mpu_temp_cdeg(r.t);
    int64_t y = yaw_udeg;
    mpu_angle_step(&y, gyro_mdps[2], DT_US);
    yaw_udeg = y;
    samples++;
    busy = 0;
}

Адресная часть — старт, адрес, номер регистра, повторный старт, адрес чтения — около 29 тактов SCL, 72,5 мкс на 400 кГц, и её прерывание ждёт опросом. Остальные 15 байт — 337,5 мкс — идут без процессора. На 200 выборках в секунду это 82,5 мс из каждой секунды на шине. Прерывания и DMA в модели не прогонялись: прошивка fw99f.c только собрана armclang, обработчики на железе не проверены.

У защёлки есть ловушка. Если INT поднялся, а чтение не состоялось — например, прерывание включили, когда INT уже стоял, — фронта больше не будет: INT ждёт чтения, чтение ждёт фронта. Прошивка после включения EXTI смотрит на PB5 и, если там единица, запускает первое чтение сама (EXTI_SWIER).

99д.6 Калибровка смещения гироскопа

Смещение нуля — то, что гироскоп показывает в покое. В модели оно 1,80, −0,90 и 0,60 °/с, шум — 0,05 °/с. Драйвер усредняет n выборок и заодно смотрит размах: если по любой оси он больше 1 °/с (131 код при ±250 °/с), прибор двигали, и результат — MPU_E_MOVED, −8. Смещение хранится в кодах × 16, чтобы не потерять доли разряда при вычитании.

Тысячи выборок хватает: остаток — единицы тысячных °/с. Толчок со скоростью 30 °/с на 50 мс во время калибровки дал размах 3965 кодов — отказ. А медленный поворот со скоростью 2 °/с размах не меняет — 41 при пороге 131, — и смещение по Z вышло 2,60 °/с вместо 0,60: постоянное вращение от смещения не отличить. Калибруют на неподвижной опоре, а не в руке.

В регистры микросхемы. Регистры с 0x13 по 0x18 — XG_OFFS_USR, YG_OFFS_USR, ZG_OFFS_USR, по 16 бит старшим вперёд (Betaflight, MPU_RA_XG_OFFS_USRH). Значение складывается с выходом гироскопа внутри, и вычитать в программе становится нечего. Единицу этих регистров документы на диске не называют; по общим сведениям — разряд диапазона ±1000 °/с, то есть 4 кода при ±250. Поэтому драйвер не верит на слово: пишет, перемеряет и, если осталось больше 0,1 °/с, обнуляет регистры и вычитает сам:

/* Смещение гироскопа: среднее в покое. use_hw — записать его в XG_OFFS_USR в предположении «1 код регистра = 4 кода
 * выхода при ±250 °/с» и перемерить: остаток больше 0,1 °/с — единица не та, регистры обнулить и вычитать программно */
int mpu_cal_gyro(mpu_t *d, uint32_t n, int use_hw)
{
    int32_t b[3], s[3], r[3];
    int16_t off[3] = {0, 0, 0};
    int rc;
    d->cal = 0;
    for (int k = 0; k < 3; k++)
        d->bias_q4[k] = 0;
    if ((rc = mpu_set_gyro_offs(d, off)) != MPU_OK || (rc = mpu_gyro_bias(d, n, b, s)) != MPU_OK)
        return rc;
    for (int k = 0; k < 3; k++)
        d->bias_q4[k] = b[k];
    d->cal = MPU_CAL_SW;
    if (!use_hw)
        return MPU_CAL_SW;
    for (int k = 0; k < 3; k++)
        off[k] = (int16_t)-rdiv((int64_t)b[k] * (1 << d->fs), 64);
    if ((rc = mpu_set_gyro_offs(d, off)) != MPU_OK || (rc = mpu_gyro_bias(d, n, r, s)) != MPU_OK)
        return rc;
    for (int k = 0; k < 3; k++) {
        if (r[k] > 16 * mdps_to_raw(MPU_HW_RESID_MDPS, d->fs) || r[k] < -16 * mdps_to_raw(MPU_HW_RESID_MDPS, d->fs)) {
            off[0] = off[1] = off[2] = 0;
            rc = mpu_set_gyro_offs(d, off);                     /* регистр понят не так — вычитаем сами */
            return rc != MPU_OK ? rc : MPU_CAL_SW;
        }
    }
    for (int k = 0; k < 3; k++)
        d->bias_q4[k] = r[k];                                   /* остаток доберём программно */
    d->cal = MPU_CAL_HW;
    return MPU_CAL_HW;
}

В модели «4 кода» драйвер записал −59, 29, −20 — это −смещение × 131 / 4 — и остаток не больше 15 тысячных °/с. На ±2000 °/с записано почти то же число: единица регистра от диапазона не зависит. В модели «2 кода» остаток после записи — половина смещения, драйвер это увидел, вернул MPU_CAL_SW и вычитает программно.

Проверка поворотом. Плату поворачивают на 90° вокруг Z за секунду (скорость — приподнятый косинус, до 180,00 °/с), 200 выборок в секунду, кадры ускорение + гироскоп из FIFO, угол — сумма скорость × 5 мс:

Без калибровки за 3 с набегает 1,8° по Z и 5,4° по X — смещение × время. Программная поправка и регистры дают тысячные градуса. Последняя строка — программа один раз «задумалась» на 600 мс: FIFO переполнился, сброс выбросил 85 кадров, и поворот, пришедшийся на паузу, пропал — угол 27,867° вместо 90.

99д.7 Грабли

Нет ответа по 0x68. AD0 на питании: адрес 0x69. Драйвер пробует оба — в модели 3 обмена вместо 2.

Отвечает, но код 0x70 или 0x71. Это MPU-6500 или MPU-9250, а не MPU-6050. Драйвер возвращает MPU_E_ID, −3.

Одни нули. Микросхема спит: после включения и после DEVICE_RESET PWR_MGMT_1 = 0x40. В модели ожидание готовности кончается через 395 мс кодом −4. Сброс — только до настройки, а не после.

Мусор после паузы в чтении FIFO. Переполнение: 1024 байта на кадр не делятся, и кадры съезжают. Проверять FIFO_OFLOW и кратность счётчика, при любом из них — сброс FIFO. Ускорение Z в гироскопе Y — его верный признак.

FIFO переполняется сам по себе. Шина не успевает: на 100 кГц кадр 12 байт стоит 1117,50 мкс при периоде 1000. Шину — на 400 кГц (CCR = F/S | 30, TRISE = 11, 15б.2), кадр — короче, или частоту ниже.

На 400 кГц сбоит. Подтяжки слабые: в быстром режиме фронт не длиннее 300 нс, и при ёмкости линии 400 пФ подтяжка нужна не больше 885 Ом (17.3). Короткая шина на плате — десятки пикофарад; длинные провода к модулю — повод опустить частоту. PB6 и PB7 — FT (с. 32), PB5 — нет: INT модуля с питанием 3,3 В ему подходит.

Прерывания перестали приходить. INT в защёлке ждёт чтения INT_STATUS, а чтение — фронта. Начинать чтение самим, если INT уже поднят (99д.5).

Угол плывёт после калибровки. Калибровали в руке или на вращающемся столе: в модели 2 °/с вращения стали смещением. Или температура ушла — смещение от неё зависит (99.6).

Скорость упирается в 250 °/с. Диапазон мал: код 32767. В модели 300 °/с на ±250 читаются как 250,13.

Ускорение при 8 кГц повторяется. Акселерометр обновляется 1 кГц — восемь кадров подряд с одним значением.

99д.8 Итоги

• Адрес 0x68 или 0x69 по AD0, WHO_AM_I = 0x68 при любом; 0x70 и 0x71 — другие микросхемы.
• После включения и сброса PWR_MGMT_1 = 0x40, сон и нули; будит запись 0x01 — такт от гироскопа X; 10 мс на установку генератора.
• Частота выборок = 8 кГц (DLPF_CFG = 0) или 1 кГц / (1 + SMPLRT_DIV): код 3 и делитель 4 — 200 Гц. Фильтр нужен и против наложения: 205 Гц вибрации на 100 Гц выборок — СКО 11,05 °/с без фильтра и 0,74 °/с с полосой 10 Гц (модель).
• 14 байт с 0x3B одной пачкой, старшим вперёд; температура — код / 340 + 36,53 °C; на 400 кГц 15 байт с INT_STATUS — 412,50 мкс.
• FIFO: кадр 12 байт на 1 кГц переполняет 1024 байта за 85,62 мс; после переполнения кадры съезжают — только сброс.
• На 100 кГц шина не успевает за кадром 12 байт на 1 кГц: 5 переполнений на 3000 кадров; на 400 кГц — ни одного.
• INT с защёлкой отпускается чтением INT_STATUS; 15 байт — через DMA1, канал 7, с LAST, STOP — по концу DMA.
• Смещение гироскопа — среднее 1000 выборок в покое: ошибка в тысячные °/с; в XG_OFFS_USR — с перемером. Поворот на 90° за 3 с: 1,803° без калибровки и 0,005° с ней.

Полные исходники

mpu99f.h — регистры, биты, коды ошибок, порт и функции драйвера.

/* mpu99f.h — раздел 99д: драйвер MPU-6050 по I2C — регистры, частота выборок, пачка из 14 байт, FIFO, прерывание
 * «данные готовы» и калибровка смещений гироскопа. Регистров микроконтроллера не знает: говорит с микросхемой только
 * через порт mpu_port_*. В прошивке fw99f.c порт — I2C1 STM32F103, в стенде host99f.c — модель model99f.py.
 * Номера регистров и биты — как в accgyro_mpu.h, accgyro_mpu6050.c, accgyro_spi_mpu6000.c (Betaflight), mpu6050.h
 * (Zephyr 4.4.2), mpu9150.h (VESC) и sensor_mpu9250.c / mpu9250.py (Klipper); даташита MPU-6050 в проекте нет. */
#ifndef MPU99F_H
#define MPU99F_H
#include <stdint.h>

/* ── адрес и регистры ─────────────────────────────────────────────────── */
#define MPU_ADDR_AD0_LO     0x68u       /* вывод AD0 на земле */
#define MPU_ADDR_AD0_HI     0x69u       /* AD0 на питании */
#define MPU_XG_OFFS_USRH    0x13u       /* смещения гироскопа X, Y, Z: по два байта, старший первым, 0x13…0x18 */
#define MPU_SMPLRT_DIV      0x19u       /* частота выборок = частота гироскопа / (1 + SMPLRT_DIV) */
#define MPU_CONFIG          0x1Au       /* DLPF_CFG [2:0] */
#define MPU_GYRO_CONFIG     0x1Bu       /* FS_SEL [4:3] */
#define MPU_ACCEL_CONFIG    0x1Cu       /* AFS_SEL [4:3] */
#define MPU_FIFO_EN         0x23u
#define MPU_INT_PIN_CFG     0x37u
#define MPU_INT_ENABLE      0x38u
#define MPU_INT_STATUS      0x3Au
#define MPU_ACCEL_XOUT_H    0x3Bu       /* 14 байт: ускорение X, Y, Z, температура, гироскоп X, Y, Z — старший первым */
#define MPU_SIGNAL_PATH_RST 0x68u
#define MPU_USER_CTRL       0x6Au
#define MPU_PWR_MGMT_1      0x6Bu
#define MPU_FIFO_COUNTH     0x72u       /* счётчик байт в FIFO: 0x72 и 0x73 */
#define MPU_FIFO_R_W        0x74u
#define MPU_WHO_AM_I        0x75u
#define MPU_WHO_6050        0x68u       /* и при адресе 0x69 */
#define MPU_WHO_6500        0x70u
#define MPU_WHO_9250        0x71u

/* ── биты ─────────────────────────────────────────────────────────────── */
#define MPU_PWR1_RESET      0x80u       /* DEVICE_RESET */
#define MPU_PWR1_SLEEP      0x40u
#define MPU_CLK_PLL_XGYRO   0x01u       /* CLKSEL = 1: генератор с опорой на гироскоп X */
#define MPU_SPR_ALL         0x07u       /* сброс тракта: гироскоп, акселерометр, температура */
#define MPU_USER_FIFO_EN    0x40u
#define MPU_USER_FIFO_RST   0x04u
#define MPU_FIFO_TEMP       0x80u
#define MPU_FIFO_XG         0x40u
#define MPU_FIFO_YG         0x20u
#define MPU_FIFO_ZG         0x10u
#define MPU_FIFO_ACCEL      0x08u
#define MPU_INT_DATA_RDY    0x01u       /* бит в INT_ENABLE и INT_STATUS */
#define MPU_INT_FIFO_OFLOW  0x10u
#define MPU_INTCFG_LATCH    0x20u       /* INT держится до чтения INT_STATUS; без него — импульс 50 мкс */

#define MPU_FIFO_SIZE       1024u       /* байт; по общим сведениям, даташита нет — в модели так */
#define MPU_RESET_MS        100u        /* после DEVICE_RESET и сброса тракта — как в Betaflight */
#define MPU_PLL_MS          10u         /* установка генератора после смены CLKSEL — «не больше 10 мс» */
#define MPU_POLL_US         100u        /* опрос INT_STATUS */
#define MPU_POLL_TRIES      2000u       /* 200 мс */
#define MPU_REST_MDPS       1000        /* калибровка: размах больше 1 °/с по любой оси — прибор двигали */
#define MPU_HW_RESID_MDPS   100         /* после записи XG_OFFS_USR остаток не больше 0,1 °/с, иначе — программно */
#define MPU_FIFO_CHUNK      8u          /* кадров FIFO за одно чтение */

enum { MPU_OK = 0, MPU_E_NACK = -1, MPU_E_BUS = -2, MPU_E_ID = -3, MPU_E_TIMEOUT = -4, MPU_E_SLEEP = -5,
       MPU_E_OFLOW = -6, MPU_E_ALIGN = -7, MPU_E_MOVED = -8, MPU_E_ARG = -9 };
enum { MPU_CAL_SW = 1, MPU_CAL_HW = 2 };

typedef struct {
    int16_t a[3], t, g[3];              /* коды как есть: ускорение, температура, угловая скорость */
} mpu_raw_t;

typedef struct {
    uint8_t addr, who, dlpf, div, fs, afs, fifo_mask, cal;
    uint16_t frame;                     /* байт в кадре FIFO */
    int16_t offs[3];                    /* записано в XG_OFFS_USR */
    int32_t bias_q4[3];                 /* программное смещение гироскопа, коды × 16 */
    uint32_t xfers, errors, overflows, lost;
} mpu_t;

/* ── порт ─────────────────────────────────────────────────────────────── */
/* ntx байт записи, затем (если nrx) повторный старт и nrx байт чтения; ntx = nrx = 0 — только адрес.
 * 0, MPU_E_NACK или MPU_E_BUS */
int mpu_port_xfer(uint8_t addr7, const uint8_t *tx, uint32_t ntx, uint8_t *rx, uint32_t nrx);
void mpu_port_delay_us(uint32_t us);

/* ── регистры ─────────────────────────────────────────────────────────── */
int mpu_write_reg(mpu_t *d, uint8_t reg, uint8_t val);
int mpu_read_regs(mpu_t *d, uint8_t reg, uint8_t *buf, uint32_t n);
int mpu_probe(mpu_t *d);
int mpu_reset(mpu_t *d);
int mpu_init(mpu_t *d, uint8_t dlpf, uint8_t div, uint8_t fs, uint8_t afs);
uint32_t mpu_rate_mhz(uint8_t dlpf, uint8_t div);

/* ── данные ───────────────────────────────────────────────────────────── */
int16_t mpu_be16(const uint8_t *b);
void mpu_parse14(const uint8_t b[14], mpu_raw_t *r);
int mpu_wait_drdy(mpu_t *d);
int mpu_read_sample(mpu_t *d, mpu_raw_t *r);
int32_t mpu_gyro_mdps(int32_t raw, uint8_t fs);
int32_t mpu_acc_mg(int32_t raw, uint8_t afs);
int32_t mpu_temp_cdeg(int32_t raw);

/* ── FIFO ─────────────────────────────────────────────────────────────── */
int mpu_fifo_start(mpu_t *d, uint8_t mask);
int mpu_fifo_count(mpu_t *d, uint16_t *n);
void mpu_fifo_parse(uint8_t mask, const uint8_t *b, mpu_raw_t *r);
int mpu_fifo_read(mpu_t *d, mpu_raw_t *out, uint32_t max, uint32_t *got);

/* ── калибровка гироскопа ─────────────────────────────────────────────── */
int mpu_gyro_bias(mpu_t *d, uint32_t n, int32_t bias_q4[3], int32_t spread[3]);
int mpu_set_gyro_offs(mpu_t *d, const int16_t off[3]);
int mpu_cal_gyro(mpu_t *d, uint32_t n, int use_hw);
int32_t mpu_gyro_corr_mdps(const mpu_t *d, int32_t raw, int axis);
void mpu_angle_step(int64_t *ang_udeg, int32_t mdps, uint32_t dt_us);

#endif

mpu99f.c — драйвер: опознание, сон и сброс, настройка, частота выборок, пачка, FIFO, калибровка.

/* mpu99f.c — раздел 99д: драйвер MPU-6050. Один и тот же файл собирается в прошивку (armclang, Cortex-M3) и в стенд
 * (arm-none-eabi-gcc, Cortex-M3 в QEMU): микроконтроллера он не знает, только порт mpu_port_*.
 * Всё целыми: угловая скорость — в тысячных °/с, ускорение — в тысячных g, температура — в сотых °C. */
#include "mpu99f.h"

static const uint16_t gyro_lsb_x10[4] = {1310, 655, 328, 164};    /* кодов на 1 °/с × 10: ±250 … ±2000 °/с */

static int xfer_done(mpu_t *d, int rc)
{
    d->xfers++;
    if (rc)
        d->errors++;
    return rc;
}

int mpu_write_reg(mpu_t *d, uint8_t reg, uint8_t val)
{
    uint8_t b[2] = {reg, val};
    return xfer_done(d, mpu_port_xfer(d->addr, b, 2, 0, 0));
}

/* номер регистра, повторный старт, n байт подряд: указатель регистров в микросхеме идёт сам */
int mpu_read_regs(mpu_t *d, uint8_t reg, uint8_t *buf, uint32_t n)
{
    return xfer_done(d, mpu_port_xfer(d->addr, &reg, 1, buf, n));
}

static int64_t rdiv(int64_t a, int64_t b)                      /* b > 0, округление к ближайшему */
{
    return a >= 0 ? (a + b / 2) / b : -((-a + b / 2) / b);
}

/* кто на шине: 0x68 (AD0 = 0) или 0x69 (AD0 = 1); WHO_AM_I у MPU-6050 — 0x68 при любом AD0 */
int mpu_probe(mpu_t *d)
{
    static const uint8_t addr[2] = {MPU_ADDR_AD0_LO, MPU_ADDR_AD0_HI};
    uint8_t who;
    d->who = 0;
    for (int i = 0; i < 2; i++) {
        d->addr = addr[i];
        if (xfer_done(d, mpu_port_xfer(d->addr, 0, 0, 0, 0)) != MPU_OK)
            continue;                                           /* нет ответа на этот адрес */
        if (mpu_read_regs(d, MPU_WHO_AM_I, &who, 1) != MPU_OK)
            return MPU_E_BUS;
        d->who = who;
        return who == MPU_WHO_6050 ? MPU_OK : MPU_E_ID;         /* 0x70 — MPU-6500, 0x71 — MPU-9250: другие карты */
    }
    return MPU_E_NACK;
}

/* полный сброс: все регистры — по умолчанию, и микросхема снова спит */
int mpu_reset(mpu_t *d)
{
    int rc;
    if ((rc = mpu_write_reg(d, MPU_PWR_MGMT_1, MPU_PWR1_RESET)) != MPU_OK)
        return rc;
    mpu_port_delay_us(MPU_RESET_MS * 1000u);
    if ((rc = mpu_write_reg(d, MPU_SIGNAL_PATH_RST, MPU_SPR_ALL)) != MPU_OK)
        return rc;
    mpu_port_delay_us(MPU_RESET_MS * 1000u);
    return MPU_OK;
}

/* разбудить с тактом от гироскопа X, частота, фильтр, диапазоны, INT по готовности данных (держится до чтения статуса) */
int mpu_init(mpu_t *d, uint8_t dlpf, uint8_t div, uint8_t fs, uint8_t afs)
{
    uint8_t v;
    int rc;
    if (dlpf > 6u || fs > 3u || afs > 3u)
        return MPU_E_ARG;
    if ((rc = mpu_write_reg(d, MPU_PWR_MGMT_1, MPU_CLK_PLL_XGYRO)) != MPU_OK)    /* SLEEP = 0 */
        return rc;
    mpu_port_delay_us(MPU_PLL_MS * 1000u);
    if ((rc = mpu_read_regs(d, MPU_PWR_MGMT_1, &v, 1)) != MPU_OK)
        return rc;
    if (v & MPU_PWR1_SLEEP)
        return MPU_E_SLEEP;                                     /* запись не дошла: микросхема спит и даёт нули */
    const uint8_t cfg[][2] = {{MPU_SMPLRT_DIV, div}, {MPU_CONFIG, dlpf}, {MPU_GYRO_CONFIG, (uint8_t)(fs << 3)},
                              {MPU_ACCEL_CONFIG, (uint8_t)(afs << 3)}, {MPU_INT_PIN_CFG, MPU_INTCFG_LATCH},
                              {MPU_INT_ENABLE, MPU_INT_DATA_RDY}};
    for (uint32_t i = 0; i < sizeof cfg / sizeof cfg[0]; i++)
        if ((rc = mpu_write_reg(d, cfg[i][0], cfg[i][1])) != MPU_OK)
            return rc;
    d->dlpf = dlpf;
    d->div = div;
    d->fs = fs;
    d->afs = afs;
    return MPU_OK;
}

/* частота выборок в тысячных герца: гироскоп — 8 кГц без фильтра (DLPF_CFG = 0), 1 кГц с ним; делится на 1 + SMPLRT_DIV */
uint32_t mpu_rate_mhz(uint8_t dlpf, uint8_t div)
{
    const uint32_t base = (dlpf == 0u || dlpf == 7u) ? 8000000u : 1000000u;
    return base / (1u + div);
}

int16_t mpu_be16(const uint8_t *b)
{
    return (int16_t)((uint16_t)b[0] << 8 | b[1]);               /* старший байт первым */
}

/* 14 байт с 0x3B: ускорение X, Y, Z, температура, гироскоп X, Y, Z */
void mpu_parse14(const uint8_t b[14], mpu_raw_t *r)
{
    for (int k = 0; k < 3; k++) {
        r->a[k] = mpu_be16(b + 2 * k);
        r->g[k] = mpu_be16(b + 8 + 2 * k);
    }
    r->t = mpu_be16(b + 6);
}

/* опрос INT_STATUS: чтение снимает флаг и отпускает вывод INT */
int mpu_wait_drdy(mpu_t *d)
{
    uint8_t st;
    int rc;
    for (uint32_t i = 0; i < MPU_POLL_TRIES; i++) {
        if ((rc = mpu_read_regs(d, MPU_INT_STATUS, &st, 1)) != MPU_OK)
            return rc;
        if (st & MPU_INT_DATA_RDY)
            return MPU_OK;
        mpu_port_delay_us(MPU_POLL_US);
    }
    return MPU_E_TIMEOUT;                                       /* спит или не настроена */
}

/* одна выборка: дождаться готовности и прочитать 14 байт одной пачкой */
int mpu_read_sample(mpu_t *d, mpu_raw_t *r)
{
    uint8_t b[14];
    int rc;
    if ((rc = mpu_wait_drdy(d)) != MPU_OK)
        return rc;
    if ((rc = mpu_read_regs(d, MPU_ACCEL_XOUT_H, b, 14)) != MPU_OK)
        return rc;
    mpu_parse14(b, r);
    return MPU_OK;
}

int32_t mpu_gyro_mdps(int32_t raw, uint8_t fs)
{
    return (int32_t)rdiv((int64_t)raw * 10000, gyro_lsb_x10[fs & 3u]);
}

int32_t mpu_acc_mg(int32_t raw, uint8_t afs)
{
    return (int32_t)rdiv((int64_t)raw * 1000, 16384 >> (afs & 3u));    /* 16384, 8192, 4096, 2048 кодов на g */
}

/* температура кристалла: код / 340 + 36,53 °C */
int32_t mpu_temp_cdeg(int32_t raw)
{
    return (int32_t)rdiv((int64_t)raw * 100, 340) + 3653;
}

/* ── FIFO ─────────────────────────────────────────────────────────────── */
static uint16_t frame_bytes(uint8_t mask)
{
    return (uint16_t)(((mask & MPU_FIFO_ACCEL) ? 6u : 0u) + ((mask & MPU_FIFO_TEMP) ? 2u : 0u) + ((mask & MPU_FIFO_XG) ? 2u : 0u)
                      + ((mask & MPU_FIFO_YG) ? 2u : 0u) + ((mask & MPU_FIFO_ZG) ? 2u : 0u));
}

/* остановить запись, сбросить FIFO, включить его, снять флаг переполнения чтением статуса, пустить запись */
int mpu_fifo_start(mpu_t *d, uint8_t mask)
{
    uint8_t st;
    int rc;
    d->fifo_mask = mask;
    d->frame = frame_bytes(mask);
    if (!d->frame)
        return MPU_E_ARG;
    if ((rc = mpu_write_reg(d, MPU_FIFO_EN, 0u)) != MPU_OK || (rc = mpu_write_reg(d, MPU_USER_CTRL, MPU_USER_FIFO_RST)) != MPU_OK
        || (rc = mpu_write_reg(d, MPU_USER_CTRL, MPU_USER_FIFO_EN)) != MPU_OK || (rc = mpu_read_regs(d, MPU_INT_STATUS, &st, 1)) != MPU_OK)
        return rc;
    return mpu_write_reg(d, MPU_FIFO_EN, mask);
}

int mpu_fifo_count(mpu_t *d, uint16_t *n)
{
    uint8_t b[2];
    int rc = mpu_read_regs(d, MPU_FIFO_COUNTH, b, 2);
    *n = (uint16_t)((b[0] & 0x1Fu) << 8 | b[1]);
    return rc;
}

/* кадр FIFO: величины идут по возрастанию номеров регистров — ускорение, температура, гироскоп X, Y, Z */
void mpu_fifo_parse(uint8_t mask, const uint8_t *b, mpu_raw_t *r)
{
    uint32_t p = 0;
    *r = (mpu_raw_t){{0, 0, 0}, 0, {0, 0, 0}};
    if (mask & MPU_FIFO_ACCEL) {
        for (int k = 0; k < 3; k++, p += 2)
            r->a[k] = mpu_be16(b + p);
    }
    if (mask & MPU_FIFO_TEMP) {
        r->t = mpu_be16(b + p);
        p += 2;
    }
    for (int k = 0; k < 3; k++) {
        if (mask & (MPU_FIFO_XG >> k)) {
            r->g[k] = mpu_be16(b + p);
            p += 2;
        }
    }
}

/* до max кадров из FIFO. Переполнение или счёт не кратен кадру — граница кадров потеряна: сброс FIFO и ошибка */
int mpu_fifo_read(mpu_t *d, mpu_raw_t *out, uint32_t max, uint32_t *got)
{
    uint8_t st, b[MPU_FIFO_CHUNK * 14u];
    uint16_t cnt;
    uint32_t n;
    int rc;
    *got = 0;
    if ((rc = mpu_read_regs(d, MPU_INT_STATUS, &st, 1)) != MPU_OK || (rc = mpu_fifo_count(d, &cnt)) != MPU_OK)
        return rc;
    if ((st & MPU_INT_FIFO_OFLOW) || cnt % d->frame) {
        d->overflows++;
        d->lost += cnt / d->frame;                              /* то, что лежало, уже со сдвигом — выбросить */
        if ((rc = mpu_fifo_start(d, d->fifo_mask)) != MPU_OK)
            return rc;
        return (st & MPU_INT_FIFO_OFLOW) ? MPU_E_OFLOW : MPU_E_ALIGN;
    }
    n = cnt / d->frame;
    if (n > max)
        n = max;
    while (*got < n) {
        uint32_t k = n - *got > MPU_FIFO_CHUNK ? MPU_FIFO_CHUNK : n - *got;
        if ((rc = mpu_read_regs(d, MPU_FIFO_R_W, b, k * d->frame)) != MPU_OK)
            return rc;
        for (uint32_t i = 0; i < k; i++)
            mpu_fifo_parse(d->fifo_mask, b + i * d->frame, &out[*got + i]);
        *got += k;
    }
    return MPU_OK;
}

/* ── калибровка гироскопа ─────────────────────────────────────────────── */
static int32_t mdps_to_raw(int32_t mdps, uint8_t fs)
{
    return (int32_t)rdiv((int64_t)mdps * gyro_lsb_x10[fs & 3u], 10000);
}

/* n выборок в покое: среднее (коды × 16) и размах по каждой оси; размах больше 1 °/с — прибор двигали */
int mpu_gyro_bias(mpu_t *d, uint32_t n, int32_t bias_q4[3], int32_t spread[3])
{
    int64_t sum[3] = {0, 0, 0};
    int32_t lo[3] = {32767, 32767, 32767}, hi[3] = {-32768, -32768, -32768};
    mpu_raw_t r;
    int rc;
    if (!n)
        return MPU_E_ARG;
    for (uint32_t i = 0; i < n; i++) {
        if ((rc = mpu_read_sample(d, &r)) != MPU_OK)
            return rc;
        for (int k = 0; k < 3; k++) {
            sum[k] += r.g[k];
            lo[k] = r.g[k] < lo[k] ? r.g[k] : lo[k];
            hi[k] = r.g[k] > hi[k] ? r.g[k] : hi[k];
        }
    }
    rc = MPU_OK;
    for (int k = 0; k < 3; k++) {
        bias_q4[k] = (int32_t)rdiv(sum[k] * 16, (int64_t)n);
        spread[k] = hi[k] - lo[k];
        if (spread[k] > mdps_to_raw(MPU_REST_MDPS, d->fs))
            rc = MPU_E_MOVED;
    }
    return rc;
}

/* XG_OFFS_USR, YG_OFFS_USR, ZG_OFFS_USR: шесть байт с 0x13, старший первым */
int mpu_set_gyro_offs(mpu_t *d, const int16_t off[3])
{
    uint8_t b[7] = {MPU_XG_OFFS_USRH};
    for (int k = 0; k < 3; k++) {
        b[1 + 2 * k] = (uint8_t)((uint16_t)off[k] >> 8);
        b[2 + 2 * k] = (uint8_t)off[k];
        d->offs[k] = off[k];
    }
    return xfer_done(d, mpu_port_xfer(d->addr, b, 7, 0, 0));
}

/* Смещение гироскопа: среднее в покое. use_hw — записать его в XG_OFFS_USR в предположении «1 код регистра = 4 кода
 * выхода при ±250 °/с» и перемерить: остаток больше 0,1 °/с — единица не та, регистры обнулить и вычитать программно */
int mpu_cal_gyro(mpu_t *d, uint32_t n, int use_hw)
{
    int32_t b[3], s[3], r[3];
    int16_t off[3] = {0, 0, 0};
    int rc;
    d->cal = 0;
    for (int k = 0; k < 3; k++)
        d->bias_q4[k] = 0;
    if ((rc = mpu_set_gyro_offs(d, off)) != MPU_OK || (rc = mpu_gyro_bias(d, n, b, s)) != MPU_OK)
        return rc;
    for (int k = 0; k < 3; k++)
        d->bias_q4[k] = b[k];
    d->cal = MPU_CAL_SW;
    if (!use_hw)
        return MPU_CAL_SW;
    for (int k = 0; k < 3; k++)
        off[k] = (int16_t)-rdiv((int64_t)b[k] * (1 << d->fs), 64);
    if ((rc = mpu_set_gyro_offs(d, off)) != MPU_OK || (rc = mpu_gyro_bias(d, n, r, s)) != MPU_OK)
        return rc;
    for (int k = 0; k < 3; k++) {
        if (r[k] > 16 * mdps_to_raw(MPU_HW_RESID_MDPS, d->fs) || r[k] < -16 * mdps_to_raw(MPU_HW_RESID_MDPS, d->fs)) {
            off[0] = off[1] = off[2] = 0;
            rc = mpu_set_gyro_offs(d, off);                     /* регистр понят не так — вычитаем сами */
            return rc != MPU_OK ? rc : MPU_CAL_SW;
        }
    }
    for (int k = 0; k < 3; k++)
        d->bias_q4[k] = r[k];                                   /* остаток доберём программно */
    d->cal = MPU_CAL_HW;
    return MPU_CAL_HW;
}

int32_t mpu_gyro_corr_mdps(const mpu_t *d, int32_t raw, int axis)
{
    return (int32_t)rdiv(((int64_t)raw * 16 - d->bias_q4[axis]) * 10000, 16 * (int64_t)gyro_lsb_x10[d->fs & 3u]);
}

/* угол += скорость × время: тысячные °/с × мкс / 1000 = миллионные доли градуса */
void mpu_angle_step(int64_t *ang_udeg, int32_t mdps, uint32_t dt_us)
{
    *ang_udeg += rdiv((int64_t)mdps * dt_us, 1000);
}

fw99f.c — прошивка F103: I2C1 на 400 кГц, приём одного, двух и N байт, калибровка, INT на EXTI5 и чтение через DMA1.

/* fw99f.c — раздел 99д: MPU-6050 на STM32F103 (BluePill), 72 МГц, APB1 = 36 МГц.
 * I2C1: PB6 — SCL, PB7 — SDA (открытый сток, 400 кГц). INT микросхемы — на PB5 (EXTI5, по фронту).
 * Запуск: кто на шине (0x68 или 0x69), сброс, 200 выборок в секунду с фильтром 42 Гц, ±250 °/с и ±2 g, калибровка
 * смещения гироскопа в покое (2 с — не трогать). Дальше по каждому INT: адресная часть опросом (~29 тактов SCL),
 * 15 байт с INT_STATUS (0x3A) — через DMA1, канал 7 (I2C1_RX), STOP — в прерывании по концу передачи.
 * Через отладчик: gyro_mdps, acc_mg, temp_cdeg, yaw_udeg — курс по гироскопу Z; samples, missed, cal_rc.
 * Тактирование до 72 МГц — как в разделе 6. */
#include "stm32f10x.h"
#include "mpu99f.h"

#define CPU_HZ          72000000u
#define APB1_MHZ        36u
#define I2C_TIMEOUT_US  1000u           /* байт на 400 кГц — 22,5 мкс */
#define RATE_DIV        4u              /* 1 кГц / (1 + 4) = 200 Гц */
#define DT_US           5000u
#define CAL_N           400u            /* 2 с при 200 Гц */

static mpu_t dev;
static uint8_t dma_buf[15];             /* INT_STATUS и 14 байт данных */
static volatile uint8_t busy;
volatile uint32_t samples, missed, bus_errors;
volatile int32_t gyro_mdps[3], acc_mg[3], temp_cdeg, cal_rc;
volatile int64_t yaw_udeg;

static int expired(uint32_t t0, uint32_t us)
{
    return DWT->CYCCNT - t0 >= us * (CPU_HZ / 1000000u);
}

void mpu_port_delay_us(uint32_t us)
{
    uint32_t t0 = DWT->CYCCNT;
    while (!expired(t0, us)) {
    }
}

static void gpio_i2c1_init(void)
{
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN | RCC_APB2ENR_AFIOEN;
    RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;
    RCC->AHBENR |= RCC_AHBENR_DMA1EN;
    /* PB6, PB7 — альтернативная функция, открытый сток, 50 МГц (1111); PB5 — вход с подтяжкой к земле (1000) */
    GPIOB->CRL = (GPIOB->CRL & 0x000FFFFFu) | 0xFF800000u;
    GPIOB->BRR = 1u << 5;                               /* ODR5 = 0: подтяжка вниз — INT активен высоким */
    I2C1->CR1 = 0;                                      /* настраивать при PE = 0 */
    I2C1->CR2 = APB1_MHZ;
    I2C1->CCR = I2C_CCR_FS | 30u;                       /* DUTY = 0: 36 МГц / (3 × 30) = 400 кГц */
    I2C1->TRISE = APB1_MHZ * 300u / 1000u + 1u;         /* 300 нс × 36 МГц = 10,8 → 10 + 1 = 11 */
    I2C1->CR1 = I2C_CR1_PE;
}

static int sr1_wait(uint16_t mask)
{
    uint32_t t0 = DWT->CYCCNT;
    while (!(I2C1->SR1 & mask)) {
        if (I2C1->SR1 & I2C_SR1_AF)
            return MPU_E_NACK;
        if (expired(t0, I2C_TIMEOUT_US))
            return MPU_E_BUS;
    }
    return MPU_OK;
}

static int fail(int rc)
{
    I2C1->CR1 |= I2C_CR1_STOP;
    I2C1->SR1 = (uint16_t)~I2C_SR1_AF;                  /* AF снимается записью нуля */
    I2C1->CR1 = (uint16_t)((I2C1->CR1 & ~I2C_CR1_POS) | I2C_CR1_ACK);
    return rc;
}

/* приём n байт ведущим: один — RM0008, с. 764; два — с. 764, бит POS; три и больше — способ 2, с. 763 */
static int rx_n(uint8_t *rx, uint32_t n)
{
    int rc;
    if (n == 1u) {
        I2C1->CR1 &= (uint16_t)~I2C_CR1_ACK;            /* NACK — до снятия ADDR */
        (void)I2C1->SR2;
        I2C1->CR1 |= I2C_CR1_STOP;
        if ((rc = sr1_wait(I2C_SR1_RXNE)) != MPU_OK)
            return rc;
        rx[0] = (uint8_t)I2C1->DR;
    } else if (n == 2u) {                               /* POS и ACK стоят с повторного старта */
        (void)I2C1->SR2;
        I2C1->CR1 &= (uint16_t)~I2C_CR1_ACK;
        if ((rc = sr1_wait(I2C_SR1_BTF)) != MPU_OK)     /* оба байта приняты: в DR и в сдвиговом */
            return rc;
        I2C1->CR1 |= I2C_CR1_STOP;
        rx[0] = (uint8_t)I2C1->DR;
        rx[1] = (uint8_t)I2C1->DR;
        I2C1->CR1 &= (uint16_t)~I2C_CR1_POS;
    } else {
        (void)I2C1->SR2;                                /* ADDR снят, ACK = 1 */
        for (uint32_t i = 0; i + 3u < n; i++) {
            if ((rc = sr1_wait(I2C_SR1_RXNE)) != MPU_OK)
                return rc;
            rx[i] = (uint8_t)I2C1->DR;
        }
        if ((rc = sr1_wait(I2C_SR1_BTF)) != MPU_OK)     /* N−2 в DR, N−1 в сдвиговом, SCL прижат */
            return rc;
        I2C1->CR1 &= (uint16_t)~I2C_CR1_ACK;
        rx[n - 3u] = (uint8_t)I2C1->DR;
        if ((rc = sr1_wait(I2C_SR1_BTF)) != MPU_OK)
            return rc;
        I2C1->CR1 |= I2C_CR1_STOP;
        rx[n - 2u] = (uint8_t)I2C1->DR;
        if ((rc = sr1_wait(I2C_SR1_RXNE)) != MPU_OK)
            return rc;
        rx[n - 1u] = (uint8_t)I2C1->DR;
    }
    I2C1->CR1 |= I2C_CR1_ACK;
    return MPU_OK;
}

/* старт, адрес, байты записи; при rd — повторный старт и адрес чтения (ADDR ещё не снят) */
static int addr_phase(uint8_t addr7, const uint8_t *tx, uint32_t ntx, int rd, int pos)
{
    int rc;
    uint32_t t0 = DWT->CYCCNT;
    while (I2C1->SR2 & I2C_SR2_BUSY) {                  /* SDA держит ведомый — раздел 17.6 */
        if (expired(t0, I2C_TIMEOUT_US))
            return MPU_E_BUS;
    }
    I2C1->CR1 |= I2C_CR1_ACK | I2C_CR1_START;
    if ((rc = sr1_wait(I2C_SR1_SB)) != MPU_OK)
        return rc;
    I2C1->DR = (uint8_t)(addr7 << 1);
    if ((rc = sr1_wait(I2C_SR1_ADDR)) != MPU_OK)
        return rc;
    (void)I2C1->SR2;
    for (uint32_t i = 0; i < ntx; i++) {
        if ((rc = sr1_wait(I2C_SR1_TXE)) != MPU_OK)
            return rc;
        I2C1->DR = tx[i];
    }
    if (ntx && (rc = sr1_wait(I2C_SR1_BTF)) != MPU_OK)
        return rc;
    if (!rd)
        return MPU_OK;
    if (pos)
        I2C1->CR1 |= I2C_CR1_POS;
    I2C1->CR1 |= I2C_CR1_START;                         /* повторный старт: номер регистра уже передан */
    if ((rc = sr1_wait(I2C_SR1_SB)) != MPU_OK)
        return rc;
    I2C1->DR = (uint8_t)(addr7 << 1 | 1u);
    return sr1_wait(I2C_SR1_ADDR);
}

int mpu_port_xfer(uint8_t addr7, const uint8_t *tx, uint32_t ntx, uint8_t *rx, uint32_t nrx)
{
    int rc;
    if (nrx && !ntx)
        return MPU_E_BUS;                               /* чтение всегда с номера регистра */
    if ((rc = addr_phase(addr7, tx, ntx, nrx != 0u, nrx == 2u)) != MPU_OK)
        return fail(rc);
    if (!nrx) {
        I2C1->CR1 |= I2C_CR1_STOP;
        return MPU_OK;
    }
    if ((rc = rx_n(rx, nrx)) != MPU_OK)
        return fail(rc);
    return MPU_OK;
}

/* ── INT → DMA ────────────────────────────────────────────────────────── */
static void irq_init(void)
{
    DMA1_Channel7->CCR = 0;                             /* настраивать выключенным */
    DMA1_Channel7->CPAR = (uint32_t)&I2C1->DR;
    DMA1_Channel7->CMAR = (uint32_t)dma_buf;
    AFIO->EXTICR[1] = (AFIO->EXTICR[1] & ~0x00F0u) | AFIO_EXTICR2_EXTI5_PB;    /* EXTI5 — порт B, с. 190 */
    EXTI->RTSR |= 1u << 5;                              /* по фронту: INT активен высоким, с. 211 */
    EXTI->PR = 1u << 5;
    EXTI->IMR |= 1u << 5;
    NVIC_SetPriority(DMA1_Channel7_IRQn, 1);
    NVIC_SetPriority(EXTI9_5_IRQn, 2);
    NVIC_EnableIRQ(DMA1_Channel7_IRQn);
    NVIC_EnableIRQ(EXTI9_5_IRQn);
}

/* INT: регистр 0x3A, повторный старт, адрес чтения — опросом; 15 байт — DMA с LAST: NACK на последнем (с. 769) */
void EXTI9_5_IRQHandler(void)
{
    static const uint8_t reg = MPU_INT_STATUS;
    EXTI->PR = 1u << 5;
    if (busy) {
        missed++;                                       /* прошлое чтение не кончилось */
        return;
    }
    DMA1_Channel7->CNDTR = sizeof dma_buf;
    DMA1_Channel7->CCR = DMA_CCR7_MINC | DMA_CCR7_TCIE | DMA_CCR7_PL_1 | DMA_CCR7_EN;   /* из периферии в память */
    if (addr_phase(dev.addr, &reg, 1, 1, 0) != MPU_OK) {
        DMA1_Channel7->CCR = 0;
        (void)fail(MPU_E_BUS);
        bus_errors++;
        return;
    }
    I2C1->CR2 |= I2C_CR2_DMAEN | I2C_CR2_LAST;          /* в событии ADDR, до его снятия (с. 769) */
    busy = 1;
    (void)I2C1->SR2;                                    /* ADDR снят: дальше байты идут сами */
}

/* конец DMA: STOP, разбор, поправка смещения, курс по гироскопу Z */
void DMA1_Channel7_IRQHandler(void)
{
    mpu_raw_t r;
    DMA1->IFCR = DMA_IFCR_CTCIF7;
    DMA1_Channel7->CCR = 0;
    I2C1->CR1 |= I2C_CR1_STOP;
    I2C1->CR2 &= (uint16_t)~(I2C_CR2_DMAEN | I2C_CR2_LAST);
    mpu_parse14(dma_buf + 1, &r);
    for (int k = 0; k < 3; k++) {
        gyro_mdps[k] = mpu_gyro_corr_mdps(&dev, r.g[k], k);
        acc_mg[k] = mpu_acc_mg(r.a[k], dev.afs);
    }
    temp_cdeg = mpu_temp_cdeg(r.t);
    int64_t y = yaw_udeg;
    mpu_angle_step(&y, gyro_mdps[2], DT_US);
    yaw_udeg = y;
    samples++;
    busy = 0;
}

int main(void)
{
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CYCCNT = 0;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
    gpio_i2c1_init();
    while (mpu_probe(&dev) != MPU_OK)
        mpu_port_delay_us(100000u);                     /* нет ответа ни на 0x68, ни на 0x69, или это не MPU-6050 */
    while (mpu_reset(&dev) != MPU_OK || mpu_init(&dev, 3u, RATE_DIV, 0u, 0u) != MPU_OK)
        mpu_port_delay_us(100000u);
    cal_rc = mpu_cal_gyro(&dev, CAL_N, 1);              /* MPU_CAL_HW или MPU_CAL_SW; меньше нуля — двигали */
    irq_init();
    if (GPIOB->IDR & (1u << 5))
        EXTI->SWIER = 1u << 5;                          /* INT уже поднят (защёлка): фронта не будет — начать самим */
    for (;;)
        __WFI();
}

Дальше: раздел 100 «Расстояние и присутствие: HC-SR04 и HC-SR501» — ультразвук и захват таймера, делитель на пятивольтовом эхо и инфракрасный датчик движения.