Запис #24 "Калібрування MPU-6050 (GY-521)"

Додано: 2016-02-15 00:42:42 'admin

Після першого тестового випробування стала необхідність в калібровці MPU-6050 (фотозвіт по роботі описаного нижче коду знаходиться в кінці статті). Причиною цього стало те, що під виставленим нулем із "голих" даних модуль знаходився не в положенні реального нуля і це було навіть помітно на око. Під час пошуків у мережі потрапив на бібліотеку із кодом для MPU-6050, написану для Arduino. Ця бібліотека знаходиться на гітхабі (https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050) і доступна всім для скачування. Автору бібліотеки окреме велике дякую за роботу, яку він провів. На форумі офіційної сторінки автора був знайдений скетч під Arduino для калібрування MPU-6050 (http://www.i2cdevlib.com/forums/topic/96-arduino-sketch-to-automatically-calculate-mpu6050-offsets). В загальному після підключення бібліотеки та компіляції коду я отримав калібрування і акселерометра і гіроскопа без яких-небудь додаткових рухів. На сайті також можна знайти багато цікавого по цій мікросхемі http://www.i2cdevlib.com/devices/mpu6050. Включаючи опис деяких регістрів, яких немає в офіційній документації. 

Код скетча для калібрування MPU-6050:

// I2Cdev і MPU6050 бібліотеки повинні бути встановленні
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

/////////////////////////////////// КОНФІГУРАЦІЯ /////////////////////////////
int buffersize=1000; //Кількість зчитувань показників датчиків для виведення середнього значення (default:1000)
int acel_deadzone=8; //Доступна похибка акселерометра (default:8)
int giro_deadzone=1; //Доступна похибка гіроскопа (default:1)

// Адреса пристрою
// AD0 low = 0x68 (по-замовчуванню)
// AD0 high = 0x69
MPU6050 accelgyro(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;

/////////////////////////////////// УСТАНОВКИ ////////////////////////////////////
void setup() {
// ініціалізація I2C шини
Wire.begin();
// Корекція частоти шини. Закоментувати для ARDUINO DUE
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.

// ініціалізація послідовного порта
Serial.begin(115200);

// ініціалізація MPU-6050.
// В загальному ця функція виконує 4 дії.
accelgyro.initialize();

// Стартове привітання
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);
// Перевірка з"єднання із MPU-6050
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
delay(1000);
// Скидання регістрів похибок.
// Після проходження калібрування сюди заносяться дані, що видала програма та перевіряється допуск відхилення
// Допуск повинен бути не більше, ніж значення acel_deadzone та giro_deadzone. Інакше проводиться повторне калібрування.
accelgyro.setXAccelOffset(0);
accelgyro.setYAccelOffset(0);
accelgyro.setZAccelOffset(0);
accelgyro.setXGyroOffset(0);
accelgyro.setYGyroOffset(0);
accelgyro.setZGyroOffset(0);
}

/////////////////////////////////// ЦИКЛ ////////////////////////////////////
void loop() {
// Перший крок. Зчитування первинних показників датчиків.
if (state==0){
Serial.println("\nReading sensors for first time...");
meansensors();
state++;
delay(1000);
}

// Другий крок. Калібрування. За початкові дані беруться показники із першого кроку.
// Калібрування відбуваєтся до тих пір, поки похибки всіх осей датчиків не будуть менше ніж значення acel_deadzone та giro_deadzone
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("\t");
Serial.print(ay_offset);
Serial.print("\t");
Serial.print(az_offset);
Serial.print("\t");
Serial.print(gx_offset);
Serial.print("\t");
Serial.print(gy_offset);
Serial.print("\t");
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);
}
}

/////////////////////////////////// ФУНКЦІЇ////////////////////////////////////

// Функція отримує середні дані по всім осям. Кількість ітерацій залежить від buffersize
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
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

if (i>100 && i<=(buffersize+100)){ //Перших 100 зчитувань ігнорується
// Сумування вимірів
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;
// Підставка поточних похибок
accelgyro.setXAccelOffset(ax_offset);
accelgyro.setYAccelOffset(ay_offset);
accelgyro.setZAccelOffset(az_offset);

accelgyro.setXGyroOffset(gx_offset);
accelgyro.setYGyroOffset(gy_offset);
accelgyro.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)<=giro_deadzone) ready++;
else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

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

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

if (ready==6) break; //Якщо всі похибки в допусках - вихід із циклу калібрування.
}
}

Код використовує бібліотеку I2Cdev.h для доступу до функцій передачі та отримання данних по шині I2C та MPU6050.h для створення об"єкту із функціоналом управління MPU-6050.

На початку об"являються змінні: buffersize - змінна, що містить кількість ітерацій для отримання середніх значень осей датчиків. acel_deadzone - значення допуска для акселерометра. Максимальне значення похибки по модулю не може перевищувати значення acel_deadzone. Наприклад, при acel_deadzone=8 і при постійному спокою датчика похибка для вісі Х може бути від -8 до 8. giro_deadzone - значення допуска для гіроскопа.

Далі йде отримання об"єкта класу MPU6050. Це дозволяє отримати доступ до потужного функціонала управління MPU6050.

Об"являються глобальні змінні:
- ax, ay, az, gx, gy, gz - змінні довжиною у 2 байта для збереження поточних даних датчиків.
- mean_ax, mean_ay, mean_az, mean_gx ,mean_gy, mean_gz - змінні що містять середні значення датчиків.
- state=0 - зміна кроків калібрування
- ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset - змінні, що містять значення похибок.

Після об"явлення змінних відбувається ініціалізація шини I2C, послідовного порта (для виводу інформації на монітор. Проходить перевірка підключення із MPU-6050. Встановлюються похибки, якщо вже є їх попередні результати (методи setXAccelOffset, setYAccelOffset, setZAccelOffset, setXGyroOffset, setYGyroOffset, setZGyroOffset). 

На методах занесення похибок варто зупинитись та розібратись у їх роботі. Ці методи, якщо переглянути код із їх об"явленнями, можна помітити, що ці методи в якості адрес регістрів використовують константи MPU6050_RA_XA_OFFS_H, MPU6050_RA_YA_OFFS_H, MPU6050_RA_ZA_OFFS_H, MPU6050_RA_XG_OFFS_USRH, MPU6050_RA_YG_OFFS_USRH, MPU6050_RA_ZG_OFFS_USRH. Які в свою чергу вказують на адреси регістрів (із файлу MPU6050.h):

#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU6050_RA_XA_OFFS_L_TC 0x07
#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU6050_RA_YA_OFFS_L_TC 0x09
#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU6050_RA_ZA_OFFS_L_TC 0x0B

Із цього видно, що використані регістри, які не описані в офіційній документації. Це регістри похибок, які використовує математика самого MPU-6050. Перевірено. Тому під час функції калібрування іде запис у ці регістри та зчитування данних із регістрів осей. І на основі результатів приймається рішення про наступний цикл калібрування.

У циклі основної програми є поділ на три кроки:
1. Отримання середніх значень осей датчиків без застосування калібровки.
2. Із отриманих даних формуються дані для першого циклу калібрування. Процес калібрування полягає у тому, щоб досягнути сначень похибок, при яких всі показники будуть у межах заданими змінними acel_deadzone та giro_deadzone. Як тільки всі 6 похибок будуть у допусках - відбувається перехід на третій крок.
3. Вивід інформації по результатам калібрування.

Це перший спосіб, який мені зустрівся в мережі. Хоча, були і інші, що використовували ресурс процесора, а не MPU-6050. Але зацікавав даний спосіб через зменшення навантаження на процесор та застосування регістрів MPU-6050, які не були описані раніше.

Фотозвіт про роботу програми:

Для інформативності роботи коду я його дещо модифікував. Додав вивід поточної інформації на монітор. 

Вивід даних осей датчиків до калібрування:

MPU6050_begin_calibration

Видно, що отриманні допуски значно відрізняються від заданих. Всі ці дані повинні бути в межах 0±8 для акселерометра і 0±1 для гіроскопа, крім ACCEL_ZOUT. Тут значення повинно бути у межах 16384±8. 

На ЛСД при застосуванні DMP маю такі показники:

MPU6050_LCD_before_calibration

Ну явно, що по вісі Y є похибка. По вісі Х похибка менша, але присутня.

Після виконання калібрування я отримав такі дані похибок для свого датчика:

MPU6050_end_calibration

Похибки для осей знаходяться в строці "Your offsets". Копіюю і вставляю у методи похибок замість нулів. Отримую результат:

MPU6050_after_calibration

Тепер значення у межах допуску.

Перевірка на DMP:

MPU6050_LCD_after_calibration

Значення стали більш точніші.

Коментарі: