Создание системы ориентации и высоты на базе ICM20948 и BMP280

В этом уроке мы создадим систему, которая измеряет:

• Тангаж - наклон вперед/назад
• Крен - наклон влево/вправо
• Рыскание - поворот вокруг оси
• Высоту - в сантиметрах относительно начальной точки

Необходимые компоненты

1. Плата с ICM20948 + BMP280
2. Arduino Uno/Nano/Mega
3. Провода для подключения
4. USB-кабель

Теоретическая часть

Как работает IMU (Inertial Measurement Unit)

ICM20948 содержит три датчика:

• Акселерометр - измеряет линейное ускорение
• Гироскоп - измеряет угловую скорость
• Магнитометр - определяет направление (в нашем коде не используется)

Как работает барометр

BMP280 измеряет:

• Атмосферное давление - уменьшается с высотой
• Температуру - для компенсации температурных эффектов

Формула расчета высоты:

h = 44330 × (1 - (P/P₀)^(1/5.255))

Схема подключения:

ICM20948+BMP280    Arduino
VCC               → 3.3V
GND               → GND
SDA               → A4 (SDA)
SCL               → A5 (SCL)

Установка библиотек

1. Adafruit ICM20948 (через менеджер библиотек)
2. iarduino_Pressure_BMP (через менеджер библиотек)
3. Adafruit Sensor (установится автоматически)

Ключевые функции:

calibrateGyro() - Калибрует гироскоп:

<i>// Усредняет 50 измерений в неподвижном состоянии</i>
<i>// Определяет нулевые смещения</i>

calibrateAlt() - Калибрует высоту:

<i>// Запоминает текущее давление как опорное</i>
<i>// Устанавливает текущую позицию как 0 см</i>

processIMUData() - Обрабатывает данные IMU:

<i>// Комплементарный фильтр объединяет:</i>
<i>// - Гироскоп (точность в движении)</i>
<i>// - Акселерометр (точность в статике)</i>

Шаг 1: Загрузка и тестирование

1. Загрузите код в Arduino
2. Откройте Serial Monitor (115200 бод)
3. Наблюдайте данные в формате:

Pitch    Roll    Yaw    AltCm

Шаг 2: Калибровка

Калибровка гироскопа:

• Установите устройство неподвижно
• Отправьте команду c
• Ждите завершения калибровки

Калибровка высоты:

• Установите устройство в нулевой точке
• Отправьте команду h
• Текущая позиция = 0 см

Шаг 3: Тестирование функций

Проверьте работу:

1. Наклоните устройство - наблюдайте изменение Pitch/Roll
2. Повращайте устройство - наблюдайте изменение Yaw
3. Поднимите/опустите - наблюдайте изменение высоты

 Команды для отладки

Анализ данных

Пример вывода:

Pitch    Roll    Yaw    AltCm
12.5    -3.2    45.0    0.0
15.1    -2.8    46.2    15.3

Чтение данных:

• Pitch = 15.1° - наклон вперед
• Roll = -2.8° - небольшой крен влево
• Yaw = 46.2° - поворот на 46 градусов
• AltCm = 15.3 - высота 15.3 см от нуля

Объяснение ключевых концепций

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

Проблема:

• Акселерометр: точен в статике, но шумит при движении
• Гироскоп: точен в движении, но дрейфует со временем

Решение:

angle = 0.98 × (angle + gyro × dt) + 0.02 × acc_angle

Барометрическая формула

Как давление преобразуется в высоту:

float pressureRatio = currentPressure / initPress;
float altitude = 44330.0 * (1.0 - pow(pressureRatio, 0.1903));

 Возможные проблемы и решения

1. Некорректные значения высоты:
   • Перекалибруйте высоту командой h
   • Дайте датчику прогреться 5-10 минут
2. Дрейф углов:
   • Перекалибруйте гироскоп командой c
   • Убедитесь в неподвижности при калибровке
3. Шум в данных:
   • Разместите устройство на устойчивой поверхности
   • Используйте фильтрацию в обработке данных

#include <wire.h>
#include <adafruit_icm20948.h>
#include <iarduino_pressure_bmp.h>

Adafruit_ICM20948 icm;
iarduino_Pressure_BMP bmp;

// Переменные для углов ориентации
float pitch, roll, yaw;

// Переменные для высоты и калибровки
float initPress = 101325.0; // Стандартное давление (1013.25 гПа)
float initAlt = 0.0;

// Переменные времени и калибровки гироскопа
unsigned long lastTime;
float gXoff, gYoff, gZoff;

// Флаги датчиков (бит 0: IMU, бит 1: BMP)
byte sensorsOK = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  Serial.println(F("=== СИСТЕМА ОРИЕНТАЦИИ И ВЫСОТЫ ==="));
  
  // Инициализация ICM20948 (IMU)
  if (icm.begin_I2C(0x69) || icm.begin_I2C(0x68)) {
    sensorsOK |= 1;
    Serial.println(F("ICM20948: ОК"));
    calibrateGyro();
  } else {
    Serial.println(F("ICM20948: Ошибка подключения"));
  }

  // Инициализация BMP280 (Барометр)
  if (bmp.begin()) {
    sensorsOK |= 2;
    Serial.println(F("BMP280: ОК"));
    calibrateAlt();
  } else {
    Serial.println(F("BMP280: Ошибка подключения"));
  }

  Serial.println(F("========================"));
  Serial.println(F("Pitch\tRoll\tYaw\tAltCm"));
  Serial.println(F("========================"));
  
  lastTime = micros();
}

void loop() {
  // ОБРАБОТКА ДАННЫХ IMU
  if (sensorsOK & 1) {
    processIMUData();
  }

  // ВЫЧИСЛЕНИЕ ВЫСОТЫ
  float altCm = 0;
  if (sensorsOK & 2) {
    altCm = calculateAltitude();
  }

  // ВЫВОД РЕЗУЛЬТАТОВ
  printData(altCm);
  
  // ПРОВЕРКА КОМАНД
  checkSerialCommands();
  
  delay(100); // 10 Hz update rate
}

// Обработка данных IMU
void processIMUData() {
  sensors_event_t accel, gyro, mag;
  if (icm.getEvent(&accel, &gyro, &mag)) {
    float dt = (micros() - lastTime) / 1000000.0;
    lastTime = micros();

    // Преобразование данных гироскопа
    float gX = (gyro.gyro.x - gXoff) * 57.2958;
    float gY = (gyro.gyro.y - gYoff) * 57.2958;
    float gZ = (gyro.gyro.z - gZoff) * 57.2958;

    // Углы из акселерометра
    float accPitch = atan2(accel.acceleration.y, accel.acceleration.z) * 57.2958;
    float accRoll = atan2(-accel.acceleration.x, 
                         sqrt(accel.acceleration.y*accel.acceleration.y + 
                              accel.acceleration.z*accel.acceleration.z)) * 57.2958;

    // Комплементарный фильтр
    pitch = 0.98 * (pitch + gX * dt) + 0.02 * accPitch;
    roll = 0.98 * (roll + gY * dt) + 0.02 * accRoll;
    yaw += gZ * dt;

    // Нормализация углов
    normalizeAngles();
  }
}

// Вычисление высоты
float calculateAltitude() {
  bmp.read();
  float currentPressure = bmp.pressure;
  float pressureRatio = currentPressure / initPress;
  float currentAltitude = 44330.0 * (1.0 - pow(pressureRatio, 0.1903));
  return (currentAltitude - initAlt) * 100.0; // Возвращаем в см
}

// Вывод данных
void printData(float altitude) {
  Serial.print(pitch, 1);  // 1 знак после запятой
  Serial.print('\t');
  Serial.print(roll, 1);
  Serial.print('\t');
  Serial.print(yaw, 1);
  Serial.print('\t');
  Serial.println(altitude, 1);
}

// Нормализация углов
void normalizeAngles() {
  pitch = constrain(pitch, -90, 90);
  roll = constrain(roll, -180, 180);
  if (yaw > 180) yaw -= 360;
  if (yaw < -180) yaw += 360;
}

// Калибровка гироскопа
void calibrateGyro() {
  Serial.println(F("Калибровка гироскопа..."));
  Serial.println(F("Не двигайте устройство 2 сек"));
  delay(2000);
  
  float x = 0, y = 0, z = 0;
  for (byte i = 0; i < 50; i++) {
    sensors_event_t a, g, m;
    icm.getEvent(&a, &g, &m);
    x += g.gyro.x; y += g.gyro.y; z += g.gyro.z;
    if (i % 10 == 0) Serial.print('.');
    delay(20);
  }
  gXoff = x / 50; gYoff = y / 50; gZoff = z / 50;
  Serial.println();
  Serial.println(F("Гироскоп откалиброван"));
}

// Калибровка высоты
void calibrateAlt() {
  Serial.println(F("Калибровка высоты..."));
  Serial.println(F("Установите нулевую точку"));
  delay(2000);
  
  float sumPressure = 0;
  for (byte i = 0; i < 10; i++) {
    bmp.read();
    sumPressure += bmp.pressure;
    Serial.print('.');
    delay(100);
  }
  
  initPress = sumPressure / 10;
  initAlt = 0.0;
  
  Serial.println();
  Serial.print(F("Нулевое давление: "));
  Serial.print(initPress / 100.0);
  Serial.println(F(" гПа"));
}

// Проверка команд Serial
void checkSerialCommands() {
  if (Serial.available()) {
    char command = Serial.read();
    switch (command) {
      case 'c': if (sensorsOK & 1) calibrateGyro(); break;
      case 'h': if (sensorsOK & 2) calibrateAlt(); break;
      case 'r': pitch = 0; roll = 0; yaw = 0; Serial.println(F("Углы сброшены")); break;
      case 's': showStatus(); break;
      case 'd': showDebugInfo(); break;
      case '?': showHelp(); break;
    }
  }
}

// Показать статус системы
void showStatus() {
  Serial.println(F("\n=== СТАТУС СИСТЕМЫ ==="));
  Serial.print(F("IMU: ")); Serial.println(sensorsOK & 1 ? "OK" : "ERROR");
  Serial.print(F("BMP: ")); Serial.println(sensorsOK & 2 ? "OK" : "ERROR");
  Serial.print(F("Опорное давление: ")); Serial.print(initPress/100.0); Serial.println(F(" гПа"));
  Serial.println(F("===================="));
}

// Отладочная информация
void showDebugInfo() {
  Serial.println(F("\n=== ДЕБАГ ИНФО ==="));
  if (sensorsOK & 2) {
    bmp.read();
    Serial.print(F("Давление: ")); Serial.print(bmp.pressure); Serial.println(F(" Па"));
    Serial.print(F("Температура: ")); Serial.print(bmp.temperature); Serial.println(F(" °C"));
  }
  Serial.print(F("Смещения гиро: "));
  Serial.print(gXoff, 6); Serial.print(F(" "));
  Serial.print(gYoff, 6); Serial.print(F(" "));
  Serial.println(gZoff, 6);
  Serial.println(F("=================="));
}

// Справка по командам
void showHelp() {
  Serial.println(F("\n=== КОМАНДЫ ==="));
  Serial.println(F("c - Калибровать гироскоп"));
  Serial.println(F("h - Калибровать высоту"));
  Serial.println(F("r - Сбросить углы"));
  Serial.println(F("s - Статус системы"));
  Serial.println(F("d - Дебаг информация"));
  Serial.println(F("? - Эта справка"));
  Serial.println(F("================"));
}</iarduino_pressure_bmp.h></adafruit_icm20948.h></wire.h>