1. Підключення MPU6050 до Wemos d1
2. Фільтр Калмана MPU6050

Для того щоб наше пристрій могло відображати і записувати в лог прискорення і перевантаження під час розгону, ми підключимо до нього популярний високоточний модуль GY-521 , На основі чіпа MPU-6050 , Що є трьох-осьовим гіроскопом і трьох-осьовим акселерометром. Гіроскоп нам поки не знадобиться, тому в цій частині я буду розповідати тільки про акселерометр.

Основні характеристики MPU6050:

• 16-бітний АЦП
• Напруга харчування 3-5в
• Підтримка протоколу I2C
• Діапазон прискорень: ± 2 ± 4 ± 8 ± 16g
• Діапазон «Гіро»: ± 250 500 1000 2000 ° / s

Підключення MPU6050 до Wemos d1

Модуль працює по I2C, тому підключити його вельми просто, з'єднаємо акселерометр з платою Wemos D1 : VCC -> 3V3, GND -> GND, SCL -> D1, SDA -> D2.

Для перевірки давайте заллємо в плату наступний скетч:

#include <Wire.h> const int MPU_addr = 0x68; // I2C address of the MPU-6050 int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ; void setup () {Serial.begin (115200); 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); } Void loop () {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 AcX = Wire.read () << 8 | Wire.read (); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) AcY = Wire.read () << 8 | Wire.read (); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) AcZ = Wire.read () << 8 | Wire.read (); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) Tmp = Wire.read () << 8 | Wire.read (); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) GyX = Wire.read () << 8 | Wire.read (); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) GyY = Wire.read () << 8 | Wire.read (); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) GyZ = Wire.read () << 8 | Wire.read (); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) Serial.print ( "AcX ="); Serial.print (AcX); Serial.print ( "| AcY ="); Serial.print (AcY); Serial.print ( "| AcZ ="); Serial.print (AcZ); Serial.print ( "| Tmp ="); Serial.print (Tmp / 340.00 + 36.53); // temperature in degrees C from datasheet Serial.print ( "| GyX ="); Serial.print (GyX); Serial.print ( "| GyY ="); Serial.print (GyY); Serial.print ( "| GyZ ="); Serial.println (GyZ); delay (500); }

Завантажити архів з скетчем: MPU6050.zip

Тепер, якщо ви відкриєте термінал com порту, то побачите приблизно наступну картину: прискорення по трьох осях, температура, кути положення по трьох осях.

з даташіта , Ми знаємо, що при налаштуванні чутливості акселерометра до 2g на кожну одиницю g доводиться 16384 одиниць вимірювання датчика. Самі наглядові напевно вже помітили по фото, що лежачи на столі датчик чомусь показує прискорення по горизонтальних осях, а по вертикальній показує значення більше 16384 (я сподіваюся все пам'ятають зі школи, що на всі об'єкти на землі діє сила тяжіння в 1g). Це проблема всіх подібних акселерометрів - вони дуже шумлять. Їх обов'язково потрібно калібрувати і фільтрувати одержувані дані.

Фільтр Калмана MPU6050

Давайте додамо в скетч спрощений фільтр Калмана, для того щоб згладити шуми датчика.

#include <Wire.h> const int MPU_addr = 0x68; // I2C address of the MPU-6050 int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ; // змінні для Калмана float varVolt = 78.9; // середнє відхилення (шукаємо в excel) float varProcess = 0.5; // швидкість реакції на зміну (підбирається вручну) float Pc = 0.0, G = 0.0, P = 1.0, Xp = 0.0, Zp = 0.0, Xe = 0,0; // змінні для Калмана // Функція фільтрації float filter (float val) {Pc = P + varProcess; G = Pc / (Pc + varVolt); P = (1 - G) * Pc; Xp = Xe; Zp = Xp; Xe = G * (val - Zp) + Xp; // "фільтроване" значення return (Xe); } Void setup () {Serial.begin (115200); 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); } Void loop () {Wire.beginTransmission (MPU_addr); Wire.write (0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission (false); Wire.requestFrom (MPU_addr, 6, true); // request a total of 14 registers AcX = Wire.read () << 8 | Wire.read (); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) AcY = Wire.read () << 8 | Wire.read (); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) AcZ = Wire.read () << 8 | Wire.read (); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) Serial.print (AcZ); Serial.print ( ""); Serial.println ((int) filter (AcZ)); delay (100); }

Завантажити архів з скетчем: MPU6050Kalman.zip

Для фільтра нам необхідно підібрати два значення:

1) varVolt - це середнє відхилення, його можна або вирахувати самостійно або забити значення в файл excel який я поклав в архів зі скетчем

2) varProcess - це час реакції, чим менше це число, тим плавніше будуть змінюватися значення.

Спробуйте погратися з цими параметрами.

Тепер зайдемо в меню «Інструменти - Плоттер по послідовному з'єднанню», відкриється ось таке вікно:

Синя лінія - це не Фільтровані дані, а червона це після обробки фільтром Калмана. Піки це - я піднімав і опускав акселерометр приблизно на пів метра над столом. З такими даними вже можна буде комфортно працювати.

На цьому поки все, в одній з наступних частин ми з вами зберемо повноцінний G-meter.