99г. Магнитометр-компас QMC5883L/HMC5883L по I2C: курс, калибровка hard/soft iron, наклон и магнитное склонение

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

Опоры: RM0008 (DocID13902 Rev 17): приём трёх байт и больше ведущим, способ 2, с. 763; приём одного байта, с. 764. DocID13587 Rev 17 (STM32F103x8/xB): PB6 — I2C1_SCL, PB7 — I2C1_SDA, оба FT, с. 32. Устройство I2C — раздел 17, здесь не повторяется. Документов производителей на QMC5883L и HMC5883L в проекте нет. Адрес QMC5883L 0x0D, его регистры и биты — по драйверу compass_qmc5883.c из Betaflight 2026.12.0-alpha (коммит 744f95f, лицензия GPL-3.0, тот же, что в разделе 99а). Адрес HMC5883L 0x1E — по compass_hmc5883l.c оттуда же; регистры, ID, порядок осей и усиления — по драйверу hmc5883l из Zephyr 4.4.2. Драйвера QMC5883L в Zephyr 4.4.2 нет. Чувствительность QMC5883L 12000 и 3000 LSB/Гс, флаг переполнения HMC5883L −4096 и поведение флагов статуса — по документации производителя, не проверено. Все числа — прогон драйвера _verify/mag99e/mag99e.c на компьютере (gcc 12.2.0) со своей моделью обеих микросхем как ведомых I2C и того, что вокруг них, на Python 3.11; прошивка собрана armclang 6.24 под Cortex-M3. Поле Земли, железо рядом, шум и провода с током — допущения: числа ниже говорят о модели, а не о датчике.

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

Оригинал	Перевод
magnetometer, e-compass	магнитометр, электронный компас
heading, azimuth	курс: угол от севера по часовой стрелке
hard iron	жёсткое железо: постоянный магнит рядом, сдвигает показания
soft iron	мягкое железо: сталь рядом, искажает поле в эллипс
tilt compensation	поправка на наклон по акселерометру
magnetic declination	магнитное склонение: угол между магнитным и истинным севером
inclination (dip)	наклонение: угол поля к горизонту
data ready (DRDY), overflow (OVL), data skip (DOR)	данные готовы, поле вне диапазона, отсчёт пропущен
output data rate (ODR), over sample ratio (OSR)	частота отсчётов, число усреднений внутри
SET/RESET	импульсы перемагничивания чувствительных элементов

99г.1 Модули: QMC5883L вместо HMC5883L и как узнать, кто на плате

Модуль GY-271 — маленькая плата с трёхосным магнитометром, стабилизатором и подтяжками. Под этим именем продают платы с двумя разными микросхемами. HMC5883L от Honeywell — та, под которую написано большинство старых примеров; по распространённым сведениям, она больше не выпускается (документа об этом в проекте нет, не проверено). На дешёвых платах вместо неё часто стоит QMC5883L — похожая по назначению, но с другим адресом и другой картой регистров. Пример для HMC5883L с такой платой не работает: в модели драйвер, знающий только HMC5883L, на плате с QMC5883L получает NACK — код −1.

Betaflight знает и третью — QMC5883P с адресом 0x2C и ID 0x80 в регистре 0x00. Драйвер этого раздела её не ищет.

Подключение такое же, как у любого датчика на I2C1:

I2C1 — на 100 кГц, как в 17.3: CCR = 180, TRISE = 37. Чтение регистра — номер регистра, повторный старт и один, три или шесть байт. Один байт F103 принимает по своему порядку (с. 764): ACK снять ещё до ADDR, потом STOP. Три и больше — способом 2 (с. 763), как в 104б.

Кто стоит на плате. Драйвер сначала спрашивает адрес 0x0D. Если ответили — пишет в CTRL2 бит сброса 0x80, ждёт 20 мс, читает ID и CTRL1: у QMC5883L ID 0xFF, а после сброса два младших бита CTRL1 — ноль, режим ожидания. Betaflight делает так же и в комментарии объясняет почему: одного 0xFF мало, такой байт прочитается и с шины, где никто не отвечает. Не нашлось — адрес 0x1E и три байта ID с 0x0A:

/* кто на шине: QMC5883L — адрес 0x0D, ID 0xFF и режим ожидания после сброса; HMC5883L — адрес 0x1E, ID «H43» */
int mag_probe(mag_t *d)
{
    uint8_t b[3];
    d->type = MAG_NONE;
    if (mag_ping(d, QMC_ADDR) == MAG_OK) {
        d->addr = QMC_ADDR;
        (void)mag_write_reg(d, QMC_CTRL2, QMC_SOFT_RST);        /* сброс: после него — режим ожидания */
        mag_port_delay_us(20000u);
        if (mag_read_regs(d, QMC_ID, b, 1) == MAG_OK && b[0] == QMC_ID_VAL
            && mag_read_regs(d, QMC_CTRL1, b + 1, 1) == MAG_OK && (b[1] & QMC_MODE_MASK) == 0u) {
            d->type = MAG_QMC;                                  /* одного 0xFF мало: так читается и пустая шина */
            return MAG_QMC;
        }
    }
    if (mag_ping(d, HMC_ADDR) == MAG_OK) {
        d->addr = HMC_ADDR;
        if (mag_read_regs(d, HMC_ID_A, b, 3) == MAG_OK && b[0] == 'H' && b[1] == '4' && b[2] == '3') {
            d->type = MAG_HMC;
            return MAG_HMC;
        }
    }
    return MAG_NONE;
}

В модели на плате с QMC5883L опрос адресов 0x08…0x77 находит один 0x0D, а опознание — 4 обмена: адрес, запись 0A 80, чтение 0D → FF, чтение 09 → 00. На плате с HMC5883L — 2 обмена и ID «H43». Если на шине обе, драйвер берёт QMC5883L; если никого — MAG_NONE, и прошивка ждёт.

99г.2 Регистры: режим, диапазон, статус и перевод в нанотеслы

QMC5883L. Всё задаёт CTRL1 (0x09): два старших бита — OSR, сколько раз микросхема усредняет внутри; биты 5:4 — диапазон RNG, 00 — ±2 Гс, 01 — ±8 Гс; биты 3:2 — частота ODR, 00…11 — 10, 50, 100 и 200 Гц; биты 1:0 — режим, 00 — ожидание, 01 — непрерывный. Сдвиги 6, 4, 2 и 0 — как в Betaflight. CTRL2 (0x0A): 0x80 — программный сброс. Регистр 0x0B — период SET/RESET: короткие импульсы, которые заново намагничивают чувствительные элементы. Betaflight пишет туда 0x01 — так же и здесь; производитель, по его документации, советует то же (не проверено). В модели этот регистр и OSR на показания не влияют.

/* непрерывный режим: QMC — 50 Гц, OSR 512, ±2 или ±8 Гс; HMC — 15 Гц, 8 отсчётов в среднем, ±1,3 или ±8,1 Гс */
int mag_init(mag_t *d, int range8)
{
    int rc;
    if (d->type == MAG_QMC) {
        if ((rc = mag_write_reg(d, QMC_SETRESET, 0x01u)) != MAG_OK)
            return rc;
        d->lsb_per_g = range8 ? QMC_LSB_8G : QMC_LSB_2G;
        return mag_write_reg(d, QMC_CTRL1, (uint8_t)(QMC_OSR_512 | (range8 ? QMC_RNG_8G : 0u) | QMC_ODR_50HZ | QMC_MODE_CONT));
    }

В модели QMC5883L получает 0B 01 и 09 05 на ±2 Гс или 09 15 на ±8 Гс. HMC5883L — 00 70 (8 отсчётов в среднем, 15 Гц), 01 20 (±1,3 Гс) или 01 E0 (±8,1 Гс) и 02 00 — непрерывный режим. После сброса QMC5883L стоит в ожидании и данных не даёт: без записи CTRL1 драйвер ждёт готовности 211 мс и возвращает MAG_E_TIMEOUT, −4.

Статус. У QMC5883L регистр 0x06: DRDY (бит 0) — есть новый отсчёт, OVL (бит 1) — поле вне диапазона, DOR (бит 2) — отсчёт пропущен, прошлый так и не прочитали. По документации производителя чтение данных снимает DRDY и DOR (не проверено; модель так и делает). Драйвер опрашивает статус раз в 5 мс и читает шесть байт, когда DRDY = 1. На 100 кГц чтение статуса — 39 тактов SCL, данных — 84, вместе 1230 мкс на отсчёт. В модели 20 чтений подряд на 50 Гц: без пауз DOR не было ни разу; с паузой 30 мс — 10 раз; с паузой 100 мс — 19: отсчёты теряются, если читать реже их частоты.

Перевод в нанотеслы. 1 Гс = 100 мкТл = 100 000 нТл. У QMC5883L по документации производителя 12000 LSB/Гс на ±2 Гс и 3000 на ±8 Гс; у HMC5883L усиление — из таблицы Zephyr: 1370, 1090, 820, 660, 440, 390, 330 и 230 LSB/Гс для ±0,88 … ±8,1 Гс. Делить надо целыми, в 64 битах: 32767 × 100 000 в 32 бита не влезает.

/* 1 Гс = 100 мкТл = 100 000 нТл: нТл = raw × 100 000 / (LSB на гаусс) */
int32_t mag_raw_to_nt(int32_t raw, int32_t lsb_per_g)
{
    return (int32_t)rdiv((int64_t)raw * 100000, lsb_per_g);
}

Поле Земли в модели — 50 мкТл с наклонением 70°: по горизонтали 17,10 мкТл, вниз — 46,98 мкТл. Точка условная. На горизонтали поля в три раза меньше, чем всего, и курс ловит именно её.

В модели прибор без железа рядом повернут на 30°. QMC5883L отдаёт байты E8 06 F5 03 FA E9 — X, Y, Z младшим байтом вперёд: 1768, 1013, −5638, то есть 14 733, 8442, −46 983 нТл. HMC5883L — 00 A1 FE 00 00 5C: 161, потом −512 — это Z, потом 92 — Y; в нанотеслах 14 771, 8440, −46 972. Порядок X, Z, Y одинаков в Zephyr и Betaflight.

99г.3 Курс через atan2 и почему без калибровки он врёт

Оси платы в этом разделе: x — вперёд, y — влево, z — вверх. Когда прибор лежит горизонтально, горизонтальная часть поля смотрит на магнитный север, и курс — угол оси x от него по часовой стрелке: atan2(My, Mx). Смотрит прибор на восток — север слева, по +y, и курс 90°.

У F103 нет FPU, и atan2f на нём — библиотечный float. Курсу хватит целых: метод CORDIC поворачивает вектор на ось x двадцатью шагами, на каждом — сдвиг и сложение, и складывает углы поворотов из таблицы atan(2⁻ⁱ). Короткий вектор сначала растягивается, иначе сдвиги его обнулят. На 3000 случайных векторах длиной от 10 до 10¹² наибольшая ошибка против atan2 с double — 0,54 сотой градуса.

/* atan(2^−i) в десятитысячных градуса: CORDIC поворачивает вектор на ось x и складывает углы поворотов */
static const int32_t atan_tab[20] = {450000, 265651, 140362, 71250, 35763, 17899, 8952, 4476, 2238, 1119,
                                     560, 280, 140, 70, 35, 17, 9, 4, 2, 1};

Почему компас врёт. Магнит рядом с датчиком — динамик, мотор, винт с намагниченной головкой — добавляет постоянный вектор: это жёсткое железо. Круг, по которому идёт конец вектора поля при повороте прибора, сдвигается. Сталь рядом — корпус, плата экрана — меняет поле по-разному вдоль разных направлений: это мягкое железо, круг становится эллипсом. В модели жёсткое железо — 28, −15 и 22 мкТл, мягкое — матрица с 1,15 и 0,88 по x и y и 0,10 между ними. Ошибка курса на горизонтали, наибольшая по кругу из 360 положений и на восьми направлениях:

Без помех остаётся только шум — 0,56°. Мягкое железо даёт ошибку, меняющую знак четыре раза за оборот, до 9,83°. А жёсткое железо в 28 мкТл больше всей горизонтальной части поля, 17,10 мкТл: круг ушёл так далеко, что начало координат вне его, и компас показывает почти что угодно — до 179,85°.

99г.4 Калибровка: смещение, масштаб и матрица

Калибровка — покрутить прибор и посмотреть, каким стал круг. Драйвер копит наименьшее и наибольшее по x, y, z и ещё по диагоналям x + y и x − y. Память — десять чисел, сколько бы точек ни пришло:

/* одна точка при вращении: наименьшее и наибольшее по x, y, z и по диагоналям x + y, x − y */
void magcal_add(magcal_t *c, const int32_t m[3])
{
    const int32_t p[5] = {m[0], m[1], m[2], m[0] + m[1], m[0] - m[1]};
    for (int i = 0; i < 5; i++) {
        if (p[i] < c->lo[i])
            c->lo[i] = p[i];
        if (p[i] > c->hi[i])
            c->hi[i] = p[i];
    }
    c->n++;
}

Дальше три ступени. Смещение — середина размаха по каждой оси: убирает жёсткое железо. Масштаб — полуразмах каждой оси к среднему: эллипс, вытянутый вдоль x или y, становится кругом. Матрица — если эллипс повёрнут, масштабом по осям его не выправить. Размах вдоль направления от смещения не зависит, поэтому из четырёх полуразмахов hx, hy, hu, hv сразу получается форма эллипса P = [[hx², q], [q, hy²]], q = (hu² − hv²) / 4. Нужная матрица — P^(−1/2); для 2×2 у неё есть формула без собственных векторов: W пропорциональна [[hy² + s, −q], [−q, hx² + s]], где s = √det P. Нужны только умножения и целый корень. Это два измерения — плоскость XY; полный эллипсоид в трёх — в 99б.5.

Опыт: прибор на столе делает один оборот за 600 отсчётов (12 с на 50 Гц), потом ошибка курса проверяется на 8 направлениях и по кругу:

Смещение найдено как 27 970 и −14 983 нТл при настоящих 28 000 и −15 000. Одно оно оставляет ошибку мягкого железа — 9,87°. Масштаб по осям (0,885 по x, 1,149 по y) снимает только её часть: эллипс в модели повёрнут. Матрица — 0,881, −0,099 и 1,147 — делает из эллипса круг радиусом 16 914…17 259 нТл, и ошибка падает до 0,79° — почти до шума. W·A отличается от единичной матрицы меньше чем на 1 %.

Мало одного оборота на столе, если нужен наклон: z на столе почти не меняется, его размах — шум, и драйвер помечает z как непроверенный (z_ok = 0), смещение по z не трогает. Чтобы найти и его, прибор 30 с крутят во все стороны: в модели 1500 случайных положений. Тогда смещение — 27 908, −14 954 и 22 008 нТл, z проверен, а ошибка курса на горизонтали по кругу — 0,92°. HMC5883L с теми же помехами после оборота из 200 отсчётов (15 Гц) — 0,69°.

99г.5 Наклон по акселерометру и магнитное склонение

atan2(My, Mx) верен, только пока прибор горизонтален. Стоит его наклонить — в x и y попадает часть вертикального поля, а оно здесь почти втрое больше горизонтального. Поправку делает акселерометр (раздел 99): в покое он показывает «вверх». Векторное произведение поля на «вверх» смотрит на восток, «вверх» на восток — на север; курс — угол оси x между ними. Ни синусов, ни углов наклона — только умножения в 64 битах:

/* с наклоном: U — «вверх» по акселерометру, восток E = M × U, север N = U × E; курс = atan2(Ex·|U|, Nx).
 * Ни синусов, ни углов наклона: только векторные произведения в 64 битах */
int32_t mag_heading_tilt(const int32_t m[3], const int32_t acc_mg[3])
{
    const int64_t mx = m[0], my = m[1], mz = m[2], ux = acc_mg[0], uy = acc_mg[1], uz = acc_mg[2];
    const int64_t ex = my * uz - mz * uy, ey = mz * ux - mx * uz, ez = mx * uy - my * ux;
    const int64_t nx = uy * ez - uz * ey;
    const int64_t u = (int64_t)isqrt64((uint64_t)(ux * ux + uy * uy + uz * uz));
    return mag_atan2_cdeg(ex * u, nx);
}

В модели после калибровки во все стороны, наибольшая ошибка по кругу:

Наклон на 5° — уже 13,81° ошибки, на 10° — около 30°. С поправкой — около градуса при любом наклоне. На горизонтали она чуть хуже простой формулы, 1,21° против 0,71°: шум акселерометра в модели 2 мg, и он тоже попадает в курс. Поправка верна только в покое: при ускорении 0,1 g вперёд ошибка — 17,14°, вбок — 16,43°. Прибору, который ездит и трясётся, нужен гироскоп и фильтр — 99б.

Если z не калибровали (оборот на столе, z_ok = 0), поправка на наклон работает со смещением z, равным нулю: при тангаже 15° и крене −10° — 21,76° ошибки. Прошивка в этом случае считает курс только по плоской формуле.

Склонение. Компас показывает на магнитный север, карта — на истинный. Угол между ними — магнитное склонение D, своё для каждого места и медленно меняется с годами; его берут из модели магнитного поля Земли для своей точки. Истинный курс = магнитный + D, восточное склонение — плюс:

/* истинный курс = магнитный + склонение (восточное — плюс), по модулю 360° */
int32_t mag_true_heading(int32_t mag_cdeg, int32_t decl_cdeg)
{
    int32_t t = (mag_cdeg + decl_cdeg) % 36000;
    return t < 0 ? t + 36000 : t;
}

В модели D = +10,00° — условное значение. Без него истинный курс на 8 направлениях ошибается на −9,46…−10,74°, с ним — не больше 0,70° по модулю.

99г.6 Помехи: провода с током, моторы и динамики

Калибровка убирает только то, что неподвижно относительно датчика и не меняется во времени. Магнит динамика, привинченный к прибору, — жёсткое железо: откалибруется. Ток в проводе рядом — нет, если он включается и выключается. Поле прямого длинного провода — μ0·I / (2π·r), 2 мкТл на ампер на 10 см:

Сравнивать надо с 17,10 мкТл горизонтального поля. В модели провод под датчиком в 5 см, ток включили после калибровки; наибольшая ошибка курса по кругу: 0,1 А — 2,22°, 0,5 А — 8,39°, 1 А — 16,18°, 2 А — 33,10°, 5 А — 176,83°. Если тот же ток 2 А идёт туда и обратно по паре проводов в 2 мм друг от друга, поле в 5 см — 0,31 мкТл, и ошибка — 1,86°. Если калибровали при включённом токе 2 А, с ним курс точен — 0,71°, а без него — 32,62°.

Отсюда правила. Силовые провода к датчику не подводить; прямой и обратный провод — рядом или свитые. Мотор — это и магнит, и ток в обмотках, который меняется вместе с нагрузкой: датчик выносят от него подальше, на мачту, как на коптерах. Калибровать в той сборке, в которой прибор работает, и при тех токах, что текут всегда.

99г.7 Грабли

Модуль не отвечает по адресу из примера. На плате QMC5883L, а пример для HMC5883L: адрес 0x0D, а не 0x1E. В модели — NACK, код −1. Опрос шины (99г.1) сразу показывает, кто там.

Отвечает по 0x0D, но курс — мусор. Драйвер HMC5883L, переставленный на адрес 0x0D, читает чужие регистры: статусом считает CTRL1, данными — половину Y, Z, статус и температуру. В модели ошибка курса — до 180,00°. Карты регистров у этих микросхем разные целиком.

Курс скачет на 90° и больше у HMC5883L. Второе слово данных принято за Y, а это Z: в памяти X, Z, Y. В модели такой разбор даёт до 179,89°.

QMC5883L не даёт данных. После сброса она в режиме ожидания: без записи CTRL1 DRDY не появится. В модели — тайм-аут через 211 мс.

Рядом магнит — курс врёт и после калибровки. На ±2 Гс код у QMC5883L ограничен 16 битами, 273 058 нТл. В модели смещение от магнита — 265 мкТл: часть отсчётов упирается в предел, ставится OVL. На калибровке 220 отсчётов из 600 отброшены, смещение по x найдено как 259 062 нТл вместо 265 000, а потом 66 положений из 180 дают переполнение, и ошибка остальных — до 27,38°. На ±8 Гс то же место — без единого OVL, смещение 264 966 нТл, ошибка 0,75°. Лучше всего — убрать магнит.

Ось z смотрит не туда. Знак z у магнитометра перевёрнут относительно акселерометра — например, его «перевели» в оси с z вниз, а остальные оси оставили. На горизонтали этого не видно: 0,71° без поправки и 1,30° с ней. При наклоне 15° и −10° — 178,42°. Тройки осей обоих датчиков должны быть правыми и совпадать.

Плата стоит повёрнутой. Модуль повернули на 90° — x смотрит вбок. На горизонтали курс сдвинут на 90,57°, при наклоне ошибка доходит до 179,86°: оси магнитометра и акселерометра разошлись. Перед формулами оси переставляют в оси прибора; оставшийся постоянный сдвиг проверяют, направив прибор на известный ориентир.

Питание и уровни. У модуля со стабилизатором подтяжки обычно к его выходу; у голой микросхемы питание по документации производителя ниже 5 В (не проверено). PB6 и PB7 — FT (с. 32): 5-вольтовые подтяжки они выдержат, но лучше 3,3 В.

99г.8 Итоги

• На плате GY-271 может стоять QMC5883L (0x0D, ID 0xFF) или HMC5883L (0x1E, ID «H43»); опознание — по адресу, ID и режиму после сброса, 4 и 2 обмена в модели.
• QMC5883L: CTRL1 0x05 — ±2 Гс, 0x15 — ±8 Гс, 50 Гц, непрерывный режим; статус 0x06 — DRDY, OVL, DOR; данные X, Y, Z младшим байтом вперёд. HMC5883L — X, Z, Y старшим байтом вперёд.
• Нанотеслы целыми: raw × 100 000 / LSB на гаусс в 64 битах; 1 LSB — 8 нТл у QMC5883L на ±2 Гс, 92 нТл у HMC5883L на ±1,3 Гс.
• Курс — CORDIC без float: 0,54 сотой градуса ошибки.
• Калибровка по наибольшему и наименьшему: без неё 179,48°, со смещением 9,87°, с масштабом 6,84°, с матрицей 0,79°.
• Наклон 10° без поправки — 29,08°, с поправкой по акселерометру — 1,15°; для неё z надо откалибровать, крутя прибор во все стороны.
• Истинный курс = магнитный + склонение места.
• Провод с током 1 А в 5 см — 16,18° ошибки; свитая пара с 2 А — 1,86°.

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

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

/* mag99e.h — раздел 99г: магнитометр-компас QMC5883L / HMC5883L, калибровка и курс — целыми, без регистров
 * микроконтроллера. Драйвер говорит с микросхемой только через порт (mag_port_*): в прошивке fw99e.c его делает
 * I2C1 STM32F103, в стенде host99e.c — модель model99e.py.
 * QMC5883L: адрес 0x0D, карта регистров и биты — как в compass_qmc5883.c Betaflight; 12000 и 3000 LSB/Гс —
 * по документации производителя (не проверено). HMC5883L: адрес 0x1E — как в compass_hmc5883l.c Betaflight;
 * регистры, ID «H43», порядок X, Z, Y и усиления — как в hmc5883l.h/.c Zephyr 4.4.2.
 * Оси платы: x — вперёд, y — влево, z — вверх (правая тройка); курс — от севера по часовой стрелке. */
#ifndef MAG99E_H
#define MAG99E_H
#include <stdint.h>

/* ── QMC5883L ─────────────────────────────────────────────────────────── */
#define QMC_ADDR        0x0Du
#define QMC_DATA        0x00u           /* X, Y, Z — по два байта, младший первым */
#define QMC_STATUS      0x06u
#define QMC_ST_DRDY     0x01u           /* новые данные */
#define QMC_ST_OVL      0x02u           /* поле вне диапазона */
#define QMC_ST_DOR      0x04u           /* отсчёт пропущен: данные не прочитаны вовремя */
#define QMC_CTRL1       0x09u           /* OSR [7:6], RNG [5:4], ODR [3:2], MODE [1:0] */
#define QMC_CTRL2       0x0Au
#define QMC_SETRESET    0x0Bu           /* период SET/RESET */
#define QMC_ID          0x0Du
#define QMC_ID_VAL      0xFFu
#define QMC_MODE_CONT   0x01u
#define QMC_MODE_MASK   0x03u
#define QMC_ODR_50HZ    (1u << 2)
#define QMC_RNG_8G      (1u << 4)       /* 0 — ±2 Гс */
#define QMC_OSR_512     (0u << 6)
#define QMC_SOFT_RST    0x80u
#define QMC_LSB_2G      12000           /* LSB на гаусс при ±2 Гс */
#define QMC_LSB_8G      3000            /* при ±8 Гс */

/* ── HMC5883L ─────────────────────────────────────────────────────────── */
#define HMC_ADDR        0x1Eu
#define HMC_CRA         0x00u           /* усреднение [6:5], частота [4:2], режим измерения [1:0] */
#define HMC_CRB         0x01u           /* усиление [7:5] */
#define HMC_MODE        0x02u
#define HMC_DATA        0x03u           /* X, Z, Y — по два байта, старший первым */
#define HMC_STATUS      0x09u
#define HMC_ST_RDY      0x01u
#define HMC_ID_A        0x0Au           /* три байта: 'H', '4', '3' */
#define HMC_CRA_VAL     0x70u           /* 8 отсчётов в среднем (3 << 5), 15 Гц (4 << 2), без смещения */
#define HMC_OVF         (-4096)         /* так микросхема помечает переполнение оси */

#define MAG_POLL_US     5000u           /* опрос готовности */
#define MAG_POLL_TRIES  40u             /* 200 мс: у HMC5883L отсчёт раз в 67 мс */

enum { MAG_NONE = 0, MAG_QMC = 1, MAG_HMC = 2 };
enum { MAG_OK = 0, MAG_E_NACK = -1, MAG_E_BUS = -2, MAG_E_ID = -3, MAG_E_TIMEOUT = -4, MAG_E_OVL = -5, MAG_E_CAL = -6 };
enum { CAL_NONE = 0, CAL_OFFSET = 1, CAL_SCALE = 2, CAL_MATRIX = 3 };

typedef struct {
    uint8_t type, addr;
    int32_t lsb_per_g;
    uint32_t xfers, errors, samples, dor, ovl;
} mag_t;

typedef struct {
    int32_t lo[5], hi[5];               /* x, y, z, x + y, x − y — нТл */
    uint32_t n;
} magcal_t;

typedef struct {
    uint8_t mode, z_ok;
    int32_t v[3];                       /* смещение — жёсткое железо, нТл */
    int32_t s[3];                       /* масштаб по осям, Q14 */
    int32_t w[2][2];                    /* матрица XY — мягкое железо, Q14 */
} magcorr_t;

/* ── порт: в прошивке — I2C1, в стенде — строки к модели ──────────────── */
/* ntx байт записи, затем (если nrx) повторный старт и nrx байт чтения; ntx = nrx = 0 — только адрес.
 * 0, MAG_E_NACK или MAG_E_BUS */
int mag_port_xfer(uint8_t addr7, const uint8_t *tx, uint32_t ntx, uint8_t *rx, uint32_t nrx);
void mag_port_delay_us(uint32_t us);

/* ── драйвер ──────────────────────────────────────────────────────────── */
int mag_ping(mag_t *d, uint8_t addr7);
int mag_write_reg(mag_t *d, uint8_t reg, uint8_t val);
int mag_read_regs(mag_t *d, uint8_t reg, uint8_t *buf, uint32_t n);
int mag_probe(mag_t *d);
int mag_init(mag_t *d, int range8);
int mag_read_raw(mag_t *d, int16_t raw[3]);
int32_t mag_raw_to_nt(int32_t raw, int32_t lsb_per_g);
int mag_read_nt(mag_t *d, int32_t nt[3]);
int mag_read_avg(mag_t *d, uint32_t n, int32_t nt[3]);

/* ── калибровка ───────────────────────────────────────────────────────── */
void magcal_reset(magcal_t *c);
void magcal_add(magcal_t *c, const int32_t m[3]);
int magcal_solve(const magcal_t *c, int mode, magcorr_t *k);
void magcorr_apply(const magcorr_t *k, const int32_t m[3], int32_t out[3]);

/* ── курс, сотые доли градуса 0…35999 ─────────────────────────────────── */
int32_t mag_atan2_cdeg(int64_t y, int64_t x);
int32_t mag_heading_flat(const int32_t m[3]);
int32_t mag_heading_tilt(const int32_t m[3], const int32_t acc_mg[3]);
int32_t mag_true_heading(int32_t mag_cdeg, int32_t decl_cdeg);

#endif

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

/* mag99e.c — раздел 99г: драйвер QMC5883L / HMC5883L, калибровка и курс. Один и тот же файл собирается в прошивку
 * (armclang, Cortex-M3) и в стенд на компьютере (gcc): микроконтроллера он не знает, только порт mag_port_*.
 * Всё целыми: поле — в нанотеслах, курс — в сотых долях градуса, у F103 нет FPU. */
#include "mag99e.h"

#define Q14         16384
#define CAL_MIN_NT  5000                /* размах меньше 5 мкТл — эту ось не поворачивали */
#define ASR(v, i)   ((v) >= 0 ? (v) >> (i) : ~(~(v) >> (i)))    /* сдвиг вправо со знаком, как деление с округлением вниз */

static const uint16_t hmc_gain[8] = {1370, 1090, 820, 660, 440, 390, 330, 230};    /* LSB/Гс: ±0,88 … ±8,1 Гс */

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

int mag_ping(mag_t *d, uint8_t addr7)
{
    return xfer_done(d, mag_port_xfer(addr7, 0, 0, 0, 0));
}

int mag_write_reg(mag_t *d, uint8_t reg, uint8_t val)
{
    uint8_t b[2] = {reg, val};
    return xfer_done(d, mag_port_xfer(d->addr, b, 2, 0, 0));
}

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

/* кто на шине: QMC5883L — адрес 0x0D, ID 0xFF и режим ожидания после сброса; HMC5883L — адрес 0x1E, ID «H43» */
int mag_probe(mag_t *d)
{
    uint8_t b[3];
    d->type = MAG_NONE;
    if (mag_ping(d, QMC_ADDR) == MAG_OK) {
        d->addr = QMC_ADDR;
        (void)mag_write_reg(d, QMC_CTRL2, QMC_SOFT_RST);        /* сброс: после него — режим ожидания */
        mag_port_delay_us(20000u);
        if (mag_read_regs(d, QMC_ID, b, 1) == MAG_OK && b[0] == QMC_ID_VAL
            && mag_read_regs(d, QMC_CTRL1, b + 1, 1) == MAG_OK && (b[1] & QMC_MODE_MASK) == 0u) {
            d->type = MAG_QMC;                                  /* одного 0xFF мало: так читается и пустая шина */
            return MAG_QMC;
        }
    }
    if (mag_ping(d, HMC_ADDR) == MAG_OK) {
        d->addr = HMC_ADDR;
        if (mag_read_regs(d, HMC_ID_A, b, 3) == MAG_OK && b[0] == 'H' && b[1] == '4' && b[2] == '3') {
            d->type = MAG_HMC;
            return MAG_HMC;
        }
    }
    return MAG_NONE;
}

/* непрерывный режим: QMC — 50 Гц, OSR 512, ±2 или ±8 Гс; HMC — 15 Гц, 8 отсчётов в среднем, ±1,3 или ±8,1 Гс */
int mag_init(mag_t *d, int range8)
{
    int rc;
    if (d->type == MAG_QMC) {
        if ((rc = mag_write_reg(d, QMC_SETRESET, 0x01u)) != MAG_OK)
            return rc;
        d->lsb_per_g = range8 ? QMC_LSB_8G : QMC_LSB_2G;
        return mag_write_reg(d, QMC_CTRL1, (uint8_t)(QMC_OSR_512 | (range8 ? QMC_RNG_8G : 0u) | QMC_ODR_50HZ | QMC_MODE_CONT));
    }
    if (d->type == MAG_HMC) {
        uint8_t g = range8 ? 7u : 1u;
        if ((rc = mag_write_reg(d, HMC_CRA, HMC_CRA_VAL)) != MAG_OK)
            return rc;
        if ((rc = mag_write_reg(d, HMC_CRB, (uint8_t)(g << 5))) != MAG_OK)
            return rc;
        d->lsb_per_g = hmc_gain[g];
        return mag_write_reg(d, HMC_MODE, 0x00u);               /* 0 — непрерывный; после включения — одиночный */
    }
    return MAG_E_ID;
}

/* дождаться готовности и прочитать X, Y, Z; у HMC5883L в памяти порядок X, Z, Y и старший байт первым */
int mag_read_raw(mag_t *d, int16_t raw[3])
{
    uint8_t st, b[6];
    int rc;
    const uint8_t hmc = d->type == MAG_HMC;
    if (d->type != MAG_QMC && !hmc)
        return MAG_E_ID;
    for (uint32_t i = 0;; i++) {
        if ((rc = mag_read_regs(d, hmc ? HMC_STATUS : QMC_STATUS, &st, 1)) != MAG_OK)
            return rc;
        if (st & (hmc ? HMC_ST_RDY : QMC_ST_DRDY))
            break;
        if (i + 1u >= MAG_POLL_TRIES)
            return MAG_E_TIMEOUT;                               /* QMC в режиме ожидания: CTRL1 не записан */
        mag_port_delay_us(MAG_POLL_US);
    }
    if ((rc = mag_read_regs(d, hmc ? HMC_DATA : QMC_DATA, b, 6)) != MAG_OK)
        return rc;
    d->samples++;
    if (!hmc) {
        for (int k = 0; k < 3; k++)
            raw[k] = (int16_t)(b[2 * k] | b[2 * k + 1] << 8);   /* младший байт первым */
        if (st & QMC_ST_DOR)
            d->dor++;
        if (st & QMC_ST_OVL) {
            d->ovl++;
            return MAG_E_OVL;
        }
        return MAG_OK;
    }
    raw[0] = (int16_t)(b[0] << 8 | b[1]);
    raw[2] = (int16_t)(b[2] << 8 | b[3]);                       /* вторым идёт Z, а не Y */
    raw[1] = (int16_t)(b[4] << 8 | b[5]);
    if (raw[0] == HMC_OVF || raw[1] == HMC_OVF || raw[2] == HMC_OVF) {
        d->ovl++;
        return MAG_E_OVL;
    }
    return MAG_OK;
}

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

/* 1 Гс = 100 мкТл = 100 000 нТл: нТл = raw × 100 000 / (LSB на гаусс) */
int32_t mag_raw_to_nt(int32_t raw, int32_t lsb_per_g)
{
    return (int32_t)rdiv((int64_t)raw * 100000, lsb_per_g);
}

int mag_read_nt(mag_t *d, int32_t nt[3])
{
    int16_t raw[3] = {0, 0, 0};
    int rc = mag_read_raw(d, raw);
    for (int k = 0; k < 3; k++)
        nt[k] = mag_raw_to_nt(raw[k], d->lsb_per_g);
    return rc;
}

int mag_read_avg(mag_t *d, uint32_t n, int32_t nt[3])
{
    int64_t s[3] = {0, 0, 0};
    int32_t m[3];
    int rc;
    if (!n)
        return MAG_E_CAL;
    for (uint32_t i = 0; i < n; i++) {
        if ((rc = mag_read_nt(d, m)) != MAG_OK)
            return rc;
        for (int k = 0; k < 3; k++)
            s[k] += m[k];
    }
    for (int k = 0; k < 3; k++)
        nt[k] = (int32_t)rdiv(s[k], (int64_t)n);
    return MAG_OK;
}

void magcal_reset(magcal_t *c)
{
    for (int i = 0; i < 5; i++) {
        c->lo[i] = INT32_MAX;
        c->hi[i] = INT32_MIN;
    }
    c->n = 0;
}

/* одна точка при вращении: наименьшее и наибольшее по x, y, z и по диагоналям x + y, x − y */
void magcal_add(magcal_t *c, const int32_t m[3])
{
    const int32_t p[5] = {m[0], m[1], m[2], m[0] + m[1], m[0] - m[1]};
    for (int i = 0; i < 5; i++) {
        if (p[i] < c->lo[i])
            c->lo[i] = p[i];
        if (p[i] > c->hi[i])
            c->hi[i] = p[i];
    }
    c->n++;
}

static uint64_t isqrt64(uint64_t v)
{
    uint64_t r = 0, b = 1ull << 62;
    while (b > v)
        b >>= 2;
    while (b) {
        if (v >= r + b) {
            v -= r + b;
            r = (r >> 1) + b;
        } else {
            r >>= 1;
        }
        b >>= 2;
    }
    return r;
}

/* Смещение — середина размаха. Масштаб — полуразмах каждой оси к среднему. Матрица XY: эллипс с полуширинами
 * hx, hy и hu, hv по диагоналям (размах не зависит от смещения) — P = [[p, q], [q, r]], p = hx², r = hy²,
 * q = (hu² − hv²) / 4; W = P^(−1/2) · √(√det P) = [[r + s, −q], [−q, p + s]] / (√s · √(p + r + 2s)), s = √det P.
 * Эллипс становится окружностью радиуса √(ab) — среднего геометрического полуосей. Квадраты — в единицах 8 нТл. */
int magcal_solve(const magcal_t *c, int mode, magcorr_t *k)
{
    int32_t h[5], mid[3];
    int64_t p, r, q, det, s, t, kk;
    k->mode = CAL_NONE;
    k->z_ok = 0;
    for (int i = 0; i < 3; i++) {
        k->v[i] = 0;
        k->s[i] = Q14;
    }
    k->w[0][0] = k->w[1][1] = Q14;
    k->w[0][1] = k->w[1][0] = 0;
    if (mode == CAL_NONE)
        return MAG_OK;
    if (c->n < 2u || mode < CAL_NONE || mode > CAL_MATRIX)
        return MAG_E_CAL;
    for (int i = 0; i < 5; i++)
        h[i] = (int32_t)(((int64_t)c->hi[i] - c->lo[i]) / 2);
    for (int i = 0; i < 3; i++)
        mid[i] = (int32_t)(((int64_t)c->hi[i] + c->lo[i]) / 2);
    if (h[0] < CAL_MIN_NT || h[1] < CAL_MIN_NT)
        return MAG_E_CAL;                                       /* прибор не повернули кругом */
    k->z_ok = (uint8_t)(4 * (int64_t)h[2] >= (int64_t)h[0] + h[1]);    /* z повернули хотя бы на полразмаха xy */
    k->v[0] = mid[0];
    k->v[1] = mid[1];
    k->v[2] = k->z_ok ? mid[2] : 0;
    k->mode = (uint8_t)mode;
    if (mode == CAL_SCALE) {
        int64_t avg = k->z_ok ? ((int64_t)h[0] + h[1] + h[2]) / 3 : ((int64_t)h[0] + h[1]) / 2;
        k->w[0][0] = k->s[0] = (int32_t)rdiv(avg * Q14, h[0]);
        k->w[1][1] = k->s[1] = (int32_t)rdiv(avg * Q14, h[1]);
        if (k->z_ok)
            k->s[2] = (int32_t)rdiv(avg * Q14, h[2]);
    } else if (mode == CAL_MATRIX) {
        p = (int64_t)(h[0] / 8) * (h[0] / 8);
        r = (int64_t)(h[1] / 8) * (h[1] / 8);
        q = ((int64_t)(h[3] / 8) * (h[3] / 8) - (int64_t)(h[4] / 8) * (h[4] / 8)) / 4;
        det = p * r - q * q;
        if (det <= 0)
            return MAG_E_CAL;
        s = (int64_t)isqrt64((uint64_t)det);
        t = (int64_t)isqrt64((uint64_t)(p + r + 2 * s));
        kk = (int64_t)isqrt64((uint64_t)s) * t;
        k->w[0][0] = (int32_t)rdiv((r + s) * Q14, kk);
        k->w[0][1] = k->w[1][0] = (int32_t)rdiv(-q * Q14, kk);
        k->w[1][1] = (int32_t)rdiv((p + s) * Q14, kk);
        if (k->z_ok)
            k->s[2] = (int32_t)rdiv((int64_t)isqrt64((uint64_t)s) * 8 * Q14, h[2]);
    }
    return MAG_OK;
}

void magcorr_apply(const magcorr_t *k, const int32_t m[3], int32_t out[3])
{
    const int64_t d0 = (int64_t)m[0] - k->v[0], d1 = (int64_t)m[1] - k->v[1], d2 = (int64_t)m[2] - k->v[2];
    out[0] = (int32_t)rdiv(k->w[0][0] * d0 + k->w[0][1] * d1, Q14);
    out[1] = (int32_t)rdiv(k->w[1][0] * d0 + k->w[1][1] * d1, Q14);
    out[2] = (int32_t)rdiv(k->s[2] * d2, Q14);
}

/* atan(2^−i) в десятитысячных градуса: CORDIC поворачивает вектор на ось x и складывает углы поворотов */
static const int32_t atan_tab[20] = {450000, 265651, 140362, 71250, 35763, 17899, 8952, 4476, 2238, 1119,
                                     560, 280, 140, 70, 35, 17, 9, 4, 2, 1};

/* угол вектора (x, y) против часовой стрелки от оси x, сотые доли градуса 0…35999; без float и libm */
int32_t mag_atan2_cdeg(int64_t y, int64_t x)
{
    int32_t a = 0, xi, yi, t, cd;
    if (x == 0 && y == 0)
        return 0;
    while (x > (1ll << 29) || x < -(1ll << 29) || y > (1ll << 29) || y < -(1ll << 29)) {
        x /= 2;                                                 /* запас на рост длины в 1,647 раза */
        y /= 2;
    }
    while (x < (1ll << 28) && x > -(1ll << 28) && y < (1ll << 28) && y > -(1ll << 28)) {
        x *= 2;                                                 /* короткий вектор: иначе сдвиги на i обнулят его */
        y *= 2;
    }
    xi = (int32_t)x;
    yi = (int32_t)y;
    if (xi < 0) {                                               /* левая полуплоскость: повернуть на 180° */
        xi = -xi;
        yi = -yi;
        a = 1800000;
    }
    for (int i = 0; i < 20; i++) {
        t = xi;
        if (yi > 0) {
            xi += ASR(yi, i);
            yi -= ASR(t, i);
            a += atan_tab[i];
        } else {
            xi -= ASR(yi, i);
            yi += ASR(t, i);
            a -= atan_tab[i];
        }
    }
    if (a < 0)
        a += 3600000;
    cd = (a + 50) / 100;
    return cd >= 36000 ? cd - 36000 : cd;
}

/* прибор горизонтально: курс оси x от магнитного севера по часовой стрелке; y платы смотрит влево */
int32_t mag_heading_flat(const int32_t m[3])
{
    return mag_atan2_cdeg(m[1], m[0]);
}

/* с наклоном: U — «вверх» по акселерометру, восток E = M × U, север N = U × E; курс = atan2(Ex·|U|, Nx).
 * Ни синусов, ни углов наклона: только векторные произведения в 64 битах */
int32_t mag_heading_tilt(const int32_t m[3], const int32_t acc_mg[3])
{
    const int64_t mx = m[0], my = m[1], mz = m[2], ux = acc_mg[0], uy = acc_mg[1], uz = acc_mg[2];
    const int64_t ex = my * uz - mz * uy, ey = mz * ux - mx * uz, ez = mx * uy - my * ux;
    const int64_t nx = uy * ez - uz * ey;
    const int64_t u = (int64_t)isqrt64((uint64_t)(ux * ux + uy * uy + uz * uz));
    return mag_atan2_cdeg(ex * u, nx);
}

/* истинный курс = магнитный + склонение (восточное — плюс), по модулю 360° */
int32_t mag_true_heading(int32_t mag_cdeg, int32_t decl_cdeg)
{
    int32_t t = (mag_cdeg + decl_cdeg) % 36000;
    return t < 0 ? t + 36000 : t;
}

fw99e.c — прошивка F103: I2C1 на 100 кГц, приём одного и N байт, калибровка по команде, курс в цикле.

/* fw99e.c — раздел 99г: компас на QMC5883L или HMC5883L, STM32F103 (BluePill), 72 МГц, APB1 = 36 МГц.
 * I2C1: PB6 — SCL, PB7 — SDA (открытый сток, 100 кГц). При запуске — какая микросхема на шине, затем непрерывный
 * режим. Цикл: среднее из четырёх отсчётов, поправка калибровки, курс, истинный курс со склонением.
 * Через отладчик: range8 — диапазон ±8 Гс (до запуска), cal_run = 1 — калибровка: 1500 точек, прибор крутить во все
 * стороны; decl_cdeg — склонение места в сотых градуса (восточное — плюс); acc_mg — «вверх» по акселерометру в
 * тысячных g, если он есть (раздел 99), иначе 0, 0, 1000 — прибор горизонтально. Тактирование до 72 МГц — как в
 * разделе 6. */
#include "stm32f10x.h"
#include "mag99e.h"

#define CPU_HZ          72000000u
#define APB1_MHZ        36u
#define I2C_TIMEOUT_US  2000u           /* байт на 100 кГц — 90 мкс */
#define CAL_N           1500u           /* 30 с при 50 Гц */
#define AVG_N           4u

static mag_t dev;
static magcal_t cal;
static magcorr_t corr;
volatile uint32_t range8, cal_run, chip, bus_errors, ovl_errors;
volatile int32_t acc_mg[3] = {0, 0, 1000}, decl_cdeg, field_nt[3], heading_cdeg, true_cdeg, cal_rc = 1;

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

void mag_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;
    /* PB6, PB7 — альтернативная функция, открытый сток, 2 МГц (1110) */
    GPIOB->CRL = (GPIOB->CRL & 0x00FFFFFFu) | 0xEE000000u;
    I2C1->CR1 = 0;                                      /* настраивать при PE = 0 */
    I2C1->CR2 = APB1_MHZ;
    I2C1->CCR = APB1_MHZ * 1000000u / (2u * 100000u);   /* стандартный режим: 36 МГц / (2 × 180) = 100 кГц */
    I2C1->TRISE = APB1_MHZ + 1u;                        /* 1000 нс × 36 МГц = 36 → 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 MAG_E_NACK;                          /* адрес или байт не подтверждён */
        if (expired(t0, I2C_TIMEOUT_US))
            return MAG_E_BUS;
    }
    return MAG_OK;
}

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

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

int mag_port_xfer(uint8_t addr7, const uint8_t *tx, uint32_t ntx, uint8_t *rx, uint32_t nrx)
{
    int rc;
    uint32_t t0 = DWT->CYCCNT;
    if (nrx == 2u || (nrx != 0u && ntx == 0u))
        return MAG_E_BUS;                               /* двум байтам нужен бит POS — драйверу они не нужны */
    while (I2C1->SR2 & I2C_SR2_BUSY) {                  /* SDA держит ведомый — раздел 17.6 */
        if (expired(t0, I2C_TIMEOUT_US))
            return MAG_E_BUS;
    }
    I2C1->CR1 |= I2C_CR1_ACK;
    I2C1->CR1 |= I2C_CR1_START;
    if ((rc = sr1_wait(I2C_SR1_SB)) != MAG_OK)
        return fail(rc);
    I2C1->DR = (uint8_t)(addr7 << 1);
    if ((rc = sr1_wait(I2C_SR1_ADDR)) != MAG_OK)
        return fail(rc);                                /* нет ответа на адрес: опрос шины идёт дальше */
    (void)I2C1->SR2;
    for (uint32_t i = 0; i < ntx; i++) {
        if ((rc = sr1_wait(I2C_SR1_TXE)) != MAG_OK)
            return fail(rc);
        I2C1->DR = tx[i];
    }
    if (ntx && (rc = sr1_wait(I2C_SR1_BTF)) != MAG_OK)
        return fail(rc);
    if (!nrx) {
        I2C1->CR1 |= I2C_CR1_STOP;
        return MAG_OK;
    }
    I2C1->CR1 |= I2C_CR1_START;                         /* повторный старт: номер регистра уже передан */
    if ((rc = sr1_wait(I2C_SR1_SB)) != MAG_OK)
        return fail(rc);
    I2C1->DR = (uint8_t)(addr7 << 1 | 1u);
    if ((rc = sr1_wait(I2C_SR1_ADDR)) != MAG_OK)
        return fail(rc);
    if ((rc = rx_n(rx, nrx)) != MAG_OK)
        return fail(rc);
    return MAG_OK;
}

int main(void)
{
    int32_t m[3], c[3], a[3];
    int rc;
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    DWT->CYCCNT = 0;
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
    gpio_i2c1_init();
    (void)magcal_solve(&cal, CAL_NONE, &corr);          /* до калибровки — без поправки */
    while ((chip = (uint32_t)mag_probe(&dev)) == MAG_NONE)
        mag_port_delay_us(100000u);                     /* нет ответа ни на 0x0D, ни на 0x1E: провод, питание */
    while (mag_init(&dev, (int)range8) != MAG_OK)
        mag_port_delay_us(100000u);
    for (;;) {
        if (cal_run) {                                  /* 30 с крутить прибор во все стороны */
            magcal_reset(&cal);
            for (uint32_t i = 0; i < CAL_N; i++) {
                if (mag_read_nt(&dev, m) == MAG_OK)
                    magcal_add(&cal, m);                /* отсчёт с переполнением не берём */
                else
                    bus_errors++;
            }
            cal_rc = magcal_solve(&cal, CAL_MATRIX, &corr);
            if (cal_rc != MAG_OK)
                (void)magcal_solve(&cal, CAL_NONE, &corr);
            cal_run = 0;
        }
        if ((rc = mag_read_avg(&dev, AVG_N, m)) != MAG_OK) {
            if (rc == MAG_E_OVL)
                ovl_errors++;                           /* рядом магнит: ±8 Гс или убрать магнит */
            else
                bus_errors++;
            continue;
        }
        magcorr_apply(&corr, m, c);
        for (int k = 0; k < 3; k++) {
            a[k] = acc_mg[k];
            field_nt[k] = c[k];
        }
        heading_cdeg = corr.z_ok ? mag_heading_tilt(c, a) : mag_heading_flat(c);   /* без z — только горизонтально */
        true_cdeg = mag_true_heading(heading_cdeg, decl_cdeg);
    }
}

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