99. Движение: MPU-6050 и ADXL345

Акселерометр меряет ускорение вместе с тяжестью, гироскоп — угловую скорость. Из первого в покое получается наклон, из второго интегрированием — угол, который со временем уходит. MPU-6050 несёт оба датчика, ADXL345 — только акселерометр. На двух этих микросхемах видно всё, на чём спотыкаются: датчик, который после включения спит, два разных порядка байт, смещение нуля гироскопа и фильтр, который собирает из двух плохих измерений одно хорошее.

Подключение по I²C — разделы 17 и 95.

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

Оригинал	Перевод
accelerometer	акселерометр
gyroscope, angular rate	гироскоп, угловая скорость
IMU (inertial measurement unit)	инерциальный модуль
full-scale range	диапазон шкалы
sensitivity, LSB/g	чувствительность: единиц кода на g
pitch, roll, yaw	тангаж, крен, рыскание
zero-rate offset, bias	смещение нуля гироскопа
drift	уход угла
complementary filter	комплементарный фильтр
big-endian, little-endian	старший байт первым, младший байт первым

99.1 Что они меряют

• Акселерометр меряет сумму тяжести и ускорения. В покое остаётся одна тяжесть, и по её направлению видно, как наклонена плата: тангаж и крен. Поворот вокруг вертикали — рыскание — вектор тяжести не меняет, и акселерометр его не видит.
• Гироскоп меряет угловую скорость. Угол — её интеграл, и любое смещение нуля копится: 0,5 °/с за минуту — это 30 градусов.

99.2 Шкалы

Диапазон задают два бита в регистрах 0x1C (ускорение) и 0x1B (угловая скорость), и от него зависит, сколько единиц кода приходится на единицу величины:

У ADXL345 в режиме полного разрешения шаг — 3,9 тысячной g на единицу кода, около 256 единиц на g при любом диапазоне.

#define MPU_LSB_PER_G(afs) (16384u >> (afs))
static const uint16_t mpu_lsb_per_dps_x10[4] = { 1310, 655, 328, 164 };

Узкий диапазон точнее, широкий не зашкаливает: для наклона хватает ±2 g, для удара нужно ±16 g.

99.3 MPU-6050: разбудить и прочитать

После включения MPU-6050 спит и в регистрах данных держит нули. Запуск — проверить код модели, записать 0 в PWR_MGMT_1 (документация советует сразу 0x01 — такт от генератора гироскопа, он стабильнее внутреннего 8 МГц) и выбрать диапазоны. Код 0x68 отдаёт только MPU-6050: модули под тем же именем с MPU-6500 и MPU-9250 отвечают 0x70 и 0x71, и mpu_init их отвергнет; чтение — 14 байт с 0x3B одной транзакцией: три ускорения, температура, три скорости, у каждого значения старший байт первым:

int mpu_init(unsigned afs, unsigned fs)
{
    uint8_t id = 0;
    if (!i2c_read(MPU_ADDR, MPU_WHO_AM_I, &id, 1) || id != 0x68u)
        return 0;                       /* нет ответа или это не MPU-6050 */
    i2c_write_reg(MPU_ADDR, MPU_PWR_MGMT_1, 0x00u);     /* разбудить: после включения он спит */
    i2c_write_reg(MPU_ADDR, MPU_ACCEL_CONFIG, (uint8_t)(afs << 3));
    i2c_write_reg(MPU_ADDR, MPU_GYRO_CONFIG, (uint8_t)(fs << 3));
    return 1;
}

void mpu_parse(const uint8_t b[14], mpu_raw_t *r)
{
    r->ax = be16(b);
    r->ay = be16(b + 2);
    r->az = be16(b + 4);
    r->temp = be16(b + 6);
    r->gx = be16(b + 8);
    r->gy = be16(b + 10);
    r->gz = be16(b + 12);
}

int32_t mpu_temp_c100(int16_t raw)
{
    return (int32_t)raw * 100 / 340 + 3653;
}

i2c_read и i2c_write_reg — функции шины из раздела 17. На компьютере вместо шины — модель регистров датчика: до mpu_init чтение отдаёт нули, после — коды 8192, 0, 14189, −1500, 131, −262 и 0. При ±2 g это 0,5 g по X и 0,866 g по Z; температура — 32,12 °C (это температура кристалла, а не воздуха); угловые скорости — 1,00 и −2,00 °/с.

99.4 ADXL345: тот же разбор задом наперёд

У ADXL345 шесть байт с 0x32, и у каждого значения первым идёт младший байт:

void adxl_parse(const uint8_t b[6], int16_t a[3])
{
    a[0] = le16(b);
    a[1] = le16(b + 2);
    a[2] = le16(b + 4);
}

Байты 00 01 00 00 FF 00 — это X = 256 и Z = 255, около 1 g. Разобранные «как у MPU-6050», старшим байтом вперёд, они дают X = 1 и Z = −256 — правдоподобные числа, в которых нет ни капли смысла. Такую ошибку не видно по схеме и не видно по отсутствию ответа: датчик отвечает, числа меняются, но не так.

99.5 Наклон по акселерометру

В покое вектор ускорения — это тяжесть, и углы получаются из его проекций:

void tilt_deg(float ax, float ay, float az, float *pitch, float *roll)
{
    *pitch = atan2f(-ax, sqrtf(ay * ay + az * az)) * 57.29578f;
    *roll = atan2f(ay, az) * 57.29578f;
}

0,5 g по X и 0,866 g по Z — это тангаж −30,00° и крен 0,00°. Работает это только в покое или при равномерном движении: любое ускорение наклоняет вектор, и в тряском приборе такой наклон дёргается.

99.6 Гироскоп: ноль и уход

Гироскоп в покое показывает не ноль, а своё смещение, и при интегрировании оно копится. Модель: плата неподвижна, гироскоп показывает смещение 0,5 °/с, отсчёты — 100 раз в секунду. Через 60 с угол по гироскопу уехал на 30,00°.

Первое лекарство — калибровка нуля: в покое усреднить отсчёты и потом вычитать среднее:

float gyro_bias(const int16_t *raw, unsigned n, unsigned fs)
{
    int32_t sum = 0;
    for (unsigned i = 0; i < n; i++)
        sum += raw[i];
    return sum * 10.0f / ((float)n * mpu_lsb_per_dps_x10[fs]);
}

По 200 отсчётам с шумом модель оценила смещение в 0,498 °/с (на самом деле — 0,504), и после вычитания за те же 60 с угол ушёл на 0,341° вместо 30 (точнее, 30,2°: в этой модели смещение задано целым кодом — 0,5 × 131 = 65,5 округлено до 66, то есть 0,504 °/с). Но смещение гуляет с температурой, так что одной калибровки при включении мало — нужен второй источник угла.

99.7 Комплементарный фильтр

Гироскопу можно верить на коротком отрезке, акселерометру — на длинном. Комплементарный фильтр так и делает: к углу прибавляет поворот по гироскопу и чуть-чуть подтягивает результат к углу по акселерометру:

float comp_step(float angle, float gyro_dps, float acc_deg, float dt, float alpha)
{
    return alpha * (angle + gyro_dps * dt) + (1.0f - alpha) * acc_deg;
}

При α = 0,98 и шаге 0,01 с постоянная времени α·dt / (1 − α) = 0,49 с: то, что меняется быстрее, приходит от гироскопа, медленнее — от акселерометра. Смещение гироскопа даёт теперь не растущую, а постоянную ошибку — всего 0,245°. Модель: плата неподвижна под 10°, гироскоп со смещением 0,5 °/с, у акселерометра шум с разбросом 2°. После первых 10 с ошибка фильтра не выходила за 0,695°, а угол по одному гироскопу за то же время ушёл на 30,00°.

99.8 Грабли

MPU-6050 отдаёт одни нули. После включения он спит. Записать 0 в PWR_MGMT_1 (0x6B).

ADXL345 отдаёт нули. Не включено измерение — бит 3 регистра 0x2D.

Показания ADXL345 — мусор при верной схеме. Порядок байт: у ADXL345 младший первым, у MPU-6050 — старший.

Угол по гироскопу уходит на десятки градусов за минуту. Интегрируется смещение нуля. Калибровать ноль в покое и корректировать акселерометром.

Наклон по акселерометру дёргается при движении. Акселерометр меряет не только тяжесть. Нужен фильтр вместе с гироскопом.

Температура MPU-6050 выше комнатной. Это температура кристалла, а не воздуха.

Рыскание по акселерометру не получается. Поворот вокруг вертикали не меняет вектор тяжести. Нужен магнитометр.

99.9 Итоги

• Акселерометр в покое показывает тяжесть — отсюда тангаж и крен; рыскание он не видит.
• Гироскоп даёт угловую скорость; угол — интеграл, и смещение 0,5 °/с за минуту набегает в 30,00°.
• MPU-6050 после включения спит, ADXL345 — в ожидании: обоих надо включить.
• Порядок байт: у MPU-6050 старший первым, у ADXL345 — младший.
• Калибровка нуля в покое сокращает уход с 30 градусов до 0,341° в минуту, но смещение гуляет с температурой.
• Комплементарный фильтр с α = 0,98 при 100 Гц — постоянная времени 0,49 с; смещение гироскопа даёт постоянную ошибку 0,245° вместо растущей.

Дальше: раздел 99а «Полётный контроллер на STM32: разбор Betaflight — гироскоп 8 кГц, фильтры, PID и DShot» — тот же гироскоп, но по SPI на 8 кГц: фильтры против шума моторов, регулятор угловой скорости и цифровой протокол моторов DShot.