99е. Датчик ориентации BNO055 по I2C: готовое слияние внутри чипа — режимы работы, кватернион и углы Эйлера, калибровка и её сохранение, растягивание такта

В 99б ориентацию собирал сам микроконтроллер: гироскоп, акселерометр и магнитометр, фильтр Маджвика, калибровка поля эллипсоидом. В 99г был отдельный магнитометр, в 99д — гироскоп-акселерометр MPU-6050. BNO055 — это все три датчика и процессор со слиянием в одном корпусе: он сам считает кватернион, углы Эйлера, тяжесть и линейное ускорение, сам калибрует датчики и отдаёт готовое по I2C. Здесь — как с ним говорить: найти на шине и сбросить, выбрать режим работы и выждать переключение, задать единицы и оси, читать кватернион, а не углы, откалибровать и не терять калибровку при каждом включении, пережить растягивание такта. И главный вопрос — что выигрывает микроконтроллер, отдав слияние микросхеме, против фильтра 99б у себя.

Опоры: RM0008 (DocID13902 Rev 17): ведомый может держать SCL, и ведущий F1 ждёт его, остановив счёт высокого уровня, с. 758; приём трёх байт и больше ведущим, способ 2, с. 763; одного и двух — с. 764; стандартный режим, CCR и TRISE, с. 783–784. DocID13587 Rev 17 (STM32F103x8/xB): ток в режиме Run на 72 МГц, тактирование периферии выключено, — не больше 32,8 мА, таблица 13, с. 42; в режиме Sleep — не больше 7,5 мА, таблица 15, с. 44. Документации производителя BNO055 в проекте нет. Адрес 0x28, коды A0 FB 32 0F, OPR_MODE (0x3D) и коды режимов 0x00…0x0C, PWR_MODE (0x3E), SYS_TRIGGER (0x3F) и команда сброса 0x20, AXIS_MAP_CONFIG (0x41) и восемь раскладок осей, адреса и цена разряда данных — по драйверу с открытым кодом: OpenMV (коммит 6236993, scripts/libraries/bno055.py, MIT). Остальное — адрес 0x29, PAGE_ID, CALIB_STAT, ST_RESULT, SYS_STATUS, SYS_ERR, UNIT_SEL, AXIS_MAP_SIGN, 22 байта смещений, время смены режима и загрузки, кадры UART, выводы PS0 и PS1 — по документации производителя, не проверено: модель повторяет, а не проверяет. Алгоритм слияния BNO055 закрыт; в модели вместо него свой фильтр, поэтому числа точности ниже говорят о модели, а не о микросхеме. Все числа — прогон драйвера _verify/ori99g/bno99g.c, собранного arm-none-eabi-gcc 12.2.1 под Cortex-M3 и запущенного в QEMU 7.2 (машина mps2-an385), со своей моделью BNO055 на Python 3.11; там же на том же Cortex-M3 работает фильтр из 99б. Прошивка собрана armclang 6.24.

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

Оригинал	Перевод
sensor fusion	слияние датчиков: одна ориентация из трёх приборов
OPR_MODE, operation mode	режим работы: какие датчики включены и считается ли слияние
NDOF, NDOF_FMC_OFF	девять степеней свободы; то же без быстрой калибровки поля
IMU, COMPASS, M4G	без магнитометра; без гироскопа; поле вместо гироскопа
CONFIG mode	режим настройки: только в нём пишутся единицы, оси, смещения
PAGE_ID	номер страницы регистров
UNIT_SEL	выбор единиц: м/с² или мg, °/с или рад/с, градусы или радианы
EUL, QUA	углы Эйлера и кватернион на выходе слияния
LIA, GRV	линейное ускорение и вектор тяжести
CALIB_STAT	состояние калибровки: по два бита на систему и каждый датчик
offset registers, radius	регистры смещений и радиусы сфер калибровки
AXIS_MAP_CONFIG, AXIS_MAP_SIGN	перестановка осей и их знаки под крепление платы
ST_RESULT	итог самотеста при включении
SYS_STATUS, SYS_ERR	что делает система и код её ошибки
PS0, PS1	выводы выбора интерфейса: I2C или UART

99е.1 Адрес, коды, сброс и страницы регистров

Адрес — 0x28 (так в OpenMV, address=0x28), при COM3 на питании — 0x29 (по документации). С регистра 0x00 подряд лежат четыре кода: кристалл A0, акселерометр FB, магнитометр 32, гироскоп 0F; OpenMV сверяет все четыре. Драйвер пробует оба адреса и сверяет первый:

/* кто на шине: 0x28 (COM3 = 0) или 0x29 (COM3 = 1); четыре кода — кристалл A0, акселерометр FB, магнитометр 32, гироскоп 0F */
int bno_probe(bno_t *d)
{
    static const uint8_t addr[2] = {BNO_ADDR_A, BNO_ADDR_B};
    for (int i = 0; i < 4; i++)
        d->id[i] = 0;
    for (int i = 0; i < (d->uart ? 1 : 2); i++) {
        d->addr = addr[i];
        if (!d->uart && xfer_done(d, bno_port_xfer(d->addr, 0, 0, 0, 0)) != BNO_OK)
            continue;                                           /* нет ответа на этот адрес */
        if (bno_read_regs(d, BNO_CHIP_ID, d->id, 4) != BNO_OK)
            return BNO_E_BUS;
        return d->id[0] == BNO_ID_CHIP ? BNO_OK : BNO_E_ID;
    }
    return BNO_E_NACK;
}

В модели плата с COM3 на земле опознаётся за 2 обмена, с COM3 на питании — за 3. Микросхема с другим кодом даёт BNO_E_ID, −3; пустая шина — BNO_E_NACK, −1.

Сброс. Бит 5 SYS_TRIGGER (0x3F), значение 0x20 — сброс всей системы. После него микросхема загружается и какое-то время не отвечает на свой адрес; OpenMV просто ждёт 700 мс. Драйвер вместо паузы вслепую опрашивает CHIP_ID каждые 10 мс до 1 с:

/* сброс: SYS_TRIGGER = 0x20, затем ждать, пока микросхема снова ответит кодом A0 */
int bno_reset(bno_t *d)
{
    uint8_t id;
    int rc = bno_write_reg(d, BNO_SYS_TRIGGER, BNO_TRIG_RST_SYS);
    if (rc != BNO_OK)
        return rc;
    for (uint32_t t = BNO_BOOT_POLL_MS; t <= BNO_BOOT_MS; t += BNO_BOOT_POLL_MS) {
        bno_port_delay_us(BNO_BOOT_POLL_MS * 1000u);
        if (bno_read_regs(d, BNO_CHIP_ID, &id, 1) == BNO_OK && id == BNO_ID_CHIP) {
            d->boot_ms = t;
            d->mode = BNO_MODE_CONFIG;
            return BNO_OK;
        }
    }
    return BNO_E_TIMEOUT;
}

В модели загрузка — 650 мс (по документации), драйвер дождался ответа на 650-й мс. После сброса микросхема в режиме CONFIG.

Страницы. Адресов 0x00…0x7F на всё не хватает: PAGE_ID (0x07) переключает страницу 0 — данные, режимы, смещения — и страницу 1 — настройки самих датчиков. Всё в этом разделе — на странице 0. Кто переключил на 1 и не вернул, читает по 0x00 не тот регистр: в модели CHIP_ID на странице 1 читается как 00, на странице 0 — A0. Поэтому настройка начинается с записи PAGE_ID = 0.

99е.2 Режимы работы, время переключения, питание и самотест

OPR_MODE (0x3D) решает, какие датчики работают и считается ли слияние. Коды — как в OpenMV:

Какие датчики в каком режиме и что значит «быстрая калибровка поля» — по документации производителя, не проверено.

Смена режима не мгновенна. По документации переход из CONFIG в рабочий режим занимает 7 мс, из рабочего в CONFIG — 19 мс. Пока он идёт, записи в регистры теряются. Драйвер ждёт и читает OPR_MODE обратно; из рабочего режима в рабочий — через CONFIG:

/* режим: из рабочего в рабочий — через CONFIG; после записи — 19 мс в CONFIG или 7 мс из него; затем прочитать обратно */
int bno_set_mode(bno_t *d, uint8_t mode)
{
    uint8_t v;
    int rc;
    if (mode > BNO_MODE_NDOF)
        return BNO_E_ARG;
    if (mode != BNO_MODE_CONFIG && d->mode != BNO_MODE_CONFIG && (rc = bno_set_mode(d, BNO_MODE_CONFIG)) != BNO_OK)
        return rc;
    if ((rc = bno_write_reg(d, BNO_OPR_MODE, mode)) != BNO_OK)
        return rc;
    bno_port_delay_us((mode == BNO_MODE_CONFIG ? BNO_T_TO_CONFIG_MS : BNO_T_FROM_CONFIG_MS) * 1000u);
    if ((rc = bno_read_regs(d, BNO_OPR_MODE, &v, 1)) != BNO_OK)
        return rc;
    d->mode = v & 0x0Fu;
    return d->mode == mode ? BNO_OK : BNO_E_MODE;
}

В модели: CONFIG → NDOF — 7 мс, NDOF → CONFIG — 19 мс, CONFIG → IMU — 7 мс, IMU → NDOF — 26 мс: записи 3D 00 и 3D 0C. Настройка после сброса — записи 3D 00, 07 00, 3E 00, 3B 00, 41 24 00 и чтение ST_RESULT → 0F, 19 мс вместе с ожиданием.

Что будет без паузы, видно в модели: из NDOF записать CONFIG и сразу UNIT_SEL = 0x04 и AXIS_MAP_CONFIG = 0x21. Через 50 мс читаются 00 и 24 — обе записи пропали. С bno_set_mode — 04 и 21. И в самом NDOF запись UNIT_SEL тоже теряется молча: 00.

Питание. PWR_MODE (0x3E): 0 — обычное, 1 — пониженное, 2 — «сон» (коды — по документации). В модели во сне регистры данных замирают: плату повернули со скоростью 45 °/с, через секунду кватернион отстаёт от правды на 42,56°; после возврата в обычное питание — 1,62°.

Что делает система. SYS_STATUS (0x39): в модели 0 в CONFIG, 5 — слияние работает (NDOF), 6 — работает без слияния (AMG), 4 — идёт самотест, 1 — ошибка; код ошибки — в SYS_ERR (0x3A), 3 — «самотест не прошёл» (значения — по документации). ST_RESULT (0x36) — итог самотеста при включении: биты 3…0 — процессор, гироскоп, магнитометр, акселерометр, 0x0F — всё исправно. Драйвер проверяет его в bno_init и умеет запустить самотест заново (бит 0 SYS_TRIGGER, только в CONFIG):

/* самотест по команде: только в CONFIG; ждать, пока SYS_STATUS уйдёт из 4 — «идёт самотест» */
int bno_selftest(bno_t *d, uint8_t *st)
{
    uint8_t sys = 4u, err;
    int rc;
    *st = 0;
    if (d->mode != BNO_MODE_CONFIG)
        return BNO_E_MODE;
    if ((rc = bno_write_reg(d, BNO_SYS_TRIGGER, BNO_TRIG_SELFTEST)) != BNO_OK)
        return rc;
    for (uint32_t t = 0; sys == 4u && t < BNO_SELFTEST_MS; t += BNO_BOOT_POLL_MS) {
        bno_port_delay_us(BNO_BOOT_POLL_MS * 1000u);
        if ((rc = bno_status(d, &sys, &err)) != BNO_OK)
            return rc;
    }
    if (sys == 4u)
        return BNO_E_TIMEOUT;
    if ((rc = bno_read_regs(d, BNO_ST_RESULT, st, 1)) != BNO_OK)
        return rc;
    return *st == BNO_ST_ALL ? BNO_OK : BNO_E_SELFTEST;
}

В модели исправная микросхема проходит самотест по команде за 405 мс, ST_RESULT = 0x0F. Микросхема с неисправным магнитометром после сброса показывает 0x0D — бит 1 пуст, — bno_init возвращает BNO_E_SELFTEST, −6, а SYS_STATUS и SYS_ERR — 1 и 3.

99е.3 Единицы, оси, кватернион и углы Эйлера

Единицы. UNIT_SEL (0x3B): бит 0 — ускорение в мg вместо м/с², бит 1 — угловая скорость в рад/с вместо °/с, бит 2 — углы в радианах, бит 4 — температура в °F (по документации). Драйвер всегда пишет его сам, не надеясь на значение после сброса. Цена разряда — как в OpenMV:

Все значения — 16 бит, младшим байтом вперёд. OpenMV делит угловую скорость на 900 — это рад/с, а после сброса, по документации, °/с и 16: такой драйвер на чужой микросхеме ошибается в 56,25 раза. Цена 16 и 900 у углов — та же, что у угловой скорости.

/* кватернион w, x, y, z: 8 байт с 0x20, 1 = 16384 */
int bno_read_quat(bno_t *d, int16_t q[4])
{
    return read_vec(d, BNO_QUA_DATA, q, 4);
}

/* угол Эйлера в сотых градуса из кода в градусном режиме */
int32_t bno_euler_cdeg(int16_t raw)
{
    int32_t a = (int32_t)raw * 100;
    return a >= 0 ? (a + 8) / 16 : -((-a + 8) / 16);
}

В модели плата неподвижна: курс 40,00°, наклоны 20,00° и 30,00°. Через 3 с после запуска NDOF с восстановленной калибровкой: углы — коды 603, 315, 474, то есть 37,69°, 19,69° и 29,63°; кватернион 14538, 4782, 1226, −5720 — от правды на 2,47° (слияние модели ещё подтягивает курс). Тяжесть — −330, 457, 803 при правде −335,41, 460,76, 798,06, линейное ускорение — −8, 10, −3: почти ноль. С битом 2 UNIT_SEL курс читается как 639, то есть 0,7100 рад.

Тяжесть и линейное ускорение. GRV — куда, по мнению слияния, смотрит «вниз», LIA — ускорение минус тяжесть. Они верны, пока слияние не путает разгон с наклоном. В модели плату разгоняют вдоль x на 2 м/с² 1 с: до разгона LIA — −2, 8, 5; через 0,2 с — 159 (1,59 м/с²), тяжесть по x — 38; через 0,9 с — 82, а тяжесть по x — 121: слияние поверило, что плата наклонилась. Короткие толчки LIA ловит, долгий разгон — нет.

Оси. Слияние считает в осях микросхемы. Если она стоит на плате повёрнутой, оси переставляют AXIS_MAP_CONFIG (0x41) и AXIS_MAP_SIGN (0x42): в первом по два бита на X, Y, Z — номер исходной оси, во втором биты 2, 1, 0 — минус у X, Y, Z (по документации). OpenMV задаёт восемь раскладок P0…P7: 0x21 или 0x24 и знаки 04, 00, 06, 02, 03, 01, 07, 05. У всех восьми одно свойство: перестановка пары осей идёт с нечётным числом минусов, без перестановки — с чётным. Иначе правая тройка осей становится левой, и ни кватернион, ни углы не сойдутся с платой. Драйвер такую запись не пускает:

/* оси: map — три поля по 2 бита (X — 1:0, Y — 3:2, Z — 5:4: номер исходной оси), sign — биты 2, 1, 0 = минус у X, Y, Z.
 * Правая тройка остаётся правой: перестановка нечётная — минусов нечётное число, чётная — чётное. Только в CONFIG */
int bno_axis_remap(bno_t *d, uint8_t map, uint8_t sign)
{
    uint8_t ax[3], b[2] = {map, sign};
    int odd = 0, minus = 0;
    for (int k = 0; k < 3; k++)
        ax[k] = (uint8_t)(map >> (2 * k) & 3u);
    if (map > 0x3Fu || sign > 7u || ax[0] > 2u || ax[1] > 2u || ax[2] > 2u || ax[0] == ax[1] || ax[0] == ax[2] || ax[1] == ax[2])
        return BNO_E_ARG;
    odd = (ax[0] != 0u) + (ax[1] != 1u) + (ax[2] != 2u) == 2;  /* ровно две оси не на месте — одна перестановка пары */
    for (int k = 0; k < 3; k++)
        minus += sign >> k & 1u;
    if ((minus & 1) != odd)
        return BNO_E_ARG;                                       /* левая тройка: углы и кватернион не сойдутся с осями */
    if (d->mode != BNO_MODE_CONFIG)
        return BNO_E_MODE;
    return bno_write_regs(d, BNO_AXIS_MAP_CONFIG, b, 2);
}

В модели микросхема повёрнута на плате на 90° (x микросхемы — по y платы), плата смотрит на север. С осями по умолчанию курс — 270,31°, с P0 (0x21, 0x04) — 0,31°. Перестановка X и Y без знака (0x21, 0x00) и поле 3 в карте (0x3F) — BNO_E_ARG, −7.

Почему кватернион. У одного угла Эйлера диапазон ±90°; когда он подходит к 90°, курс и второй наклон сливаются, и сами углы теряют смысл — «замок» из 99б.2. В модели плату наклоняют со скоростью 30 °/с и читают углы и кватернион каждые 50 мс. Наибольший скачок курса между соседними чтениями — 100,25° (углы с 51,75; 89,31; −52,31 до 152,00; 88,81; −152,50 при истинном наклоне 87,65°), а кватернион за те же 50 мс повернулся не больше чем на 1,81°. Для управления брать кватернион, углы — только показывать.

99е.4 Калибровка: CALIB_STAT, процедура и 22 байта во флеше

BNO055 калибрует себя сам, пока работает слияние, и сообщает, как далеко продвинулся: CALIB_STAT (0x35), по два бита на систему, гироскоп, акселерометр и магнитометр (биты 7:6, 5:4, 3:2, 1:0), от 0 до 3 (по документации).

/* CALIB_STAT: по два бита, 0 — не откалиброван, 3 — полностью */
int bno_calib(bno_t *d, uint8_t *sys, uint8_t *gyr, uint8_t *acc, uint8_t *mag)
{
    uint8_t c = 0;
    int rc = bno_read_regs(d, BNO_CALIB_STAT, &c, 1);
    *sys = c >> 6 & 3u;
    *gyr = c >> 4 & 3u;
    *acc = c >> 2 & 3u;
    *mag = c & 3u;
    return rc;
}

Процедура (по документации; в модели — так же): гироскопу — несколько секунд покоя; магнитометру — «восьмёрка» в воздухе, чтобы поле прошло через все стороны; акселерометру — шесть устойчивых положений, по несколько секунд в каждом, в том числе вверх ногами. Модель — свои правила: 1 с покоя для гироскопа, сфера по точкам и 24 из 40 направлений вокруг её центра для поля, 6 положений через 30° для ускорения. Статусы система-гироскоп-акселерометр-магнитометр каждые 3 с: покой 0–3 с, «восьмёрка» 3–18 с, шесть положений 18–36 с:

Гироскоп готов через 1,00 с, поле — через 13,00 с, акселерометр и вся система — через 34,51 с. Если только крутить плату на столе вокруг вертикали 15 с, поле остаётся на 0 — 0300 через 20 с: точки лежат в одной плоскости, сферу по ним не построить (как в 99г.4).

22 байта. Итог калибровки — регистры с 0x55 по 0x6A (по документации): смещения акселерометра X, Y, Z (1 м/с² = 100), магнитометра (1 мкТл = 16), гироскопа (1 °/с = 16), радиус акселерометра и радиус магнитометра, все — 16 бит младшим вперёд. Читать и писать их — в CONFIG; драйвер переходит в него и возвращает прежний режим:

/* записать сохранённые смещения: только в CONFIG (в рабочем режиме запись не принимается), потом прежний режим */
int bno_offsets_write(bno_t *d, const uint8_t off[BNO_OFFSETS_LEN])
{
    uint8_t prev = d->mode;
    int rc;
    if ((rc = bno_set_mode(d, BNO_MODE_CONFIG)) != BNO_OK || (rc = bno_write_regs(d, BNO_OFFSETS, off, BNO_OFFSETS_LEN)) != BNO_OK)
        return rc;
    return prev == BNO_MODE_CONFIG ? BNO_OK : bno_set_mode(d, prev);
}

В модели после процедуры 22 байта — 0E00F6FF1400400110FFE0010800FBFF0600D5032003: акселерометр 0,14, −0,10, 0,20 м/с² при истинных 0,15, −0,10, 0,20; поле 20,00, −15,00, 30,00 мкТл — ровно жёсткое железо модели; гироскоп 0,50, −0,31, 0,38 °/с при истинных 0,50, −0,30, 0,40; радиусы — 981 и 800.

Во флеш. Калибровка живёт в микросхеме до сброса или снятия питания. Прошивка один раз, как только CALIB_STAT = 0xFF, читает 22 байта и пишет их в последнюю страницу флеша, как в 11.5: метка, байты, CRC-32 — 8 слов:

int bno_rec_unpack(const uint32_t r[BNO_REC_WORDS], uint8_t off[BNO_OFFSETS_LEN])
{
    if (r[0] != BNO_REC_MAGIC)
        return BNO_E_EMPTY;                                     /* стёртая страница: 0xFFFFFFFF */
    if (bno_crc32(r, 4u * (BNO_REC_WORDS - 1u)) != r[BNO_REC_WORDS - 1u])
        return BNO_E_CRC;                                       /* запись побита или недописана */
    for (uint32_t i = 0; i < BNO_OFFSETS_LEN; i++)
        off[i] = (uint8_t)(r[1 + i / 4u] >> (8u * (i % 4u)));
    return BNO_OK;
}

При старте — обратно в микросхему, до перехода в NDOF:

/* при старте: годная запись во флеше — смещения в микросхему (в CONFIG), иначе калибровать заново */
static void restore_offsets(void)
{
    uint8_t off[BNO_OFFSETS_LEN];
    if (bno_rec_unpack((const uint32_t *)CAL_ADDR, off) == BNO_OK && bno_offsets_write(&dev, off) == BNO_OK)
        restored = 1;
}

В модели запись — 42303535 FFF6000E 01400014 01E0FF10 FFFB0008 03D50006 00000320 D6E446B8; разбор возвращает те же 22 байта, один испорченный бит — BNO_E_CRC, −8, стёртая страница — BNO_E_EMPTY, −9.

Что даёт восстановление — новое включение той же платы, курс 30°, NDOF, курс через 0,5…3 с:

Без смещений жёсткое железо 20, −15, 30 мкТл уводит курс на 40°, пока плату снова не покрутят «восьмёркой». Смещения из флеша дают градус сразу. Записанные в рабочем режиме — пропадают. И ещё: в модели после восстановления CALIB_STAT остаётся 0300, хотя смещения работают, — ждать 0xFF после восстановления не надо (как ведёт себя кристалл — не проверено).

99е.5 Растягивание такта, 100 кГц и UART

По документации BNO055 держит SCL низким, пока готовит ответ: ведомый так может, и ведущий I2C в F1 ждёт его сам — проверяет линию после TRISE и, пока она низкая, не считает высокий уровень (RM0008, с. 758). Беда бывает у ведущих, которые этого не умеют, и у программы: драйвер ждёт каждый флаг SB, ADDR, RXNE, BTF с тайм-аутом, и если тайм-аут короче, чем микросхема держит линию, чтение обрывается. Сколько BNO055 держит SCL, документы на диске не говорят; в модели — допущение: 300 мкс на каждое чтение или 1500 мкс на каждом десятом.

Кватернион — 102 такта SCL: 1020,00 мкс на 100 кГц и 255,00 на 400. Порт с тайм-аутом 1 мс на флаг теряет каждое долгое чтение — 10 из 100; с 5 мс — ни одного. Прошивка этого раздела берёт 100 кГц: на 100 Гц кватерниона это 10,2 % шины. CCR = 180 и TRISE = 37 — 36 МГц / (2 × 180) и 1000 нс × 36 МГц + 1 (RM0008, с. 783–784). После тайм-аута шина может остаться занятой — как её освободить, в 17.5 и 17.6.

UART. Выводы PS1 = 1, PS0 = 0 переводят BNO055 на UART (по документации, 115200 бод). Регистры те же, обмен — кадрами: запрос 0xAA, 0x01 (чтение) или 0x00 (запись), номер регистра, длина и данные; ответ на чтение — 0xBB, длина, данные; на запись — 0xEE, 0x01; ошибка — 0xEE и код, 0x07 — внутренняя шина занята, повторить. Растягивания здесь нет, и шина не нужна никому, кроме датчика. Драйвер тот же, меняется только транспорт:

/* 0 — успех; больше нуля — код из ответа 0xEE; BNO_E_UART — ответ не по формату */
int bno_uart_resp(const uint8_t *f, uint32_t nf, int rd, uint8_t *data, uint8_t n)
{
    if (nf >= 2u && f[0] == 0xEEu)
        return (!rd && f[1] == 0x01u) ? BNO_OK : (int)f[1];    /* 0x01 — запись прошла; остальное — ошибка */
    if (!rd || nf != 2u + n || f[0] != 0xBBu || f[1] != n)
        return BNO_E_UART;
    for (uint32_t i = 0; i < n; i++)
        data[i] = f[2 + i];
    return BNO_OK;
}

В модели тот же драйвер по UART опознал коды A0 FB 32 0F, сбросил, настроил и включил NDOF; кватернион — 14 байт в обе стороны, 1215,28 мкс. На ответ 0xEE 0x07 драйвер повторил запрос: 2 обмена, 1 повтор, 1736,11 мкс. Прошивка fw99g.c — только I2C; порт UART для F103 не написан.

99е.6 Слияние внутри или фильтр 99б на МК

Один и тот же стенд, одна модель движения: 55 с — покой 5 с, 25 с вращений до 60, 30 и 45 °/с, покой, поворот вокруг вертикали со скоростью 90 °/с, качание. Смещение гироскопа 0,50, −0,30, 0,40 °/с, поле и акселерометр откалиброваны. Первый путь: BNO055 в NDOF, драйвер 100 раз в секунду читает кватернион. Второй: BNO055 в AMG (сырые датчики, без слияния), драйвер читает 18 байт и отдаёт их фильтру Маджвика из 99б — ahrs99b.c без изменений, β = 1 первые 3 с, дальше 0,033, смещение по паузам, — на том же Cortex-M3 в QEMU. Ошибки — после первых 5 с:

Точность первой строки — точность фильтра модели (Махони с интегралом, double), а не настоящего BNO055: его алгоритм закрыт, сравнить можно лишь устройство двух путей. Фильтр 99б на этом опыте нашёл смещение на 15 паузах.

Что видно и без закрытого алгоритма:

• Шина. Кватернион — 8 байт, 255,00 мкс на 400 кГц; сырые датчики — 18 байт, 480,00 мкс.
• Процессор. Шаг ahrs_update на Cortex-M3 — 13 900 команд, при 72 МГц не меньше 193 мкс, на 100 Гц — не меньше 1,9 % процессора (99б.6). С BNO055 в NDOF этих команд нет: только разбор 8 байт.
• Ток микроконтроллера. Эти 1,9 % — время Run вместо Sleep. По таблицам DocID13587 (не больше 32,8 мА в Run и 7,5 мА в Sleep на 72 МГц) это около 0,49 мА в среднем — 1,93 % от разницы 25,3 мА. Ток самого BNO055 в режимах по документам на диске не проверить: слияние не исчезает, оно переезжает в микросхему.
• Задержка. Слияние BNO055, по документации, обновляется 100 раз в секунду: данным в регистрах до 10 мс. Фильтр на МК может бежать чаще, если датчик отдаёт сырые данные чаще.
• Свобода. Свой фильтр можно настроить, отключить магнитометр рядом с мотором, проверить. Слияние в микросхеме — как есть.

99е.7 Грабли

Не отвечает на 0x28. COM3 на питании — адрес 0x29; драйвер пробует оба: в модели 3 обмена вместо 2. Или PS1 стоит в 1 — микросхема на UART.

После сброса — NACK. Микросхема загружается: в модели 650 мс. Опрашивать CHIP_ID, а не слать настройку вслепую.

Настройка «не применяется». Записи сразу после смены режима теряются: переход в CONFIG — 19 мс, из него — 7 мс. В модели UNIT_SEL и AXIS_MAP_CONFIG, записанные без паузы, остались 00 и 24. Единицы, оси и смещения пишутся только в CONFIG — в NDOF запись тоже молча пропадает.

Калибровка теряется при каждом включении. Смещения живут в микросхеме до сброса. Без восстановления в модели курс ошибается на 40°, с 22 байтами из флеша — на 1°. Сохранять один раз при CALIB_STAT = 0xFF, возвращать при старте в CONFIG.

Магнитометр не калибруется. Плату крутят на столе: точки в одной плоскости, в модели поле так и стоит на 0. Нужна «восьмёрка» в воздухе.

Рядом мотор — курс уехал. Поле мотора складывается с земным. В модели откалиброванной плате добавили 30, −20, 10 мкТл на 4 с: курс в NDOF ушёл на 38,64°, через 2 с после выключения ещё 26,71°, статус поля упал с 3 до 1. В IMU та же помеха — −0,14°: магнитометр не используется, но и севера нет. Датчик — подальше от моторов и проводов с током (99г.6), или IMU и курс от другого источника.

Оси перепутаны. Микросхема повёрнута на плате: в модели курс 270,31° вместо 0. Раскладка из P0…P7 под крепление; перестановка без знака (левая тройка) драйвером не пропускается.

Курс прыгает у вертикали. Углы Эйлера у ±90°: скачок 100,25° за 50 мс при повороте кватерниона на 1,81°. Брать кватернион.

Угловая скорость в 56 раз не та. Делят на 900, а UNIT_SEL по умолчанию — °/с и 16 кодов.

Чтение обрывается раз в несколько секунд. Микросхема держит SCL дольше тайм-аута порта: в модели 10 ошибок из 100 при 1 мс и ни одной при 5 мс.

Питание. Сама микросхема — не для 5 В (по документации); модули с регулятором и сдвигом уровней питают от 5 В, голую микросхему и модуль без регулятора — только от 3,3 В. PB6 и PB7 у F103 выдерживают 5 В (FT, DocID13587, с. 32), но подтяжки к 5 В на линии бьют уже по BNO055.

99е.8 Итоги

• Адрес 0x28 или 0x29 по COM3; коды A0 FB 32 0F с 0x00; после сброса (0x20 в 0x3F) — 650 мс без ответа в модели, опрашивать CHIP_ID.
• Режим — OPR_MODE (0x3D): CONFIG 0x00, IMU 0x08, NDOF 0x0C; в CONFIG — 19 мс, из него — 7 мс; записи во время смены и настройка вне CONFIG теряются.
• Кватернион — 8 байт с 0x20, 1 = 16384; углы — 1° = 16; LIA и GRV — 1 м/с² = 100. У ±90° углы Эйлера дают скачок курса 100,25° за 50 мс — брать кватернион.
• Оси — AXIS_MAP_CONFIG и AXIS_MAP_SIGN только правой тройкой: P0 вернул курс 0,31° вместо 270,31°.
• Калибровка: CALIB_STAT по 2 бита; в модели гироскоп — 1,00 с, «восьмёрка» — 13,00 с, шесть положений — 34,51 с. 22 байта с 0x55 — в флеш при 0xFF, обратно в CONFIG при старте: курс 1,08° вместо −40,71°.
• Растягивание такта: F1 ждёт сам (RM0008, с. 758), но тайм-аут порта — миллисекунды: при 1 мс в модели 10 обрывов из 100, при 5 мс — 0. Или UART (PS1 = 1): 14 байт кватерниона — 1215,28 мкс.
• Слияние внутри снимает с Cortex-M3 13 900 команд на шаг фильтра 99б (не меньше 1,9 % процессора на 100 Гц) и 10 байт шины на отсчёт. В модели наклон 0,22° и курс 0,41° СКО против 0,46° и 2,15° у фильтра 99б, но точность настоящего слияния BNO055 по документам на диске не проверить.

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

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

/* bno99g.h — раздел 99е: драйвер BNO055 — опознание, сброс, режимы работы, единицы, оси, кватернион и углы Эйлера,
 * линейное ускорение и тяжесть, статус калибровки, 22 байта смещений и их запись во флеш, самотест, кадры UART.
 * Регистров микроконтроллера не знает: говорит с микросхемой только через порт bno_port_*. В прошивке fw99g.c порт —
 * I2C1 STM32F103, в стенде host99g.c — модель model99g.py.
 * Карта регистров, коды режимов, задержки и формат кадров UART — по документации производителя BNO055 (в проекте её
 * нет, не проверено); адрес 0x28, коды A0 FB 32 0F, OPR_MODE 0x3D и коды 0x00…0x0C, PWR_MODE 0x3E, SYS_TRIGGER 0x3F
 * (0x20 — сброс), AXIS_MAP 0x41/0x42, данные 0x08…0x34 и их цена разряда совпадают с bno055.py из OpenMV. */
#ifndef BNO99G_H
#define BNO99G_H
#include <stdint.h>

/* ── адрес и регистры страницы 0 ──────────────────────────────────────── */
#define BNO_ADDR_A          0x28u       /* вывод COM3 на земле */
#define BNO_ADDR_B          0x29u       /* COM3 на питании */
#define BNO_CHIP_ID         0x00u       /* 4 байта: кристалл, акселерометр, магнитометр, гироскоп */
#define BNO_ID_CHIP         0xA0u
#define BNO_ID_ACC          0xFBu
#define BNO_ID_MAG          0x32u
#define BNO_ID_GYR          0x0Fu
#define BNO_PAGE_ID         0x07u       /* 0 — данные и настройка, 1 — настройка датчиков */
#define BNO_ACC_DATA        0x08u       /* 18 байт: ускорение, поле, угловая скорость; X, Y, Z младшим первым */
#define BNO_EUL_DATA        0x1Au       /* 6 байт: курс, крен, тангаж */
#define BNO_QUA_DATA        0x20u       /* 8 байт: w, x, y, z */
#define BNO_LIA_DATA        0x28u       /* 6 байт линейного ускорения, следом 6 байт тяжести (GRV, 0x2E) */
#define BNO_GRV_DATA        0x2Eu
#define BNO_TEMP            0x34u
#define BNO_CALIB_STAT      0x35u       /* биты 7:6 — система, 5:4 — гироскоп, 3:2 — акселерометр, 1:0 — магнитометр */
#define BNO_ST_RESULT       0x36u       /* 3 — процессор, 2 — гироскоп, 1 — магнитометр, 0 — акселерометр */
#define BNO_SYS_STATUS      0x39u
#define BNO_SYS_ERR         0x3Au
#define BNO_UNIT_SEL        0x3Bu
#define BNO_OPR_MODE        0x3Du
#define BNO_PWR_MODE        0x3Eu
#define BNO_SYS_TRIGGER     0x3Fu
#define BNO_AXIS_MAP_CONFIG 0x41u       /* за ним AXIS_MAP_SIGN (0x42) */
#define BNO_OFFSETS         0x55u       /* 22 байта: смещения акселерометра, магнитометра, гироскопа, два радиуса */
#define BNO_OFFSETS_LEN     22u

/* ── режимы, питание, единицы, команды ───────────────────────────────── */
enum { BNO_MODE_CONFIG = 0x00, BNO_MODE_ACCONLY = 0x01, BNO_MODE_MAGONLY = 0x02, BNO_MODE_GYRONLY = 0x03,
       BNO_MODE_ACCMAG = 0x04, BNO_MODE_ACCGYRO = 0x05, BNO_MODE_MAGGYRO = 0x06, BNO_MODE_AMG = 0x07,
       BNO_MODE_IMU = 0x08, BNO_MODE_COMPASS = 0x09, BNO_MODE_M4G = 0x0A, BNO_MODE_NDOF_FMC_OFF = 0x0B,
       BNO_MODE_NDOF = 0x0C };
enum { BNO_PWR_NORMAL = 0x00, BNO_PWR_LOW = 0x01, BNO_PWR_SUSPEND = 0x02 };
#define BNO_UNIT_MG         0x01u       /* ускорение в мg, иначе м/с² */
#define BNO_UNIT_RPS        0x02u       /* угловая скорость в рад/с, иначе °/с */
#define BNO_UNIT_RAD        0x04u       /* углы Эйлера в радианах, иначе в градусах */
#define BNO_UNIT_FAHR       0x10u       /* температура в °F, иначе в °C */
#define BNO_TRIG_SELFTEST   0x01u
#define BNO_TRIG_RST_SYS    0x20u
#define BNO_ST_ALL          0x0Fu
#define BNO_SYS_FUSION      5u          /* SYS_STATUS: слияние работает */
#define BNO_SYS_ERROR       1u          /* SYS_STATUS: ошибка, подробности в SYS_ERR */

/* ── цена разряда ─────────────────────────────────────────────────────── */
#define BNO_LSB_DEG         16          /* углы Эйлера: 16 кодов на градус */
#define BNO_LSB_RAD         900         /* или 900 на радиан */
#define BNO_LSB_QUAT        16384       /* кватернион: 2^14 на единицу */
#define BNO_LSB_MS2         100         /* ускорение, LIA и GRV: 100 кодов на 1 м/с² */
#define BNO_LSB_DPS         16          /* угловая скорость: 16 кодов на °/с */
#define BNO_LSB_UT          16          /* поле: 16 кодов на мкТл */

/* ── задержки ─────────────────────────────────────────────────────────── */
#define BNO_T_FROM_CONFIG_MS 7u         /* из CONFIG в любой рабочий режим */
#define BNO_T_TO_CONFIG_MS  19u         /* из рабочего режима в CONFIG */
#define BNO_BOOT_MS         1000u       /* после сброса ждать ответа не дольше */
#define BNO_BOOT_POLL_MS    10u
#define BNO_SELFTEST_MS     1000u

enum { BNO_OK = 0, BNO_E_NACK = -1, BNO_E_BUS = -2, BNO_E_ID = -3, BNO_E_TIMEOUT = -4, BNO_E_MODE = -5,
       BNO_E_SELFTEST = -6, BNO_E_ARG = -7, BNO_E_CRC = -8, BNO_E_EMPTY = -9, BNO_E_UART = -10 };

typedef struct {
    uint8_t addr, mode, units, uart;    /* uart = 1: кадры UART вместо I2C */
    uint8_t id[4];
    uint32_t xfers, errors, retries, boot_ms;
} bno_t;

/* ── порт ─────────────────────────────────────────────────────────────── */
/* I2C: ntx байт записи, затем (если nrx) повторный старт и nrx байт чтения; ntx = nrx = 0 — только адрес.
 * 0, BNO_E_NACK или BNO_E_BUS (в том числе — истёк тайм-аут, пока микросхема держала SCL) */
int bno_port_xfer(uint8_t addr7, const uint8_t *tx, uint32_t ntx, uint8_t *rx, uint32_t nrx);
/* UART: отправить ntx байт и принять до max байт ответа; число принятых или BNO_E_BUS */
int bno_port_uart(const uint8_t *tx, uint32_t ntx, uint8_t *rx, uint32_t max);
void bno_port_delay_us(uint32_t us);

/* ── регистры и режимы ────────────────────────────────────────────────── */
int bno_write_regs(bno_t *d, uint8_t reg, const uint8_t *buf, uint32_t n);
int bno_write_reg(bno_t *d, uint8_t reg, uint8_t val);
int bno_read_regs(bno_t *d, uint8_t reg, uint8_t *buf, uint32_t n);
int bno_probe(bno_t *d);
int bno_reset(bno_t *d);
int bno_set_mode(bno_t *d, uint8_t mode);
int bno_set_mode_nowait(bno_t *d, uint8_t mode);
int bno_axis_remap(bno_t *d, uint8_t map, uint8_t sign);
int bno_init(bno_t *d, uint8_t units, uint8_t map, uint8_t sign);
int bno_selftest(bno_t *d, uint8_t *st);
int bno_status(bno_t *d, uint8_t *sys, uint8_t *err);

/* ── данные ───────────────────────────────────────────────────────────── */
int16_t bno_le16(const uint8_t *b);
int bno_read_quat(bno_t *d, int16_t q[4]);
int bno_read_euler(bno_t *d, int16_t e[3]);
int bno_read_lia_grv(bno_t *d, int16_t lia[3], int16_t grv[3]);
int bno_read_amg(bno_t *d, int16_t acc[3], int16_t mag[3], int16_t gyr[3]);
int32_t bno_euler_cdeg(int16_t raw);
int bno_calib(bno_t *d, uint8_t *sys, uint8_t *gyr, uint8_t *acc, uint8_t *mag);

/* ── смещения и флеш ──────────────────────────────────────────────────── */
#define BNO_REC_WORDS       8u          /* метка, 22 байта смещений и 2 байта нулей, CRC-32 — 32 байта */
#define BNO_REC_MAGIC       0x42303535u /* «B055» */
int bno_offsets_read(bno_t *d, uint8_t off[BNO_OFFSETS_LEN]);
int bno_offsets_write(bno_t *d, const uint8_t off[BNO_OFFSETS_LEN]);
uint32_t bno_crc32(const void *p, uint32_t n);
void bno_rec_pack(const uint8_t off[BNO_OFFSETS_LEN], uint32_t r[BNO_REC_WORDS]);
int bno_rec_unpack(const uint32_t r[BNO_REC_WORDS], uint8_t off[BNO_OFFSETS_LEN]);

/* ── кадры UART (PS1 = 1, PS0 = 0) ───────────────────────────────────── */
uint32_t bno_uart_req(uint8_t *f, int rd, uint8_t reg, const uint8_t *data, uint8_t n);
int bno_uart_resp(const uint8_t *f, uint32_t nf, int rd, uint8_t *data, uint8_t n);

#endif

bno99g.c — драйвер: опознание, сброс, режимы, оси, самотест, данные, калибровка, запись для флеша, кадры UART.

/* bno99g.c — раздел 99е: драйвер BNO055. Один и тот же файл собирается в прошивку (armclang, Cortex-M3) и в стенд
 * (arm-none-eabi-gcc, Cortex-M3 в QEMU): микроконтроллера он не знает, только порт bno_port_*.
 * Всё целыми: кватернион — в 1/16384, углы — в сотых градуса, ускорения — в сотых м/с², как в регистрах. */
#include "bno99g.h"

#define UART_TRIES 3u                   /* ответ 0xEE 0x07 — «шина занята»: повторить */

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

/* ── кадры UART: запрос 0xAA, чтение/запись, регистр, длина[, данные]; ответ 0xBB, длина, данные или 0xEE, код ── */
uint32_t bno_uart_req(uint8_t *f, int rd, uint8_t reg, const uint8_t *data, uint8_t n)
{
    f[0] = 0xAAu;
    f[1] = rd ? 0x01u : 0x00u;
    f[2] = reg;
    f[3] = n;
    if (rd)
        return 4u;
    for (uint32_t i = 0; i < n; i++)
        f[4 + i] = data[i];
    return 4u + n;
}

/* 0 — успех; больше нуля — код из ответа 0xEE; BNO_E_UART — ответ не по формату */
int bno_uart_resp(const uint8_t *f, uint32_t nf, int rd, uint8_t *data, uint8_t n)
{
    if (nf >= 2u && f[0] == 0xEEu)
        return (!rd && f[1] == 0x01u) ? BNO_OK : (int)f[1];    /* 0x01 — запись прошла; остальное — ошибка */
    if (!rd || nf != 2u + n || f[0] != 0xBBu || f[1] != n)
        return BNO_E_UART;
    for (uint32_t i = 0; i < n; i++)
        data[i] = f[2 + i];
    return BNO_OK;
}

static int uart_xfer(bno_t *d, int rd, uint8_t reg, uint8_t *buf, uint32_t n)
{
    uint8_t f[4 + BNO_OFFSETS_LEN + 2];
    int rc = BNO_E_UART;
    if (n > BNO_OFFSETS_LEN)
        return BNO_E_ARG;
    for (uint32_t t = 0; t < UART_TRIES; t++) {
        uint32_t nf = bno_uart_req(f, rd, reg, buf, (uint8_t)n);
        int got = xfer_done(d, bno_port_uart(f, nf, f, sizeof f));
        if (got < 0)
            return got;
        rc = bno_uart_resp(f, (uint32_t)got, rd, buf, (uint8_t)n);
        if (rc != 0x07)                                         /* 0x07 — переполнение шины внутри: повтор */
            break;
        d->retries++;
    }
    return rc > 0 ? BNO_E_UART : rc;
}

int bno_write_regs(bno_t *d, uint8_t reg, const uint8_t *buf, uint32_t n)
{
    uint8_t b[1 + BNO_OFFSETS_LEN];
    if (n > BNO_OFFSETS_LEN)
        return BNO_E_ARG;
    if (d->uart) {
        for (uint32_t i = 0; i < n; i++)
            b[i] = buf[i];
        return uart_xfer(d, 0, reg, b, n);
    }
    b[0] = reg;
    for (uint32_t i = 0; i < n; i++)
        b[1 + i] = buf[i];
    return xfer_done(d, bno_port_xfer(d->addr, b, n + 1u, 0, 0));
}

int bno_write_reg(bno_t *d, uint8_t reg, uint8_t val)
{
    return bno_write_regs(d, reg, &val, 1);
}

/* номер регистра, повторный старт, n байт подряд */
int bno_read_regs(bno_t *d, uint8_t reg, uint8_t *buf, uint32_t n)
{
    if (d->uart)
        return uart_xfer(d, 1, reg, buf, n);
    return xfer_done(d, bno_port_xfer(d->addr, &reg, 1, buf, n));
}

/* кто на шине: 0x28 (COM3 = 0) или 0x29 (COM3 = 1); четыре кода — кристалл A0, акселерометр FB, магнитометр 32, гироскоп 0F */
int bno_probe(bno_t *d)
{
    static const uint8_t addr[2] = {BNO_ADDR_A, BNO_ADDR_B};
    for (int i = 0; i < 4; i++)
        d->id[i] = 0;
    for (int i = 0; i < (d->uart ? 1 : 2); i++) {
        d->addr = addr[i];
        if (!d->uart && xfer_done(d, bno_port_xfer(d->addr, 0, 0, 0, 0)) != BNO_OK)
            continue;                                           /* нет ответа на этот адрес */
        if (bno_read_regs(d, BNO_CHIP_ID, d->id, 4) != BNO_OK)
            return BNO_E_BUS;
        return d->id[0] == BNO_ID_CHIP ? BNO_OK : BNO_E_ID;
    }
    return BNO_E_NACK;
}

/* сброс: SYS_TRIGGER = 0x20, затем ждать, пока микросхема снова ответит кодом A0 */
int bno_reset(bno_t *d)
{
    uint8_t id;
    int rc = bno_write_reg(d, BNO_SYS_TRIGGER, BNO_TRIG_RST_SYS);
    if (rc != BNO_OK)
        return rc;
    for (uint32_t t = BNO_BOOT_POLL_MS; t <= BNO_BOOT_MS; t += BNO_BOOT_POLL_MS) {
        bno_port_delay_us(BNO_BOOT_POLL_MS * 1000u);
        if (bno_read_regs(d, BNO_CHIP_ID, &id, 1) == BNO_OK && id == BNO_ID_CHIP) {
            d->boot_ms = t;
            d->mode = BNO_MODE_CONFIG;
            return BNO_OK;
        }
    }
    return BNO_E_TIMEOUT;
}

/* только запись OPR_MODE — без паузы и проверки; так делать не надо (99е.7) */
int bno_set_mode_nowait(bno_t *d, uint8_t mode)
{
    int rc = bno_write_reg(d, BNO_OPR_MODE, mode);
    if (rc == BNO_OK)
        d->mode = mode;
    return rc;
}

/* режим: из рабочего в рабочий — через CONFIG; после записи — 19 мс в CONFIG или 7 мс из него; затем прочитать обратно */
int bno_set_mode(bno_t *d, uint8_t mode)
{
    uint8_t v;
    int rc;
    if (mode > BNO_MODE_NDOF)
        return BNO_E_ARG;
    if (mode != BNO_MODE_CONFIG && d->mode != BNO_MODE_CONFIG && (rc = bno_set_mode(d, BNO_MODE_CONFIG)) != BNO_OK)
        return rc;
    if ((rc = bno_write_reg(d, BNO_OPR_MODE, mode)) != BNO_OK)
        return rc;
    bno_port_delay_us((mode == BNO_MODE_CONFIG ? BNO_T_TO_CONFIG_MS : BNO_T_FROM_CONFIG_MS) * 1000u);
    if ((rc = bno_read_regs(d, BNO_OPR_MODE, &v, 1)) != BNO_OK)
        return rc;
    d->mode = v & 0x0Fu;
    return d->mode == mode ? BNO_OK : BNO_E_MODE;
}

/* оси: map — три поля по 2 бита (X — 1:0, Y — 3:2, Z — 5:4: номер исходной оси), sign — биты 2, 1, 0 = минус у X, Y, Z.
 * Правая тройка остаётся правой: перестановка нечётная — минусов нечётное число, чётная — чётное. Только в CONFIG */
int bno_axis_remap(bno_t *d, uint8_t map, uint8_t sign)
{
    uint8_t ax[3], b[2] = {map, sign};
    int odd = 0, minus = 0;
    for (int k = 0; k < 3; k++)
        ax[k] = (uint8_t)(map >> (2 * k) & 3u);
    if (map > 0x3Fu || sign > 7u || ax[0] > 2u || ax[1] > 2u || ax[2] > 2u || ax[0] == ax[1] || ax[0] == ax[2] || ax[1] == ax[2])
        return BNO_E_ARG;
    odd = (ax[0] != 0u) + (ax[1] != 1u) + (ax[2] != 2u) == 2;  /* ровно две оси не на месте — одна перестановка пары */
    for (int k = 0; k < 3; k++)
        minus += sign >> k & 1u;
    if ((minus & 1) != odd)
        return BNO_E_ARG;                                       /* левая тройка: углы и кватернион не сойдутся с осями */
    if (d->mode != BNO_MODE_CONFIG)
        return BNO_E_MODE;
    return bno_write_regs(d, BNO_AXIS_MAP_CONFIG, b, 2);
}

/* CONFIG, обычное питание, страница 0, единицы, оси; самотест после включения — все четыре бита */
int bno_init(bno_t *d, uint8_t units, uint8_t map, uint8_t sign)
{
    uint8_t st;
    int rc;
    d->mode = 0xFFu;                                            /* не знаем, в каком режиме микросхема: сначала в CONFIG */
    if ((rc = bno_write_reg(d, BNO_OPR_MODE, BNO_MODE_CONFIG)) != BNO_OK)
        return rc;
    bno_port_delay_us(BNO_T_TO_CONFIG_MS * 1000u);
    d->mode = BNO_MODE_CONFIG;
    if ((rc = bno_write_reg(d, BNO_PAGE_ID, 0u)) != BNO_OK || (rc = bno_write_reg(d, BNO_PWR_MODE, BNO_PWR_NORMAL)) != BNO_OK
        || (rc = bno_write_reg(d, BNO_UNIT_SEL, units)) != BNO_OK || (rc = bno_axis_remap(d, map, sign)) != BNO_OK)
        return rc;
    d->units = units;
    if ((rc = bno_read_regs(d, BNO_ST_RESULT, &st, 1)) != BNO_OK)
        return rc;
    return st == BNO_ST_ALL ? BNO_OK : BNO_E_SELFTEST;
}

/* SYS_STATUS и SYS_ERR одним чтением */
int bno_status(bno_t *d, uint8_t *sys, uint8_t *err)
{
    uint8_t b[2];
    int rc = bno_read_regs(d, BNO_SYS_STATUS, b, 2);
    *sys = b[0];
    *err = b[1];
    return rc;
}

/* самотест по команде: только в CONFIG; ждать, пока SYS_STATUS уйдёт из 4 — «идёт самотест» */
int bno_selftest(bno_t *d, uint8_t *st)
{
    uint8_t sys = 4u, err;
    int rc;
    *st = 0;
    if (d->mode != BNO_MODE_CONFIG)
        return BNO_E_MODE;
    if ((rc = bno_write_reg(d, BNO_SYS_TRIGGER, BNO_TRIG_SELFTEST)) != BNO_OK)
        return rc;
    for (uint32_t t = 0; sys == 4u && t < BNO_SELFTEST_MS; t += BNO_BOOT_POLL_MS) {
        bno_port_delay_us(BNO_BOOT_POLL_MS * 1000u);
        if ((rc = bno_status(d, &sys, &err)) != BNO_OK)
            return rc;
    }
    if (sys == 4u)
        return BNO_E_TIMEOUT;
    if ((rc = bno_read_regs(d, BNO_ST_RESULT, st, 1)) != BNO_OK)
        return rc;
    return *st == BNO_ST_ALL ? BNO_OK : BNO_E_SELFTEST;
}

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

static int read_vec(bno_t *d, uint8_t reg, int16_t *v, uint32_t n)
{
    uint8_t b[18];
    int rc = bno_read_regs(d, reg, b, 2u * n);
    for (uint32_t i = 0; i < n; i++)
        v[i] = rc == BNO_OK ? bno_le16(b + 2 * i) : 0;
    return rc;
}

/* кватернион w, x, y, z: 8 байт с 0x20, 1 = 16384 */
int bno_read_quat(bno_t *d, int16_t q[4])
{
    return read_vec(d, BNO_QUA_DATA, q, 4);
}

/* курс, крен, тангаж: 6 байт с 0x1A, 16 кодов на градус (или 900 на радиан) */
int bno_read_euler(bno_t *d, int16_t e[3])
{
    return read_vec(d, BNO_EUL_DATA, e, 3);
}

/* линейное ускорение и тяжесть одной пачкой: 12 байт с 0x28, 100 кодов на 1 м/с² */
int bno_read_lia_grv(bno_t *d, int16_t lia[3], int16_t grv[3])
{
    int16_t v[6];
    int rc = read_vec(d, BNO_LIA_DATA, v, 6);
    for (int k = 0; k < 3; k++) {
        lia[k] = v[k];
        grv[k] = v[3 + k];
    }
    return rc;
}

/* сырые датчики одной пачкой: 18 байт с 0x08 — ускорение, поле, угловая скорость */
int bno_read_amg(bno_t *d, int16_t acc[3], int16_t mag[3], int16_t gyr[3])
{
    int16_t v[9];
    int rc = read_vec(d, BNO_ACC_DATA, v, 9);
    for (int k = 0; k < 3; k++) {
        acc[k] = v[k];
        mag[k] = v[3 + k];
        gyr[k] = v[6 + k];
    }
    return rc;
}

/* угол Эйлера в сотых градуса из кода в градусном режиме */
int32_t bno_euler_cdeg(int16_t raw)
{
    int32_t a = (int32_t)raw * 100;
    return a >= 0 ? (a + 8) / 16 : -((-a + 8) / 16);
}

/* CALIB_STAT: по два бита, 0 — не откалиброван, 3 — полностью */
int bno_calib(bno_t *d, uint8_t *sys, uint8_t *gyr, uint8_t *acc, uint8_t *mag)
{
    uint8_t c = 0;
    int rc = bno_read_regs(d, BNO_CALIB_STAT, &c, 1);
    *sys = c >> 6 & 3u;
    *gyr = c >> 4 & 3u;
    *acc = c >> 2 & 3u;
    *mag = c & 3u;
    return rc;
}

/* 22 байта смещений: в CONFIG, одной пачкой, потом вернуть прежний режим */
int bno_offsets_read(bno_t *d, uint8_t off[BNO_OFFSETS_LEN])
{
    uint8_t prev = d->mode;
    int rc;
    if ((rc = bno_set_mode(d, BNO_MODE_CONFIG)) != BNO_OK || (rc = bno_read_regs(d, BNO_OFFSETS, off, BNO_OFFSETS_LEN)) != BNO_OK)
        return rc;
    return prev == BNO_MODE_CONFIG ? BNO_OK : bno_set_mode(d, prev);
}

/* записать сохранённые смещения: только в CONFIG (в рабочем режиме запись не принимается), потом прежний режим */
int bno_offsets_write(bno_t *d, const uint8_t off[BNO_OFFSETS_LEN])
{
    uint8_t prev = d->mode;
    int rc;
    if ((rc = bno_set_mode(d, BNO_MODE_CONFIG)) != BNO_OK || (rc = bno_write_regs(d, BNO_OFFSETS, off, BNO_OFFSETS_LEN)) != BNO_OK)
        return rc;
    return prev == BNO_MODE_CONFIG ? BNO_OK : bno_set_mode(d, prev);
}

/* ── запись для флеша: метка, 22 байта смещений и два нуля, CRC-32 ── */
uint32_t bno_crc32(const void *p, uint32_t n)
{
    const uint8_t *b = p;
    uint32_t c = 0xFFFFFFFFu;
    while (n--) {
        c ^= *b++;
        for (int i = 0; i < 8; i++)
            c = c & 1u ? c >> 1 ^ 0xEDB88320u : c >> 1;
    }
    return ~c;
}

void bno_rec_pack(const uint8_t off[BNO_OFFSETS_LEN], uint32_t r[BNO_REC_WORDS])
{
    r[0] = BNO_REC_MAGIC;
    for (uint32_t i = 1; i < BNO_REC_WORDS - 1u; i++)
        r[i] = 0;
    for (uint32_t i = 0; i < BNO_OFFSETS_LEN; i++)
        r[1 + i / 4u] |= (uint32_t)off[i] << (8u * (i % 4u));
    r[BNO_REC_WORDS - 1u] = bno_crc32(r, 4u * (BNO_REC_WORDS - 1u));
}

int bno_rec_unpack(const uint32_t r[BNO_REC_WORDS], uint8_t off[BNO_OFFSETS_LEN])
{
    if (r[0] != BNO_REC_MAGIC)
        return BNO_E_EMPTY;                                     /* стёртая страница: 0xFFFFFFFF */
    if (bno_crc32(r, 4u * (BNO_REC_WORDS - 1u)) != r[BNO_REC_WORDS - 1u])
        return BNO_E_CRC;                                       /* запись побита или недописана */
    for (uint32_t i = 0; i < BNO_OFFSETS_LEN; i++)
        off[i] = (uint8_t)(r[1 + i / 4u] >> (8u * (i % 4u)));
    return BNO_OK;
}

fw99g.c — прошивка F103: I2C1 на 100 кГц с тайм-аутом под растянутый SCL, смещения из флеша и в флеш, кватернион 100 раз в секунду.

/* fw99g.c — раздел 99е: BNO055 на STM32F103 (BluePill), 72 МГц, APB1 = 36 МГц.
 * I2C1: PB6 — SCL, PB7 — SDA (открытый сток, 100 кГц). Выводы модуля: COM3 на земле (адрес 0x28), PS0 = PS1 = 0 (I2C).
 * Запуск: кто на шине, сброс, CONFIG, единицы (м/с², °/с, градусы, °C), оси по умолчанию; смещения — из последней
 * страницы флеша, если там годная запись; режим NDOF. Дальше SysTick 100 раз в секунду: кватернион (8 байт), раз в
 * секунду — CALIB_STAT; как только он 0xFF и смещения ещё не сохранены — 22 байта в CONFIG, запись во флеш, снова NDOF.
 * Порт терпит растянутый SCL: ожидание каждого флага — до 5 мс.
 * Через отладчик: quat[], calib, saved, restored, bus_errors. Тактирование до 72 МГц — как в разделе 6. */
#include "stm32f10x.h"
#include "bno99g.h"

#define CPU_HZ          72000000u
#define APB1_MHZ        36u
#define I2C_TIMEOUT_US  5000u           /* байт на 100 кГц — 90 мкс; остальное — запас на растянутый SCL */
#define CAL_ADDR        0x0800FC00u     /* последняя страница 1 Кбайт у F103C8 */

static bno_t dev;
static volatile uint32_t ticks;
volatile int16_t quat[4];
volatile uint8_t calib, saved, restored;
volatile uint32_t bus_errors;

void SysTick_Handler(void)
{
    ticks++;
}

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

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

int bno_port_uart(const uint8_t *tx, uint32_t ntx, uint8_t *rx, uint32_t max)
{
    (void)tx;
    (void)ntx;
    (void)rx;
    (void)max;
    return BNO_E_BUS;                                   /* в этой прошивке только I2C */
}

static void gpio_i2c1_init(void)
{
    RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
    RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;
    /* PB6, PB7 — альтернативная функция, открытый сток, 50 МГц (1111) */
    GPIOB->CRL = (GPIOB->CRL & 0x00FFFFFFu) | 0xFF000000u;
    I2C1->CR1 = 0;                                      /* настраивать при PE = 0 */
    I2C1->CR2 = APB1_MHZ;
    I2C1->CCR = 180u;                                   /* стандартный режим: 36 МГц / (2 × 180) = 100 кГц */
    I2C1->TRISE = APB1_MHZ + 1u;                        /* 1000 нс × 36 МГц = 36 → 36 + 1 = 37 */
    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 BNO_E_NACK;
        if (expired(t0, I2C_TIMEOUT_US))
            return BNO_E_BUS;                           /* SCL прижат дольше 5 мс или шина встала */
    }
    return BNO_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);
    if (rc == BNO_E_BUS)
        bus_errors++;
    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)) != BNO_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)) != BNO_OK)
            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)) != BNO_OK)
                return rc;
            rx[i] = (uint8_t)I2C1->DR;
        }
        if ((rc = sr1_wait(I2C_SR1_BTF)) != BNO_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)) != BNO_OK)
            return rc;
        I2C1->CR1 |= I2C_CR1_STOP;
        rx[n - 2u] = (uint8_t)I2C1->DR;
        if ((rc = sr1_wait(I2C_SR1_RXNE)) != BNO_OK)
            return rc;
        rx[n - 1u] = (uint8_t)I2C1->DR;
    }
    I2C1->CR1 |= I2C_CR1_ACK;
    return BNO_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) {
        if (expired(t0, I2C_TIMEOUT_US))
            return BNO_E_BUS;
    }
    I2C1->CR1 |= I2C_CR1_ACK | I2C_CR1_START;
    if ((rc = sr1_wait(I2C_SR1_SB)) != BNO_OK)
        return rc;
    I2C1->DR = (uint8_t)(addr7 << 1);
    if ((rc = sr1_wait(I2C_SR1_ADDR)) != BNO_OK)
        return rc;
    (void)I2C1->SR2;
    for (uint32_t i = 0; i < ntx; i++) {
        if ((rc = sr1_wait(I2C_SR1_TXE)) != BNO_OK)
            return rc;
        I2C1->DR = tx[i];
    }
    if (ntx && (rc = sr1_wait(I2C_SR1_BTF)) != BNO_OK)
        return rc;
    if (!rd)
        return BNO_OK;
    if (pos)
        I2C1->CR1 |= I2C_CR1_POS;
    I2C1->CR1 |= I2C_CR1_START;
    if ((rc = sr1_wait(I2C_SR1_SB)) != BNO_OK)
        return rc;
    I2C1->DR = (uint8_t)(addr7 << 1 | 1u);
    return sr1_wait(I2C_SR1_ADDR);
}

int bno_port_xfer(uint8_t addr7, const uint8_t *tx, uint32_t ntx, uint8_t *rx, uint32_t nrx)
{
    int rc;
    if (nrx && !ntx)
        return BNO_E_BUS;
    if ((rc = addr_phase(addr7, tx, ntx, nrx != 0u, nrx == 2u)) != BNO_OK)
        return fail(rc);
    if (!nrx) {
        I2C1->CR1 |= I2C_CR1_STOP;
        return BNO_OK;
    }
    if ((rc = rx_n(rx, nrx)) != BNO_OK)
        return fail(rc);
    return BNO_OK;
}

/* ── смещения во флеше: стереть страницу и записать 16 полуслов (раздел 11.5) ── */
static void flash_wait(void)
{
    while (FLASH->SR & FLASH_SR_BSY) {
    }
}

static int rec_save(const uint32_t r[BNO_REC_WORDS])
{
    const uint16_t *h = (const uint16_t *)r;
    int bad = 0;
    FLASH->KEYR = 0x45670123u;
    FLASH->KEYR = 0xCDEF89ABu;
    flash_wait();
    FLASH->CR |= FLASH_CR_PER;
    FLASH->AR = CAL_ADDR;
    FLASH->CR |= FLASH_CR_STRT;
    flash_wait();
    FLASH->CR &= ~FLASH_CR_PER;
    FLASH->CR |= FLASH_CR_PG;
    for (uint32_t i = 0; i < 2u * BNO_REC_WORDS; i++) {
        *(volatile uint16_t *)(CAL_ADDR + 2u * i) = h[i];
        flash_wait();
        bad |= (FLASH->SR & FLASH_SR_PGERR) != 0u;
    }
    FLASH->CR &= ~FLASH_CR_PG;
    FLASH->CR |= FLASH_CR_LOCK;
    return bad ? BNO_E_CRC : BNO_OK;
}

/* при старте: годная запись во флеше — смещения в микросхему (в CONFIG), иначе калибровать заново */
static void restore_offsets(void)
{
    uint8_t off[BNO_OFFSETS_LEN];
    if (bno_rec_unpack((const uint32_t *)CAL_ADDR, off) == BNO_OK && bno_offsets_write(&dev, off) == BNO_OK)
        restored = 1;
}

/* полная калибровка (0xFF) — один раз прочитать 22 байта и сохранить */
static void save_offsets(void)
{
    uint8_t off[BNO_OFFSETS_LEN];
    uint32_t r[BNO_REC_WORDS];
    if (bno_offsets_read(&dev, off) != BNO_OK)
        return;
    bno_rec_pack(off, r);
    if (rec_save(r) == BNO_OK)
        saved = 1;
}

int main(void)
{
    uint32_t last = 0;
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CYCCNT = 0;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
    gpio_i2c1_init();
    while (bno_probe(&dev) != BNO_OK)
        bno_port_delay_us(100000u);                     /* нет ответа ни на 0x28, ни на 0x29, или код не A0 */
    while (bno_reset(&dev) != BNO_OK || bno_init(&dev, 0u, 0x24u, 0x00u) != BNO_OK)
        bno_port_delay_us(100000u);
    restore_offsets();
    while (bno_set_mode(&dev, BNO_MODE_NDOF) != BNO_OK)
        bno_port_delay_us(100000u);
    SysTick_Config(CPU_HZ / 100u);                      /* 100 раз в секунду — как обновляется слияние */
    for (;;) {
        __WFI();
        if (ticks == last)
            continue;
        last = ticks;
        int16_t q[4];
        if (bno_read_quat(&dev, q) == BNO_OK)
            for (int k = 0; k < 4; k++)
                quat[k] = q[k];
        if (last % 100u == 0u) {
            uint8_t s, g, a, m;
            if (bno_calib(&dev, &s, &g, &a, &m) == BNO_OK) {
                calib = (uint8_t)(s << 6 | g << 4 | a << 2 | m);
                if (calib == 0xFFu && !saved)
                    save_offsets();
            }
        }
    }
}

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