ARDUINO И ДАТЧИК MPU6050


MPU6050 представляет собой 3-х осевой гироскоп и 3-х же осевой акселерометр в одном корпусе. Сразу поясню, что это и для чего:

  • Гироскоп измеряет угловую скорость вращения вокруг оси, условно в градусах/секунду. Если датчик лежит на столе – по всем трём осям будет значение около нуля. Для нахождения текущего угла по скорости нужно интегрировать эту скорость. Гироскоп может чуть привирать, поэтому ориентироваться на него для расчёта текущего угла не получится даже при идеальной калибровке.
  • Акселерометр измеряет ускорение вдоль оси, условно в метрах/секунду/секунду. Если датчик лежит на столе или движется с постоянной скоростью – на оси будет спроецирован вектор силы тяжести. Если датчик движется с ускорением – вдобавок к ускорению свободного падения получим составляющие вектора ускорения. Если датчик находится в свободном падении (в том числе на орбите планеты) – величины ускорений по всем осям будут равны 0. Зная проекции вектора силы тяжести можно с высокой точностью определить угол наклона датчика относительно него (привет, школьная математика). Если датчик движется – однозначно определить направление вектора силы тяжести не получится, соответственно угол тоже.

Итак, по отдельности акселерометр и гироскоп не могут обеспечить точное измерение угла, но вместе – ещё как могут! Об этом мы поговорим ниже. А начнём с подключения и базового общения с датчиком.

ПОДКЛЮЧЕНИЕ


Датчик подключается на шину i2c (SDA -> A4, SCL -> A5, GND -> GND). На плате стоит стабилизатор, позволяющий питаться от пина 5V (VCC -> 5V).

На модуле выведен пин AD0. Если он никуда не подключен (или подключен к GND) – адрес датчика на шине i2c будет 0x68, если подключен к питанию (VCC) – адрес будет 0x69. Таким образом без дополнительных микросхем можно подключить два датчика на шину с разными адресами.

ПОЛУЧЕНИЕ СЫРЫХ ДАННЫХ


С MPU6050 можно общаться напрямую при помощи встроенной библиотеки Wire.h и даташита, а можно взять очень мощную библиотеку от i2cdev. Для работы понадобится сама библиотека MPU6050 и библиотека I2Cdev (удобного способа скачать там нет, поэтому выложил архив с обеими на Яндекс.Диск).

Мы будем работать с библиотекой, но я оставлю пример опроса всех ускорений и угловых скоростей напрямую, т.к. библиотека тяжёлая, а пример очень лёгкий.

#include "Wire.h"
const int MPU_addr = 0x68; // адрес датчика

// массив данных
// [accX, accY, accZ, temp, gyrX, gyrY, gyrZ]
// acc - ускорение, gyr - угловая скорость, temp - температура (raw)
int16_t data[7];  

void setup() {
  // инициализация
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  
  Serial.begin(9600);
}
void loop() {
  getData();  // получаем

  // выводим 
  for (byte i = 0; i < 7; i++) {
    Serial.print(data[i]);
    Serial.print('\t');
  }
  Serial.println();
  delay(200);
}

void getData() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  for (byte i = 0; i < 7; i++) {
    data[i] = Wire.read() << 8 | Wire.read();
  }
}

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  // состояние соединения
  Serial.println(mpu.testConnection() ? "MPU6050 OK" : "MPU6050 FAIL");
  delay(1000);
}

void loop() {
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  Serial.print(ax); Serial.print('\t');
  Serial.print(ay); Serial.print('\t');
  Serial.print(az); Serial.print('\t');
  Serial.print(gx); Serial.print('\t');
  Serial.print(gy); Serial.print('\t');
  Serial.println(gz);

  delay(5);
}

РАСШИФРОВКА СЫРЫХ ДАННЫХ


По каждой оси и параметру датчик выдаёт 16-битное знаковое значение (-32768.. 32767). При стандартных настройках (как в примерах выше) это значение отражает:

  • Ускорение в диапазоне -2.. 2 g (9.82 м/с/с).
  • Угловую скорость в диапазоне -250.. 250 градусов/секунду.

Таким образом, для перевода сырых данных в величины СИ (если это нужно) можно сделать по каждой оси:

  • Ускорение в м/с/с при чувствительности 2: float accX_f = accX / 32768 * 2
  • Угловая скорость в град/с при чувствительности 250: float gyrX_f = gyrX / 32768 * 250

Думаю закономерность понятна.

НАСТРОЙКА ЧУВСТВИТЕЛЬНОСТИ


Датчик позволяет настроить чувствительность по каждому параметру, в библиотеке i2cdev это делается так:

  • setFullScaleAccelRange(MPU6050_ACCEL_FS_2); - диапазон -2.. 2 g
  • setFullScaleAccelRange(MPU6050_ACCEL_FS_4); - диапазон -4.. 4 g
  • setFullScaleAccelRange(MPU6050_ACCEL_FS_8); - диапазон -8.. 8 g
  • setFullScaleAccelRange(MPU6050_ACCEL_FS_16); - диапазон -16.. 16 g
  • setFullScaleGyroRange(MPU6050_GYRO_FS_250); - диапазон -250.. 250 град/с
  • setFullScaleGyroRange(MPU6050_GYRO_FS_500); - диапазон -500.. 500 град/с
  • setFullScaleGyroRange(MPU6050_GYRO_FS_1000); - диапазон -1000.. 1000 град/с
  • setFullScaleGyroRange(MPU6050_GYRO_FS_2000); - диапазон -2000.. 2000 град/с
void setup() {
  Wire.begin();
  mpu.initialize();
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
}

Перевести сырые данные можно таким же способом, как в предыдущем пункте, но с учётом нового диапазона.

КАЛИБРОВКА ОФФСЕТОВ


Для достижения максимальной точности измерений нужно откалибровать акселерометр и гироскоп. Калибровка акселерометра позволяет выставить "ноль" для вектора силы тяжести, а калибровка гироскопа уменьшает его "дрифт", то есть статическое отклонение в режиме покоя. Идеально откалиброванный и лежащий горизонтально датчик должен показывать ускорение ~16384 по оси Z и нули по всем остальным осям ускорения и угловой скорости. Но это фантастика =)

Максимально правильно использовать калибровку в проекте нужно так: калибровка по запросу (кнопка, меню, и т.д.), затем запись калибровочных значений в EEPROM. При запуске - чтение и настройка оффсетов из. Рассмотрим несколько примеров калибровки, первый - из библиотеки. Калибрует долго, но максимально точно. При малых вибрациях и движениях датчика в процессе калибровки (даже от громкого звука) калибровка может не закончиться. Второй вариант - мой упрощённый алгоритм калибровки, калибрует быстро, без возможности зависнуть при тряске, но даёт менее точный результат. Я делюсь примерами, в свой проект их нужно будет переносить вручную и аккуратно =)

// положи датчик горизонтально, надписями вверх
// ДАЖЕ НЕ ДЫШИ НА НЕГО
// отправь любой символ в сериал, чтобы начать калибровку
// жди результатов

// --------------------- НАСТРОЙКИ ----------------------
const int buffersize = 70;     // количество итераций калибровки
const int acel_deadzone = 10;  // точность калибровки акселерометра (по умолчанию 8)
const int gyro_deadzone = 6;   // точность калибровки гироскопа (по умолчанию 2)
// --------------------- НАСТРОЙКИ ----------------------

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

MPU6050 mpu(0x68);

int16_t ax, ay, az, gx, gy, gz;

int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;
int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////
void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();

  // ждём очистки сериал
  while (Serial.available() && Serial.read()); // чистим
  while (!Serial.available()) {
    Serial.println(F("Send any character to start sketch.\n"));
    delay(1500);
  }
  while (Serial.available() && Serial.read()); // чистим ещё на всякий

  Serial.println("\nMPU6050 Calibration Sketch");
  delay(2000);
  Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
  delay(3000);
  // проверка соединения
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(1000);
  // сбросить оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
}

///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {
  if (state == 0) {
    Serial.println("\nReading sensors for first time...");
    meansensors();
    state++;
    delay(1000);
  }

  if (state == 1) {
    Serial.println("\nCalculating offsets...");
    calibration();
    state++;
    delay(1000);
  }

  if (state == 2) {
    meansensors();
    Serial.println("\nFINISHED!");
    Serial.print("\nSensor readings with offsets:\t");
    Serial.print(mean_ax);
    Serial.print("\t");
    Serial.print(mean_ay);
    Serial.print("\t");
    Serial.print(mean_az);
    Serial.print("\t");
    Serial.print(mean_gx);
    Serial.print("\t");
    Serial.print(mean_gy);
    Serial.print("\t");
    Serial.println(mean_gz);
    Serial.print("Your offsets:\t");
    Serial.print(ax_offset);
    Serial.print(", ");
    Serial.print(ay_offset);
    Serial.print(", ");
    Serial.print(az_offset);
    Serial.print(", ");
    Serial.print(gx_offset);
    Serial.print(", ");
    Serial.print(gy_offset);
    Serial.print(", ");
    Serial.println(gz_offset);
    Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
    Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
    while (1);
  }
}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors() {
  long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;

  while (i < (buffersize + 101)) { // read raw accel/gyro measurements from device mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded
      buff_ax = buff_ax + ax;
      buff_ay = buff_ay + ay;
      buff_az = buff_az + az;
      buff_gx = buff_gx + gx;
      buff_gy = buff_gy + gy;
      buff_gz = buff_gz + gz;
    }
    if (i == (buffersize + 100)) {
      mean_ax = buff_ax / buffersize;
      mean_ay = buff_ay / buffersize;
      mean_az = buff_az / buffersize;
      mean_gx = buff_gx / buffersize;
      mean_gy = buff_gy / buffersize;
      mean_gz = buff_gz / buffersize;
    }
    i++;
    delay(2);
  }
}

void calibration() {
  ax_offset = -mean_ax / 8;
  ay_offset = -mean_ay / 8;
  az_offset = (16384 - mean_az) / 8;

  gx_offset = -mean_gx / 4;
  gy_offset = -mean_gy / 4;
  gz_offset = -mean_gz / 4;
  while (1) {
    int ready = 0;
    mpu.setXAccelOffset(ax_offset);
    mpu.setYAccelOffset(ay_offset);
    mpu.setZAccelOffset(az_offset);

    mpu.setXGyroOffset(gx_offset);
    mpu.setYGyroOffset(gy_offset);
    mpu.setZGyroOffset(gz_offset);

    meansensors();
    Serial.println("...");

    if (abs(mean_ax) <= acel_deadzone) ready++;
    else ax_offset = ax_offset - mean_ax / acel_deadzone;

    if (abs(mean_ay) <= acel_deadzone) ready++;
    else ay_offset = ay_offset - mean_ay / acel_deadzone;

    if (abs(16384 - mean_az) <= acel_deadzone) ready++;
    else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;

    if (abs(mean_gx) <= gyro_deadzone) ready++;
    else gx_offset = gx_offset - mean_gx / (gyro_deadzone + 1);

    if (abs(mean_gy) <= gyro_deadzone) ready++;
    else gy_offset = gy_offset - mean_gy / (gyro_deadzone + 1);

    if (abs(mean_gz) <= gyro_deadzone) ready++;
    else gz_offset = gz_offset - mean_gz / (gyro_deadzone + 1);

    if (ready == 6) break;
  }
}

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;

#define BUFFER_SIZE 100

int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();

  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);

  Serial.println(F("Send any character to start sketch"));
  delay(100);
  while (1) {  						  //входим в бесконечный цикл
    if (Serial.available() > 0) {     //если нажата любая кнопка
      Serial.read();                  //прочитать (чтобы не висел в буфере)
      break;                          //выйти из цикла
    }
  }
  delay(1000);
}


void loop() {
  // выводим начальные значения
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);

  calibration();

  // выводим значения после калибровки
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);
  delay(20);

  while (1);
}

// ======= ФУНКЦИЯ КАЛИБРОВКИ ======= 
void calibration() {
  long offsets[6];
  long offsetsOld[6];
  int16_t mpuGet[6];

  // используем стандартную точность
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);

  // обнуляем оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  delay(10);
  Serial.println("Calibration start. It will take about 5 seconds");

  for (byte n = 0; n < 10; n++) {     // 10 итераций калибровки
    for (byte j = 0; j < 6; j++) {    // обнуляем калибровочный массив
      offsets[j] = 0;
    }
    for (byte i = 0; i < 100 + BUFFER_SIZE; i++) { // делаем BUFFER_SIZE измерений для усреднения mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]); if (i >= 99)                         // пропускаем первые 99 измерений
        for (byte j = 0; j < 6; j++) {
          offsets[j] += (long)mpuGet[j];   // записываем в калибровочный массив
        }
    }
    for (byte i = 0; i < 6; i++) {      
      offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку
      if (i == 2) offsets[i] += 16384;                               // если ось Z, калибруем в 16384
      offsetsOld[i] = offsets[i];
    }
    // ставим новые оффсеты
    mpu.setXAccelOffset(offsets[0] / 8);
    mpu.setYAccelOffset(offsets[1] / 8);
    mpu.setZAccelOffset(offsets[2] / 8);
    mpu.setXGyroOffset(offsets[3] / 4);
    mpu.setYGyroOffset(offsets[4] / 4);
    mpu.setZGyroOffset(offsets[5] / 4);
    delay(2);
  }
  /*
  // выводим в порт
  Serial.println("Calibration end. Your offsets:");
  Serial.println("accX accY accZ gyrX gyrY gyrZ");
  Serial.print(mpu.getXAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getYAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getZAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getXGyroOffset()); Serial.print(", ");
  Serial.print(mpu.getYGyroOffset()); Serial.print(", ");
  Serial.print(mpu.getZGyroOffset()); Serial.println(" ");
  Serial.println(" ");
  */
}

// калибровка и запись в EEPROM
// при запуске настраиваем оффсеты

// послать символ в сериал - начать повторную калибровку

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "EEPROMex.h"
MPU6050 mpu;

#define BUFFER_SIZE 100
#define START_BYTE 1010

int16_t ax, ay, az;
int16_t gx, gy, gz;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();

  //----------------- ВСПОМИНАЕМ ОФФСЕТЫ ------------------------
  int offsets[6];
  for (byte i = 0; i < 6; i++) { // обнуляем калибровочный массив // по умолч оффсеты хранятся в ячейках 1010, 1012, 1014, 1016, 1018, 1020 offsets[i] = EEPROM.readInt(START_BYTE + i * 2); } // ставим оффсеты из памяти mpu.setXAccelOffset(offsets[0]); mpu.setYAccelOffset(offsets[1]); mpu.setZAccelOffset(offsets[2]); mpu.setXGyroOffset(offsets[3]); mpu.setYGyroOffset(offsets[4]); mpu.setZGyroOffset(offsets[5]); //----------------- ВСПОМИНАЕМ ОФФСЕТЫ ------------------------ Serial.println(F("Send any character to start sketch")); delay(100); while (1) { //входим в бесконечный цикл if (Serial.available() > 0) {     //если нажата любая кнопка
      Serial.read();                  //прочитать (чтобы не висел в буфере)
      break;                          //выйти из цикла
    }
  }
  delay(1000);
}

void loop() {
  calibration();

  Serial.println("Current offsets:");
  Serial.println("accX accY accZ gyrX gyrY gyrZ");
  Serial.print(mpu.getXAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getYAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getZAccelOffset()); Serial.print(", ");
  Serial.print(mpu.getXGyroOffset()); Serial.print(", ");
  Serial.print(mpu.getYGyroOffset()); Serial.print(", ");
  Serial.print(mpu.getZGyroOffset()); Serial.println(" ");
  Serial.println(" ");  

  // выводим значения после калибровки
  Serial.println("Readings:");
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print(" ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.println(gz);
  delay(20);

  while (1);
}

// =======  ФУНКЦИЯ КАЛИБРОВКИ И ЗАПИСИ В ЕЕПРОМ ======= 
void calibration() {
  long offsets[6];
  long offsetsOld[6];
  int16_t mpuGet[6];

  // используем стандартную точность
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);

  // обнуляем оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  delay(5);

  for (byte n = 0; n < 10; n++) {     // 10 итераций калибровки
    for (byte j = 0; j < 6; j++) {    // обнуляем калибровочный массив
      offsets[j] = 0;
    }
    for (byte i = 0; i < 100 + BUFFER_SIZE; i++) { // делаем BUFFER_SIZE измерений для усреднения mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]); if (i >= 99)                         // пропускаем первые 99 измерений
        for (byte j = 0; j < 6; j++) {
          offsets[j] += (long)mpuGet[j];   // записываем в калибровочный массив
        }
    }
    for (byte i = 0; i < 6; i++) {
      offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку
      if (i == 2) offsets[i] += 16384;                               // если ось Z, калибруем в 16384
      offsetsOld[i] = offsets[i];
    }
    // ставим новые оффсеты
    mpu.setXAccelOffset(offsets[0] / 8);
    mpu.setYAccelOffset(offsets[1] / 8);
    mpu.setZAccelOffset(offsets[2] / 8);
    mpu.setXGyroOffset(offsets[3] / 4);
    mpu.setYGyroOffset(offsets[4] / 4);
    mpu.setZGyroOffset(offsets[5] / 4);
    delay(2);
  }

  // запись в память
  for (byte i = 0; i < 6; i++) {    // обнуляем калибровочный массив
    // оффсеты хранятся в ячейках 1010, 1012, 1014, 1016, 1018, 1020
    if (i < 3)
      EEPROM.updateInt(START_BYTE + i * 2, offsets[i] / 8);
    else
      EEPROM.updateInt(START_BYTE + i * 2, offsets[i] / 4);
  }
}

// ======= ФУНКЦИЯ КАЛИБРОВКИ ======= 
void calibration() {
  long offsets[6];
  long offsetsOld[6];
  int16_t mpuGet[6];

  // используем стандартную точность
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);

  // обнуляем оффсеты
  mpu.setXAccelOffset(0);
  mpu.setYAccelOffset(0);
  mpu.setZAccelOffset(0);
  mpu.setXGyroOffset(0);
  mpu.setYGyroOffset(0);
  mpu.setZGyroOffset(0);
  delay(10);

  for (byte n = 0; n < 10; n++) {     // 10 итераций калибровки
    for (byte j = 0; j < 6; j++) {    // обнуляем калибровочный массив
      offsets[j] = 0;
    }
    for (byte i = 0; i < 100 + BUFFER_SIZE; i++) { // делаем BUFFER_SIZE измерений для усреднения mpu.getMotion6(&mpuGet[0], &mpuGet[1], &mpuGet[2], &mpuGet[3], &mpuGet[4], &mpuGet[5]); if (i >= 99)                         // пропускаем первые 99 измерений
        for (byte j = 0; j < 6; j++) {
          offsets[j] += (long)mpuGet[j];   // записываем в калибровочный массив
        }
    }
    for (byte i = 0; i < 6; i++) {      
      offsets[i] = offsetsOld[i] - ((long)offsets[i] / BUFFER_SIZE); // учитываем предыдущую калибровку
      if (i == 2) offsets[i] += 16384;                               // если ось Z, калибруем в 16384
      offsetsOld[i] = offsets[i];
    }

    // ставим новые оффсеты
    mpu.setXAccelOffset(offsets[0] / 8);
    mpu.setYAccelOffset(offsets[1] / 8);
    mpu.setZAccelOffset(offsets[2] / 8);
    mpu.setXGyroOffset(offsets[3] / 4);
    mpu.setYGyroOffset(offsets[4] / 4);
    mpu.setZGyroOffset(offsets[5] / 4);
    delay(2);
  }
}

Также в библиотеке есть готовые функции для калибровки акселя и гиро, они принимают количество итераций калибровки (до 15):

mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);

Функции блокирующие, выполняются и автоматически выставляют оффсеты, т.е. датчик сразу калибруется. Для чтения оффсетов (например для записи в EEPROM) можно воспользоваться тем же способом, что и раньше:

mpu.getXAccelOffset();
mpu.getYAccelOffset();
mpu.getZAccelOffset();
mpu.getXGyroOffset();
mpu.getYGyroOffset();
mpu.getZGyroOffset();

ПОЛУЧЕНИЕ УГЛОВ


Для получения углов из показаний акселерометра и гироскопа есть три основных способа:

  • Фильтрация при помощи фильтра Калмана.
  • Фильтрация при помощи комплементарного фильтра.
  • Определение углов при помощи встроенного в датчик DMP (Digital Motion Processor).

Из всех трёх самый хороший результат даёт встроенный процессор, на его фоне Калман и комплементари можно даже не рассматривать, они практически не работают. Единственное, способ через DMP занимает довольно таки много памяти (12 212 байт Flash и 541 байт оперативки), а также сама функция сравнительно долго выполняется (~4.5 миллисекунды).

Пример с получением углов таким способом есть в библиотеке (DMPangles), но там как говорится чёрт ногу сломит, поэтому прилагаю упрощённый и укороченный вариант:

// получаем углы при помощи DMP
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"
#include "Wire.h"
MPU6050 mpu;

float angleX = 0;
float angleY = 0;
float angleZ = 0;

void setup() {
  Wire.begin();
  Wire.setClock(400000);
  Serial.begin(115200);
  mpu.initialize();
  initDMP();
}

void loop() {
  getAngles();

  Serial.print(angleX); Serial.print(',');
  Serial.print(angleY); Serial.print(',');
  Serial.println(angleZ);
  delay(20);
}

// НУЖНЫЕ ПЕРЕМЕННЫЕ
const float toDeg = 180.0 / M_PI;
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// инициализация
void initDMP() {
  devStatus = mpu.dmpInitialize();
  mpu.setDMPEnabled(true);
  mpuIntStatus = mpu.getIntStatus();
  packetSize = mpu.dmpGetFIFOPacketSize();
}

// получение углов в angleX, angleY, angleZ
void getAngles() {
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    angleX = ypr[2] * toDeg;
    angleY = ypr[1] * toDeg;
    angleZ = ypr[0] * toDeg;
  }
}

Также приложу совмещённый пример с упрощённым комплементари, обычным и Калманом, всё в одном скетче. Но повторюсь, по сравнению с DMP оно вообще не то показывает =) По поводу скорости: рассмотренный ниже Калман отрабатывает за 3 миллисекунды, но эти 1.5 мс разницы с DMP того не стоят.

#define COMPL_K 0.07

#include "Wire.h"
#include "Kalman.h"

Kalman kalmanX;
Kalman kalmanY;

uint8_t IMUAddress = 0x68;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;

float accXangle; // Angle calculate using the accelerometer
float accYangle;
float temp;
float gyroXangle = 180; // Angle calculate using the gyro
float gyroYangle = 180;
float compAngleX = 180; // Calculate the angle using a Kalman filter
float compAngleY = 180;
float kalAngleX; // Calculate the angle using a Kalman filter
float kalAngleY;

uint32_t timer;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(IMUAddress);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  kalmanX.setAngle(180); // Set starting angle
  kalmanY.setAngle(180);
  timer = micros();
}

void loop() {
  //uint32_t looptime = micros();
  getValues();
  calculateAngles();
  //Serial.println(micros() - looptime);

  Serial.print(kalAngleX); Serial.print(" ");
  Serial.print(kalAngleY); Serial.println();

  /*
     Serial.print(compAngleX); Serial.print(" ");
     Serial.print(compAngleY); Serial.println();
  */
  delay(20); // The accelerometer's maximum samples rate is 1kHz
}

void getValues() {
  /* Update all the values */
  Wire.beginTransmission(IMUAddress);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(IMUAddress, 14, true); // request a total of 14 registers
  accX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  accY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  accZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  tempRaw = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  gyroX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  gyroY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  gyroZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  //temp = ((float)tempRaw + 12412.0) / 340.0;
}

void calculateAngles() {
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  float gyroXrate = (float)gyroX / 131.0;
  float gyroYrate = -((float)gyroY / 131.0);

  // Calculate gyro angle without any filter
  gyroXangle += gyroXrate * ((float)(micros() - timer) / 1000000);
  gyroYangle += gyroYrate * ((float)(micros() - timer) / 1000000);
  /*
    // Calculate gyro angle using the unbiased rate
    gyroXangle += kalmanX.getRate() * ((float)(micros() - timer) / 1000000);
    gyroYangle += kalmanY.getRate() * ((float)(micros() - timer) / 1000000);
  */
  /*
    // Calculate the angle using a Complimentary filter
    compAngleX = ((float)(1 - COMPL_K) * (compAngleX + (gyroXrate * (float)(micros() - timer) / 1000000))) + (COMPL_K * accXangle);
    compAngleY = ((float)(1 - COMPL_K) * (compAngleY + (gyroYrate * (float)(micros() - timer) / 1000000))) + (COMPL_K * accYangle);
  */
  
  // Calculate the angle using a Kalman filter
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (float)(micros() - timer) / 1000000);
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (float)(micros() - timer) / 1000000);

  timer = micros();
}

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
 
 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").
 
 Contact information
 -------------------
 
 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#ifndef _Kalman_h
#define _Kalman_h

class Kalman {
public:
    Kalman() {
        /* We will set the varibles like so, these can also be tuned by the user */
        Q_angle = 0.001;
        Q_bias = 0.003;
        R_measure = 0.03;
        
        bias = 0; // Reset bias
        P[0][0] = 0; // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
        P[0][1] = 0;
        P[1][0] = 0;
        P[1][1] = 0;
    };
    // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    double getAngle(double newAngle, double newRate, double dt) {
        // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
        // Modified by Kristian Lauszus
        // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
                        
        // Discrete Kalman filter time update equations - Time Update ("Predict")
        // Update xhat - Project the state ahead
        /* Step 1 */
        rate = newRate - bias;
        angle += dt * rate;
        
        // Update estimation error covariance - Project the error covariance ahead
        /* Step 2 */
        P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
        P[0][1] -= dt * P[1][1];
        P[1][0] -= dt * P[1][1];
        P[1][1] += Q_bias * dt;
        
        // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
        // Calculate Kalman gain - Compute the Kalman gain
        /* Step 4 */
        S = P[0][0] + R_measure;
        /* Step 5 */
        K[0] = P[0][0] / S;
        K[1] = P[1][0] / S;
        
        // Calculate angle and bias - Update estimate with measurement zk (newAngle)
        /* Step 3 */
        y = newAngle - angle;
        /* Step 6 */
        angle += K[0] * y;
        bias += K[1] * y;
        
        // Calculate estimation error covariance - Update the error covariance
        /* Step 7 */
        P[0][0] -= K[0] * P[0][0];
        P[0][1] -= K[0] * P[0][1];
        P[1][0] -= K[1] * P[0][0];
        P[1][1] -= K[1] * P[0][1];
        
        return angle;
    };
    void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
    double getRate() { return rate; }; // Return the unbiased rate
    
    /* These are used to tune the Kalman filter */
    void setQangle(double newQ_angle) { Q_angle = newQ_angle; };
    void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
    void setRmeasure(double newR_measure) { R_measure = newR_measure; };
    
private:
    /* Kalman filter variables */
    double Q_angle; // Process noise variance for the accelerometer
    double Q_bias; // Process noise variance for the gyro bias
    double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
    
    double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix
    double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix
    double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
    
    double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
    double K[2]; // Kalman gain - This is a 2x1 matrix
    double y; // Angle difference - 1x1 matrix
    double S; // Estimate error - 1x1 matrix
};

#endif

Большую кучу теории про Эйлера и кватернионы можно почитать вот здесь.

НЕКОТОРЫЕ АЛГОРИТМЫ


// скетч определения периода тряски акселерометра

#define DEADZONE 4000
#define MAX_LOOP 900

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;

float k = 0.1;
int16_t ax, ay, az, ax_f;
int16_t gx, gy, gz;
int16_t loopTime;
boolean toggle_flag;
unsigned long maxTimer, analyzeTimer;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  //Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  // Acceleration range: ± 2   ± 4  ± 8  ± 16 g
  // 1G value:           16384 8192 4096 2048
  // MAX G value:        32768
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);

  // Gyroscope range: 250   500  1000 2000 °/s
  // MAX value:       32768
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);

  pinMode(13, OUTPUT);
}

void loop() {
  if (millis() - analyzeTimer > 2) {                   // шаг анализа периода
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);      // запросить данные
    ax_f += (ax - ax_f) * 0.1;                         // отфильтровать

    if (ax_f > DEADZONE && toggle_flag) {
      toggle_flag = false;
      digitalWrite(13, 1);
      loopTime = millis() - maxTimer;   // расчёт периода движения
      maxTimer = millis();
    } else if (ax_f < -DEADZONE && !toggle_flag) { toggle_flag = true; digitalWrite(13, 0); loopTime = millis() - maxTimer; // расчёт периода движения maxTimer = millis(); } if (millis() - maxTimer > MAX_LOOP) loopTime = 0;
    Serial.println(loopTime);
    analyzeTimer = millis();
  }
}

// скетч определения периода вращения акселерометра

#define DEADZONE 700
#define MIN_LOOP 100
#define MAX_LOOP 900

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;

float k = 0.2;
int16_t ax, ay, az, ay_f;
int16_t gx, gy, gz;
int last_acc, level;
boolean rising_flag = true, falling_flag = false, toggle_flag;
unsigned long maxTimer, analyzeTimer;
int loopTime;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  // Acceleration range: ± 2   ± 4  ± 8  ± 16 g
  // 1G value:           16384 8192 4096 2048
  // MAX G value:        32768
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_16);

  // Gyroscope range: 250   500  1000 2000 °/s
  // MAX value:       32768
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
}

void loop() {
  if (millis() - analyzeTimer > 3) {                   // шаг анализа периода
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);      // запросить данные
    ay_f += (ay - ay_f) * k;                           // отфильтровать

    if (rising_flag)   // поднимаемся
      // ждём падения сигнала на величину DEADZONE
      if (last_acc < ay_f + DEADZONE) { // если сигнал не упал, принять величину как точку для следующего сравнения last_acc = ay_f; toggle_flag = true; } else { // если есть падение, и прошёл минимальный период // (защита от всяких случайных ударов и дёрганий) if (toggle_flag && (millis() - maxTimer > MIN_LOOP)) {
          toggle_flag = false;
          rising_flag = false;
          falling_flag = true;
          loopTime = millis() - maxTimer;   // расчёт периода движения
          maxTimer = millis();
          //level = last_acc;
        }
      }

    if (falling_flag) {  // опускается
      // ждём прироста сигнала на величину DEADZONE
      if (last_acc > ay_f - DEADZONE) {  // падаем
        // если сигнал не вырос, принять величину как точку для следующего сравнения
        last_acc = ay_f;
        toggle_flag = true;
      } else {
        // если есть прирост, и прошёл минимальный период
        // (защита от всяких случайных ударов и дёрганий)
        if (toggle_flag && (millis() - maxTimer > MIN_LOOP)) {
          toggle_flag = false;
          rising_flag = true;
          falling_flag = false;
          //level = last_acc;
        }
      }
    }
    /*
        Serial.print("$");
        Serial.print(ay_f);
        Serial.print(" ");
        Serial.print(level);
        Serial.print(" ");
        Serial.print(loopTime*10);
        Serial.println(";");
    */
    if (millis() - maxTimer > MAX_LOOP) loopTime = 0;
    Serial.println(loopTime);
    analyzeTimer = millis();
  }
}

// простая сигнализация
// датчик реагирует даже на постукивание 
// пальцем на противоположном конце стола

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;
long ACC, GYR;
long maxACC, maxGYR;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  mpu.initialize();
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  delay(2000);

  // "калибровка" максимальных значений
  for (int i = 0; i < 30; i++) { mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); ACC = abs(ax) + abs(ay) + abs(az); GYR = abs(gx) + abs(gy) + abs(gz); if (ACC > maxACC) maxACC = ACC;
    if (GYR > maxGYR) maxGYR = GYR;
    delay(5);
  }
  maxACC = maxACC + trACC;
  maxGYR = maxGYR + trGYR;
}

void loop() {
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  // сумма модулей
  ACC = abs(ax) + abs(ay) + abs(az);
  GYR = abs(gx) + abs(gy) + abs(gz);

  if (ACC > maxACC || GYR > maxGYR) {
    Serial.println("KEK!");
  }
  delay(10);
}

Что касается получения линейных скоростей и перемещений при помощи двойного интегрирования ускорения (инерционная навигация), то задача не очень простая, нужны хитрые многоосевые фильтры и желательно процессор помощнее, также об этом можно почитать вот тут.