MPU 9250 и Arduino – схема подключения – блог

MPU 9250 и Arduino – схема подключения - блог Квадрокоптеры

Mpu 9250 и arduino

Сегодня мы хотим познакомить вас с 9-осевым датчиком 3D положения MPU 9250. Этот модуль выбран нами для обзора совсем не случайно, а по веским причинам – из-за функциональности, практичности и миниатюрности. Его можно использовать в различных спортивных диагностирующих приборах, манипуляторах, роботах, 3D-контроллерах и аппаратах автомобильной электроники, устройствах такого плана как квадрокоптеры ∕ дроны, а также подключать к планшетам ∕ смартфонам (например, в качестве компаса или инструмента навигации).

Да и в целом любителям робототехники и проектирования подобный девайс с функцией 3 в 1 (акселерометр, гироскоп, магнитометр) точно не помешает, ведь такие устройства наравне с датчиками движения широко востребованы в среде любителей «самоделок». А раз так – нам необходимо детальнее познакомиться с его техническими характеристиками и схемами подключения к Arduino.
Начнем с первого пункта – параметры:

  • питание: 3.5 – 5 V (имеется внутренний стабилизатор);
  • интерфейс: шина I2C (400кГц), SPI (1 МГц);
  • рабочие диапазоны гироскопа: ± 250 /500 /1000 /2000 ° / сек;
  • — акселерометра: ± 2 /± /4 ±/ 8 ± /16 г;
  • диапазон измерения магнитометра: ± 4800uT;
  • габаритные размеры: 15 x 25 мм.

Кроме того, в приборе продуманы дополнительное выводы – AUX I2C.

Теперь разберемся с главным вопросом – как подключить MPU 9250 к Ардуино. Для этой цели нам понадобятся: плата расширения Arduino Uno, непосредственно IMU датчик, макетная плата, соединительные провода (для подключения к персональному компьютеру – USB кабель). Схема подключения сенсора выглядит следующим образом (через I2C):
MPU 9250 и Arduino – схема подключения - блогДля считывания данных одновременно с трех датчиков предлагаем написать такой скетч:

#include <Wire.h>
#include <TimerOne.h>
 
#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C
 
#define GYRO_FULL_SCALE_250_DPS 0x00 
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18
 
#define ACC_FULL_SCALE_2_G 0x00 
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18
 
// This function read Nbytes bytes from I2C device at address Address. 
// Put read bytes starting at register Register in the Data array. 
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
    {
      // Set register address
      Wire.beginTransmission(Address);
      Wire.write(Register);
      Wire.endTransmission();
 
      // Read Nbytes
      Wire.requestFrom(Address, Nbytes); 
      uint8_t index=0;
      while (Wire.available())
      Data[index  ]=Wire.read();
    }
 
// Write a byte (Data) in device (Address) at register (Register)
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
    {
      // Set register address
      Wire.beginTransmission(Address);
      Wire.write(Register);
      Wire.write(Data);
      Wire.endTransmission();
    }
 
// Initial time
long int ti;
volatile bool intFlag=false;
 
// Initializations
void setup()
    {
      // Arduino initializations
      Wire.begin();
      Serial.begin(115200);
 
      // Set accelerometers low pass filter at 5Hz
      I2CwriteByte(MPU9250_ADDRESS,29,0x06);
      // Set gyroscope low pass filter at 5Hz
      I2CwriteByte(MPU9250_ADDRESS,26,0x06);
 
      // Configure gyroscope range
      I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_1000_DPS);
      // Configure accelerometers range
      I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_4_G);
      // Set by pass mode for the magnetometers
      I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);
 
      // Request continuous magnetometer measurements in 16 bits
      I2CwriteByte(MAG_ADDRESS,0x0A,0x16);
 
      pinMode(13, OUTPUT);
      Timer1.initialize(10000); // initialize timer1, and set a 1/2 second period
      Timer1.attachInterrupt(callback); // attaches callback() as a timer overflow interrupt
 
      // Store initial time
      ti=millis();
    } 
 
// Counter
long int cpt=0;
 
void callback()
    { 
      intFlag=true;
      digitalWrite(13, digitalRead(13) ^ 1);
    }
 
// Main loop, read and display data
void loop()
    {
      while (!intFlag);
      intFlag=false;
 
      // Display time
      Serial.print (millis()-ti,DEC);
      Serial.print ("t");
 
      // _______________
      // ::: Counter :::
 
      // Display data counter
      // Serial.print (cpt  ,DEC);
      // Serial.print ("t");
 
      // ____________________________________
      // ::: accelerometer and gyroscope :::
 
      // Read accelerometer and gyroscope
      uint8_t Buf[14];
      I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
 
      // Create 16 bits values from 8 bits data
 
      // Accelerometer
      int16_t ax=-(Buf[0]<<8 | Buf[1]);
      int16_t ay=-(Buf[2]<<8 | Buf[3]);
      int16_t az=Buf[4]<<8 | Buf[5];
 
      // Gyroscope
      int16_t gx=-(Buf[8]<<8 | Buf[9]);
      int16_t gy=-(Buf[10]<<8 | Buf[11]);
      int16_t gz=Buf[12]<<8 | Buf[13];
 
      // Display values
 
     // Accelerometer
     Serial.print (ax,DEC); 
     Serial.print ("t");
     Serial.print (ay,DEC);
     Serial.print ("t");
     Serial.print (az,DEC); 
     Serial.print ("t");
 
     // Gyroscope
     Serial.print (gx,DEC); 
     Serial.print ("t");
     Serial.print (gy,DEC);
     Serial.print ("t");
     Serial.print (gz,DEC); 
     Serial.print ("t");
 
     // _____________________
     // ::: Magnetometer :::
 
     // Read register Status 1 and wait for the DRDY: Data Ready
 
     uint8_t ST1;
     do
         {
           I2Cread(MAG_ADDRESS,0x02,1,&ST1);
         }
     while (!(ST1&0x01));
 
     // Read magnetometer data 
     uint8_t Mag[7]; 
     I2Cread(MAG_ADDRESS,0x03,7,Mag);
 
     // Create 16 bits values from 8 bits data
 
     // Magnetometer
     int16_t mx=-(Mag[3]<<8 | Mag[2]);
     int16_t my=-(Mag[1]<<8 | Mag[0]);
     int16_t mz=-(Mag[5]<<8 | Mag[4]);
 
     // Magnetometer
     Serial.print (mx 200,DEC); 
     Serial.print ("t");
     Serial.print (my-70,DEC);
     Serial.print ("t");
     Serial.print (mz-700,DEC); 
     Serial.print ("t");
 
     // End of line
     Serial.println("");
     // delay(100); 
   }

В этой прошивке используется специализированное ПО – библиотека Wire. Ее можно отыскать и импортировать в Arduino IDE (менеджер библиотек), а вот вторую библиотеку – TimerOne – придется отыскать и скачать в Интернете (архив распаковываем в директории libraries).

На этом пока всё. Хороших вам проектов!

Mpu 9250 и arduino – схема подключения

Акселерометр сравнивает проекцию ускорения объекта с гравитационным ускорением и способен замерять линейную скорость объекта, а вкупе с гироскопом – и положение в пространстве.

Магнитометр представляет собой устройство для измерения интенсивности ближайшего магнитного поля, действующего на объект (впрочем, название датчика говорит само за себя).

А еще, по некоторым данным, наш модуль MPU 9250 является самым миниатюрным в мире девятиосевым сенсором. Это говорит о высокой производительности микросхемы (по названию которой и назван наш “герой”), что было обеспечено применением технологии CMOS MEMS.

Состоит корпус модуля из двух мельчайших кристаллов, один из которых отвечает за гироскоп и акселерометр, а другой за магнитометр. Данные с них обрабатываются встроенным сигнальным процессором DMP с помощью алгоритмов Motion Fusion и передаются по интерфейсам I2C или SPI.

Смотрите про коптеры:  Кто и зачем использует дроны в бизнесе?

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

Mpu9250 tilt compensated compass and y-axis inclination · issue #102 · kriswiner/mpu9250

Calibration definitely helped for pitch as it seems pretty accurate, but yaw still not reliable. I can put the MPU on a flat surface and rotate it 180 degrees and the values are only changing about 90 degrees.
Unfortunately I cannot plot anything as I’m running this off of an ESP8266 and I don’t have a display other than serial output at the moment.

Here is some output:
MPU9250 9-axis motion sensor…
MPU9250 I AM 71 I should be 71
MPU9250 is online…
x-axis self test: acceleration trim within : 0.3% of factory value
y-axis self test: acceleration trim within : 0.9% of factory value
z-axis self test: acceleration trim within : 1.2% of factory value
x-axis self test: gyration trim within : -2.5% of factory value
y-axis self test: gyration trim within : 0.2% of factory value
z-axis self test: gyration trim within : 0.0% of factory value
Calibrate gyro and accel
accel biases (mg)
-3.54
-9.83
5.55
gyro biases (dps)
-0.73
-0.22
0.82
MPU9250 initialized for active data mode….
AK8963 I AM 48 I should be 48
AK8963 initialized for active data mode….
Mag Calibration: Wave device in a figure eight until done!
Mag Calibration done!
mag biases (mG)
89.90
124.47
140.43

Here is rotation of what should be 90 degrees around zero, but it shows only about 45 degrees either way:
ax = -86.36 ay = 23.62 az = 995.24 mg
gx = 0.07 gy = -0.05 gz = -0.70 deg/s
mx = -17 my = 550 mz = 105 mG
q0 = -1.00 qx = -0.01 qy = -0.04 qz = -0.02
Gyro temperature is 29.1 degrees C
Yaw, Pitch, Roll: 0.95, 4.96, 1.36
rate = 629.44 Hz
ax = -86.18 ay = 23.25 az = 998.35 mg
gx = 0.03 gy = -0.08 gz = -0.74 deg/s
mx = -8 my = 555 mz = 138 mG
q0 = -1.00 qx = -0.01 qy = -0.04 qz = -0.01
Gyro temperature is 29.2 degrees C
Yaw, Pitch, Roll: 0.09, 4.93, 1.34
rate = 633.74 Hz
ax = -86.61 ay = 21.61 az = 997.31 mg
gx = 0.08 gy = -0.02 gz = -0.89 deg/s
mx = -23 my = 552 mz = 149 mG
q0 = -1.00 qx = -0.01 qy = -0.04 qz = -0.02
Gyro temperature is 29.2 degrees C
Yaw, Pitch, Roll: 1.55, 4.96, 1.24
rate = 635.80 Hz
ax = -100.65 ay = 28.87 az = 1000.61 mg
gx = -2.37 gy = -0.98 gz = 26.52 deg/s
mx = -70 my = 548 mz = 48 mG
q0 = -1.00 qx = -0.01 qy = -0.04 qz = -0.07
Gyro temperature is 29.2 degrees C
Yaw, Pitch, Roll: 6.89, 4.83, 1.44
rate = 634.39 Hz
ax = -144.29 ay = 33.81 az = 994.45 mg
gx = -2.75 gy = -0.71 gz = 35.93 deg/s
mx = -122 my = 514 mz = 57 mG
q0 = -0.99 qx = -0.01 qy = -0.04 qz = -0.12
Gyro temperature is 29.2 degrees C
Yaw, Pitch, Roll: 12.20, 4.75, 1.49
rate = 634.14 Hz
ax = -42.30 ay = 5.43 az = 1003.23 mg
gx = -1.75 gy = -1.54 gz = 23.83 deg/s
mx = -187 my = 488 mz = 36 mG
q0 = -0.98 qx = 0.00 qy = -0.04 qz = -0.17
Gyro temperature is 29.3 degrees C
Yaw, Pitch, Roll: 18.79, 4.99, 0.67
rate = 634.69 Hz
ax = -92.04 ay = 19.71 az = 996.89 mg
gx = -0.64 gy = -0.37 gz = 5.81 deg/s
mx = -228 my = 436 mz = 76 mG
q0 = -0.97 qx = 0.00 qy = -0.04 qz = -0.25
Gyro temperature is 29.3 degrees C
Yaw, Pitch, Roll: 28.33, 4.92, 1.12
rate = 640.26 Hz
ax = -108.40 ay = 19.17 az = 991.52 mg
gx = -2.78 gy = -0.11 gz = 26.28 deg/s
mx = -248 my = 369 mz = 60 mG
q0 = -0.95 qx = 0.01 qy = -0.04 qz = -0.30
Gyro temperature is 29.4 degrees C
Yaw, Pitch, Roll: 34.08, 4.75, 0.66
rate = 645.93 Hz
ax = -75.07 ay = 5.68 az = 1004.15 mg
gx = -0.70 gy = -0.27 gz = 6.65 deg/s
mx = -269 my = 294 mz = 98 mG
q0 = -0.93 qx = 0.01 qy = -0.04 qz = -0.37
Gyro temperature is 29.4 degrees C
Yaw, Pitch, Roll: 42.34, 4.47, 0.53
rate = 643.03 Hz
ax = -83.86 ay = 9.03 az = 1001.22 mg
gx = -0.07 gy = -0.17 gz = -0.39 deg/s
mx = -253 my = 277 mz = 100 mG
q0 = -0.93 qx = 0.01 qy = -0.04 qz = -0.36
Gyro temperature is 29.5 degrees C
Yaw, Pitch, Roll: 40.78, 4.70, 0.47
rate = 642.78 Hz
ax = -82.03 ay = 7.08 az = 998.35 mg
gx = 0.04 gy = -0.21 gz = -0.34 deg/s
mx = -269 my = 279 mz = 95 mG
q0 = -0.93 qx = 0.01 qy = -0.04 qz = -0.37
Gyro temperature is 29.5 degrees C
Yaw, Pitch, Roll: 42.41, 4.66, 0.43
rate = 642.95 Hz
ax = -80.69 ay = 8.91 az = 997.19 mg
gx = -0.05 gy = -0.26 gz = -0.18 deg/s
mx = -242 my = 248 mz = 79 mG
q0 = -0.93 qx = 0.01 qy = -0.04 qz = -0.37
Gyro temperature is 29.5 degrees C
Yaw, Pitch, Roll: 42.48, 4.71, 0.46
rate = 642.55 Hz
ax = -81.05 ay = 8.06 az = 1002.99 mg
gx = -0.10 gy = -0.22 gz = -0.27 deg/s
mx = -255 my = 243 mz = 78 mG
q0 = -0.92 qx = 0.01 qy = -0.04 qz = -0.39
Gyro temperature is 29.5 degrees C
Yaw, Pitch, Roll: 44.24, 4.72, 0.35
rate = 645.27 Hz
ax = -78.25 ay = 6.96 az = 997.07 mg
gx = 2.94 gy = 0.37 gz = -30.75 deg/s
mx = -271 my = 310 mz = 79 mG
q0 = -0.95 qx = 0.01 qy = -0.04 qz = -0.32
Gyro temperature is 29.6 degrees C
Yaw, Pitch, Roll: 36.51, 4.73, 0.50
rate = 641.91 Hz
ax = -91.13 ay = 16.91 az = 997.31 mg
gx = 2.53 gy = 0.46 gz = -30.54 deg/s
mx = -222 my = 445 mz = 88 mG
q0 = -0.98 qx = 0.00 qy = -0.04 qz = -0.21
Gyro temperature is 29.6 degrees C
Yaw, Pitch, Roll: 23.57, 4.98, 0.92
rate = 641.77 Hz
ax = -83.37 ay = 22.22 az = 1000.06 mg
gx = 0.91 gy = -0.47 gz = -7.46 deg/s
mx = -91 my = 533 mz = 55 mG
q0 = -1.00 qx = -0.01 qy = -0.04 qz = -0.08
Gyro temperature is 29.6 degrees C
Yaw, Pitch, Roll: 8.57, 5.00, 1.34
rate = 645.76 Hz
ax = -83.50 ay = 24.41 az = 998.54 mg
gx = -0.09 gy = -0.06 gz = -0.94 deg/s
mx = -59 my = 541 mz = 62 mG
q0 = -1.00 qx = -0.01 qy = -0.04 qz = -0.06
Gyro temperature is 29.6 degrees C
Yaw, Pitch, Roll: 5.22, 4.77, 1.40
rate = 640.34 Hz
ax = -90.94 ay = 17.58 az = 995.85 mg
gx = 3.11 gy = 0.01 gz = -25.48 deg/s
mx = 26 my = 544 mz = 83 mG
q0 = -1.00 qx = -0.01 qy = -0.04 qz = 0.02
Gyro temperature is 29.6 degrees C
Yaw, Pitch, Roll: -3.89, 5.13, 1.08
rate = 638.08 Hz
ax = -119.38 ay = 28.26 az = 991.46 mg
gx = 3.87 gy = 3.49 gz = -38.80 deg/s
mx = 167 my = 479 mz = 55 mG
q0 = -0.98 qx = -0.02 qy = -0.05 qz = 0.17
Gyro temperature is 29.7 degrees C
Yaw, Pitch, Roll: -20.34, 6.09, 1.43
rate = 631.06 Hz
ax = -103.21 ay = 23.80 az = 996.22 mg
gx = 0.45 gy = -0.10 gz = -7.36 deg/s
mx = 235 my = 375 mz = 34 mG
q0 = -0.96 qx = -0.02 qy = -0.05 qz = 0.27
Gyro temperature is 29.7 degrees C
Yaw, Pitch, Roll: -32.28, 5.88, 1.26
rate = 630.63 Hz
ax = -104.06 ay = 19.29 az = 1000.55 mg
gx = -0.11 gy = -0.31 gz = -1.44 deg/s
mx = 258 my = 344 mz = 29 mG
q0 = -0.95 qx = -0.02 qy = -0.05 qz = 0.32
Gyro temperature is 29.7 degrees C
Yaw, Pitch, Roll: -38.36, 5.85, 1.03
rate = 629.53 Hz
ax = -102.66 ay = 18.86 az = 994.38 mg
gx = -0.17 gy = -0.18 gz = 0.01 deg/s
mx = 248 my = 326 mz = 43 mG
q0 = -0.94 qx = -0.02 qy = -0.05 qz = 0.32
Gyro temperature is 29.7 degrees C
Yaw, Pitch, Roll: -38.89, 5.90, 1.00
rate = 630.05 Hz
ax = -141.97 ay = 10.38 az = 995.00 mg
gx = 0.55 gy = -0.20 gz = -8.06 deg/s
mx = 255 my = 319 mz = 50 mG
q0 = -0.94 qx = -0.03 qy = -0.05 qz = 0.33
Gyro temperature is 29.7 degrees C
Yaw, Pitch, Roll: -39.50, 6.03, 1.00
rate = 632.66 Hz
ax = -103.45 ay = 16.17 az = 996.83 mg
gx = -0.24 gy = -0.13 gz = 0.11 deg/s
mx = 260 my = 324 mz = 38 mG
q0 = -0.94 qx = -0.03 qy = -0.05 qz = 0.33
Gyro temperature is 29.8 degrees C
Yaw, Pitch, Roll: -39.44, 5.93, 1.01
rate = 633.16 Hz
ax = -100.71 ay = 17.21 az = 998.17 mg
gx = -0.26 gy = -0.17 gz = 0.09 deg/s
mx = 269 my = 315 mz = 32 mG
q0 = -0.94 qx = -0.03 qy = -0.05 qz = 0.34
Gyro temperature is 29.8 degrees C
Yaw, Pitch, Roll: -40.75, 5.89, 0.99
rate = 634.98 Hz
ax = -102.29 ay = 17.03 az = 995.00 mg
gx = -0.26 gy = -0.12 gz = 0.07 deg/s
mx = 273 my = 326 mz = 22 mG
q0 = -0.94 qx = -0.03 qy = -0.05 qz = 0.34
Gyro temperature is 29.8 degrees C
Yaw, Pitch, Roll: -40.46, 5.92, 0.95
rate = 634.85 Hz
ax = -105.22 ay = 16.48 az = 993.04 mg
gx = 0.35 gy = 0.08 gz = -7.43 deg/s
mx = 262 my = 308 mz = 46 mG
q0 = -0.94 qx = -0.03 qy = -0.05 qz = 0.33
Gyro temperature is 29.8 degrees C
Yaw, Pitch, Roll: -40.25, 5.94, 0.97
rate = 633.52 Hz
ax = -104.31 ay = 16.05 az = 994.20 mg
gx = -0.13 gy = -0.19 gz = -0.12 deg/s
mx = 273 my = 283 mz = 74 mG
q0 = -0.93 qx = -0.03 qy = -0.05 qz = 0.36
Gyro temperature is 29.8 degrees C
Yaw, Pitch, Roll: -43.76, 5.98, 0.90
rate = 633.12 Hz

Смотрите про коптеры:  Обзор квадрокоптера PILOTAGE Shadow FPV | Квадрокоптеры и гексакоптеры | Обзоры | Клуб DNS

What orientation should the sensor be in to calibrate? What axis of movement for the figure 8? Could my sensor be bad as calibrations seem wildly different everytime.

Комплектация

  • 1х сенсор MPU-9250;
  • 1х СОединитель типа “ПАПА-ПАПА”;

Комплектующие

Список компонентов для создания компаса на основе Ардуино довольно небольшой:

MPU 9250 на Ali Express стоит примерно $3,5 доллара. Это не самая дешевая IC, но уровень шума был значительно ниже, рекомендуем. Также нужно понимать, что нет ничего особенного в выборе Arduino или основы для нашего устройства. К тому же в нашем случае Arduino является клоном и прекрасно работает. А корпус вы можете сделать вообще на свое усмотрение.

Конструкция

В качестве основного корпуса берем простой кусок древесины и обрезаем ее примерно до 10 см. Затем мы отмечаем два отверстия по длине IC. Важно, чтобы вы правильно установили IC. Если вы сделали что-то не так, пожалуйста, используйте другую сторону или даже лучше, используйте другой кусок дерева.

Общие сведения

9-осевой сенсор MPU-9250 9DOF — сенсор второго поколения компании InvenSense для определения положения в пространстве, включающий в себя 3-осевой Гироскоп, 3-осевой Акселерометр и 3-осевой Компас (Магнетометр).

Питание

Для удобства подключения к Arduino воспользуйтесь Trema Shield, Trema Power Shield, Motor Shield или Trema Set Shield.

Входное напряжение питания 3,3 В или 5 В постоянного тока, подаётся на выводы VCC и GND модуля.

Подключение

Подключить сенсор к микроконтроллеру Вы можете одним из 2 способов:

  • по шине I2C: используя 4 контакта VCC, GNG, SCL, SDA
  • по шине SPI: используя 5 контактов VCC, GND, SCL, SDA, CS, SDO

Соединения

С протоколом I2C схема соединения в нашем компасе будет несложной, делаем соединения следующим образом:

Смотрите про коптеры:  Квадрокоптер GPTOYS F51C Waterproof для полетов над водой.

SDA <–> A4SCl <–> A5VCC <–> 5VGND <–> GND

Убедитесь, что проводные соединения надежны и исправны. Убедитесь, что вы использовали достаточную длину провода.

Характеристики

  • Микросхема : MPU9250
  • Интерфейс: I2C (400кГц) / SPI (1 МГц)
  • Буфер: FIFO 512B
  • Рабочие диапазоны гироскопа: ±250, ±500, ±1000, ±2000 °/с;
  • Рабочие диапазоны акселерометра: ±2, ±4, ±8, ±16 g;
  • Рабочий диапазон магнитометра: ±4800 мкТл;
  • Напряжение питания: 2,4–3,6 В;
  • Рабочий ток: гироскоп – 3,2 мА, акселерометр – 450 мкА, магнитометр – 280 мкА;
  • Размер: 15мм х 25мм

Калибровка компаса и вывод значений осей в монитор порта.

Подключим сенсор через I2C.

При старте происходит калибровка компаса, а далее в монитор порта выводятся значения каждой оси для акселерометра, гироскопа и магнетометра.

#include <Wire.h>
#include <I2Cdev.h>
#include <MPU9250.h>
// По умолчанию адрес устройства на шине I2C - 0x68
MPU9250 accelgyro;
I2Cdev   I2C_M;
uint8_t buffer_m[6];
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t   mx, my, mz;
float heading;
float tiltheading;
float Axyz[3];
float Gxyz[3];
float Mxyz[3];
// время выполнения предварительной калибровки
#define sample_num_mdate  5000
volatile float mx_sample[3];
volatile float my_sample[3];
volatile float mz_sample[3];
static float mx_centre = 0;
static float my_centre = 0;
static float mz_centre = 0;
volatile int mx_max = 0;
volatile int my_max = 0;
volatile int mz_max = 0;
volatile int mx_min = 0;
volatile int my_min = 0;
volatile int mz_min = 0;
void setup()
{
    //подключаемся к шине I2C (I2Cdev не может сделать это самостоятельно)
    Wire.begin();
    // инициализация подключения в Мониторе порта
    // ( 38400бод выбрано потому, что стабильная работа наблюдается и при 8MHz и при 16Mhz, поэтому
    // в дальнейшем выставляйте скорость согласно ваших требований)
    Serial.begin(38400);
    // Инициализация устройства
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();
    // Подтверждение подключения
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
    delay(1000);
    Serial.println("     ");
    // Предварительная калибровка магнитометра
    Mxyz_init_calibrated ();
}
void loop()
{
    getAccel_Data();             // Получение значений Акселерометра
    getGyro_Data();              // Получение значений Гироскопа
    getCompassDate_calibrated(); // В этой функции происходит калибровка магнитометра
    getHeading();                // после чего мы получаем откалиброванные значения углов поворота
    getTiltHeading();            // и наклона
    Serial.println("calibration parameter: ");
    Serial.print(mx_centre);
    Serial.print("         ");
    Serial.print(my_centre);
    Serial.print("         ");
    Serial.println(mz_centre);
    Serial.println("     ");
    Serial.println("Acceleration(g) of X,Y,Z:");
    Serial.print(Axyz[0]);
    Serial.print(",");
    Serial.print(Axyz[1]);
    Serial.print(",");
    Serial.println(Axyz[2]);
    Serial.println("Gyro(degress/s) of X,Y,Z:");
    Serial.print(Gxyz[0]);
    Serial.print(",");
    Serial.print(Gxyz[1]);
    Serial.print(",");
    Serial.println(Gxyz[2]);
    Serial.println("Compass Value of X,Y,Z:");
    Serial.print(Mxyz[0]);
    Serial.print(",");
    Serial.print(Mxyz[1]);
    Serial.print(",");
    Serial.println(Mxyz[2]);
    Serial.println("The clockwise angle between the magnetic north and X-Axis:"); // "Угол поворота"
    Serial.print(heading);
    Serial.println(" ");
    Serial.println("The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:"); // "Угол наклона"
    Serial.println(tiltheading);
    Serial.println("   ");
    Serial.println();
    delay(1000);
}
void getHeading(void)
{
    heading = 180 * atan2(Mxyz[1], Mxyz[0]) / PI;
    if (heading < 0) heading  = 360;
}
void getTiltHeading(void)
{
    float pitch = asin(-Axyz[0]);
    float roll = asin(Axyz[1] / cos(pitch));
    float xh = Mxyz[0] * cos(pitch)   Mxyz[2] * sin(pitch);
    float yh = Mxyz[0] * sin(roll) * sin(pitch)   Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
    float zh = -Mxyz[0] * cos(roll) * sin(pitch)   Mxyz[1] * sin(roll)   Mxyz[2] * cos(roll) * cos(pitch);
    tiltheading = 180 * atan2(yh, xh) / PI;
    if (yh < 0)    tiltheading  = 360;
}
void Mxyz_init_calibrated ()
{
    Serial.println(F("Before using 9DOF,we need to calibrate the compass first. It will takes about 1 minute."));  // Перед использованием сенсора необходимо произвести калибровку компаса. Это займёт около минуты.
    Serial.print("  ");
    Serial.println(F("During  calibrating, you should rotate and turn the 9DOF all the time within 1 minute."));   // На протяжении всего времени калибровки Вам необходимо вращать сенсор во все стороны.
    Serial.print("  ");
    Serial.println(F("If you are ready, please sent a command data 'ready' to start sample and calibrate."));      // Если Вы готовы, для начала калибровки отправьте в Мониторе Порта "ready". 
    while (!Serial.find("ready"));
    Serial.println("  ");
    Serial.println("ready");
    Serial.println("Sample starting......");
    Serial.println("waiting ......");
    get_calibration_Data ();
    Serial.println("     ");
    Serial.println("compass calibration parameter ");
    Serial.print(mx_centre);
    Serial.print("     ");
    Serial.print(my_centre);
    Serial.print("     ");
    Serial.println(mz_centre);
    Serial.println("    ");
}
void get_calibration_Data ()
{
    for (int i = 0; i < sample_num_mdate; i  )
    {
        get_one_sample_date_mxyz();
        /*
        Serial.print(mx_sample[2]);
        Serial.print(" ");
        Serial.print(my_sample[2]);                            // здесь Вы можете увидеть полученные "сырые" значения.
        Serial.print(" ");
        Serial.println(mz_sample[2]);
        */
        if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
        if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; // Поиск максимального значения
        if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];
        if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
        if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; // Поиск минимального значения
        if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];
    }
    mx_max = mx_sample[1];
    my_max = my_sample[1];
    mz_max = mz_sample[1];
    mx_min = mx_sample[0];
    my_min = my_sample[0];
    mz_min = mz_sample[0];
    mx_centre = (mx_max   mx_min) / 2;
    my_centre = (my_max   my_min) / 2;
    mz_centre = (mz_max   mz_min) / 2;
}
void get_one_sample_date_mxyz()
{
    getCompass_Data();
    mx_sample[2] = Mxyz[0];
    my_sample[2] = Mxyz[1];
    mz_sample[2] = Mxyz[2];
}
void getAccel_Data(void)
{
    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Axyz[0] = (double) ax / 16384;
    Axyz[1] = (double) ay / 16384;
    Axyz[2] = (double) az / 16384;
}
void getGyro_Data(void)
{
    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Gxyz[0] = (double) gx * 250 / 32768;
    Gxyz[1] = (double) gy * 250 / 32768;
    Gxyz[2] = (double) gz * 250 / 32768;
}
void getCompass_Data(void)
{
    I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); // активируем магнетометр
    delay(10);
    I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);
    mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
    my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
    mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;
    Mxyz[0] = (double) mx * 1200 / 4096;
    Mxyz[1] = (double) my * 1200 / 4096;
    Mxyz[2] = (double) mz * 1200 / 4096;
}
void getCompassDate_calibrated ()
{
    getCompass_Data();
    Mxyz[0] = Mxyz[0] - mx_centre;
    Mxyz[1] = Mxyz[1] - my_centre;
    Mxyz[2] = Mxyz[2] - mz_centre;
}

Оцените статью
Добавить комментарий

Adblock
detector