Gyroskop a akcelerometr

Tento modul obsahuje integrovaný obvod s funkcí zastupující dvě zařízení, gyroskop a akcelerometr. Gyroskop je zařízení, které se používá hlavně v navigaci a obsahuje setrvačník, který zachovává polohu osy své rotace. Můžeme tedy díky němu poznat, jaká je poloha celého modulu vůči ploše Země. Akcelerometr je pak zařízení, které měří gravitační zrychlení a můžeme s ním tedy měřit náklon či vibrace modulu.

Námi vybraný modul gyroskopu a akcelerometru pak ještě obsahuje DMP (Digital Motion Processor), který nám velice zjednoduší práci s daty, protože v reálném čase sbírá data ze zmíněných dvou zařízení a poskytuje nám přímo tři důležité hodnoty, které udávají rotace kolem jednotlivých os. V angličtině se nazývají yaw (Z osa), pitch (Y osa) a roll (X osa). Modul má v sobě také integrovaný teploměr, ten je však velmi nepřesný a proto se nevyužívá.

Tento modul s gyroskopem a akcelerometrem má celkem 8 propojovacích pinů, nám však bude stačit zapojit pouze 5. Propojíme tedy VCC na napájecí napětí +5V, GND na zem, SCL na A5, SDA na A4 a INT na D2. Piny SCL a SDA jsou připojeny na I2C sběrnici a pin INT slouží pro detekci přerušení od DMP.

První ukázkový kód pro zapojení a vyzkoušení gyroskopu a akcelerometru obsahuje jednoduchou komunikaci přes knihovnu Wire, která je již od instalace přítomna v Arduino IDE. Na začátku programu tedy vytvoříme potřebné proměnné a v podprogramu setup nastavíme nejprve modul přes I2C sběrnici a také komunikaci po sériové lince. V nekonečné smyčce loop pak vždy aktivujeme I2C sběrnici, na ní pošleme žádost o data od modulu a poté vyčteme data ze všech tří os akcelerometru a gyroskopu. Tato naměřená data se poté vytisknou po sériové lince včetně informace o teplotě.

// Arduino gyroskop a akcelerometr 1

// připojení knihovny Wire
#include <Wire.h>
// inicializace proměnné pro určení adresy senzoru
// 0x68 nebo 0x69, dle připojení AD0
const int MPU_addr=0x68;
// inicializace proměnných, do kterých se uloží data
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

void setup()
{
  // komunikace přes I2C sběrnici
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  // komunikace přes sériovou linku rychlostí 115200 baud
  Serial.begin(115200);
}

void loop()
{
  // zapnutí přenosu
  Wire.beginTransmission(MPU_addr);
  // zápis do registru ACCEL_XOUT_H
  Wire.write(0x3B);
  Wire.endTransmission(false);
  // vyzvednutí dat z 14 registrů
  Wire.requestFrom(MPU_addr,14,true);
  AcX=Wire.read()<<8|Wire.read();    
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();
  Tmp=Wire.read()<<8|Wire.read();
  GyX=Wire.read()<<8|Wire.read();
  GyY=Wire.read()<<8|Wire.read();
  GyZ=Wire.read()<<8|Wire.read();
  // výpis surových dat z proměnných na sériovou linku
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  // přepočtení teploty dle datasheetu
  Serial.print(" | Temp = "); Serial.print(Tmp/340.00+36.53);
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(500);
}

Data, která dostaneme s prvním ukázkovým kódem, však nejsou moc čitelná a slouží hlavně pro první otestování modulu. Pro získání lepších dat je pak nutné využít knihovny MPU-6050, která je ke stažení zde a knihovny I2Cdev, která je ke stažení zde. Po stažení a rozbalení balíčků s knihovnami zkopírujeme obě složky do naší složky libraries v místě, kde si ukládáme veškeré programy (standardně v Dokumentech). Po novém spuštění Arduino IDE uvidíme mezi příklady knihovnu MPU-6050. Druhý ukázkový kód je založen právě na příkladu od zmíněné knihovny a jeho výstupem jsou již data o rotacích jednotlivých os získáné z DMP čipu.

V druhém ukázkovém programu jsou tedy opět nejprve připojeny všechny důležité knihovny a poté vytvořeny všechny potřebné proměnné. Následně je vytvořen podprogram dmpINT, který obsahuje nastavení informací o detekci přerušení. Poté následuje podprogram setup s nastavením všech potřebných pinů a sběrnic, včetně sériové linky a také kontroly připojeného DSP. V nekonečné smyčce loop pak vždy detekujeme přerušení od DMP čipu a v případě jeho nečinnosti můžeme provádět libovolné operace uvnitř while smyčky (označeno komentářem). Při zachycení přerušení vyčteme informace o rotacích kolem všech os a tyto informace následně vytiskneme do sériové linky.

Níže si můžete prohlédnout nejprve zdrojový kód, poté obrázek objasňující zmíněné rotace kolem os a nakonec ukázkový výpis z tohoto ukázkového programu.

// Arduino gyroskop a akcelerometr 2

// knihovny potřebné pro modul
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

// nastavení adresy modulu
// 0x68 nebo 0x69, dle připojení AD0
// MPU6050 mpu(0x69)
MPU6050 mpu;
// číslo pinu s LED diodou pro notifikaci
#define LED_PIN 13 

// inicializace proměnných, do kterých se uloží data
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

// inicializace proměnných pro výpočet
Quaternion q;           // [w, x, y, z] kvaternion
VectorFloat gravity;    // [x, y, z] vektor setrvačnosti
float rotace[3];        // rotace kolem os x,y,z

// Rutina přerušení
volatile bool mpuInterrupt = false;
void dmpINT() {
  mpuInterrupt = true;
}

void setup() {
  // nastavení LED jako výstupní
  pinMode(LED_PIN, OUTPUT);
  // nastavení I2C sběrnice
  Wire.begin();
  // komunikace přes sériovou linku rychlostí 115200 baud
  Serial.begin(115200);
  while (!Serial);
  // inicializace akcelerometru a gyroskopu
  Serial.println(F("Inicializace I2C zarizeni.."));
  mpu.initialize();
  Serial.println(F("Test pripojenych zarizeni.."));
  Serial.println(mpu.testConnection() ? F("Modul pripojeni") : F("Pripojeni modulu selhalo"));
  // incializace DMP
  Serial.println(F("Inicializace DMP..."));
  devStatus = mpu.dmpInitialize();
  // kontrola funkčnosti DMP
  if (devStatus == 0) {
      // zapnutí DMP
      Serial.println(F("Povoleni DMP..."));
      mpu.setDMPEnabled(true);
      // nastavení pinu INT jako přerušovacího, interrupt 0 odpovídá pinu 2
      attachInterrupt(0, dmpINT, RISING);
      mpuIntStatus = mpu.getIntStatus();
      Serial.println(F("DMP pripraveno, cekam na prvni preruseni.."));
      dmpReady = true;
      // načtení velikosti zpráv, které bude DMP posílat
      packetSize = mpu.dmpGetFIFOPacketSize();
  }
  else {
      // V případě chyby:
      // 1 : selhání připojení k DMP
      // 2 : selhání při nastavení DMP
      Serial.print(F("DMP inicializace selhala (kod "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }
  digitalWrite(LED_PIN, LOW);
}

void loop() {
// dokud nepošle DMP přerušení, můžeme provádět ostatní příkazy
// ve smyčce while níže
if (!dmpReady) return;
// tato podmínka čeká na příjem přerušení a můžeme v ní provádět
// ostatní operace
while (!mpuInterrupt && fifoCount < packetSize) {
    // místo pro ostatní operace
    // ..
}

// získání informace o statusu DSP
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// získání velikosti zásobníku dat
fifoCount = mpu.getFIFOCount();
// kontrola přetečení zásobníku dat
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    mpu.resetFIFO();
    Serial.println(F("Preteceni zasobniku dat!"));
    // v případě přetečení zásobníku je nutné
    // častěji vyčítat data
}
else if (mpuIntStatus & 0x02) {
    // kontrola délky dat
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    // čtení paketu ze zásobníku
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    // při větším množství paketů snížíme počítadlo
    fifoCount -= packetSize;
    // přečtení dat z DSP a uložení do proměnných
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(rotace, &q, &gravity);

    // výpis informací o rotacích kolem jednotlivých os
    Serial.print("Rotace \t X ");
    Serial.print(rotace[2] * 180/M_PI);
    Serial.print("st \t Y ");
    Serial.print(rotace[1] * 180/M_PI);
    Serial.print("st \t Z ");
    Serial.print(rotace[0] * 180/M_PI);
    Serial.println("st");
}
}

Obrázek s názorným příkladem rotací kolem os x, y a z včetně anglických názvů.

Ukázkový výpis ze Sériového monitoru:

Inicializace I2C zarizeni..
Test pripojenych zarizeni..
Modul pripojeni
Inicializace DMP...
Povoleni DMP...
DMP pripraveno, cekam na prvni preruseni..
Rotace 	 X -2.15st 	 Y -2.90st 	 Z -0.03st
Rotace 	 X -2.06st 	 Y -2.90st 	 Z -0.03st
..

Tento modul obsahující gyroskop, akcelerometr a DMP čip se využívá často u kvadrokoptér, helikoptér a podobných zařízení, kdy potřebujeme kontrolovat právě rotace samotných zařízení či různé vibrace vyskytující se v okolí. Rád bych upozornil na to, že po každém nahrání programu či zapnutí zařízení s tímto senzorem je nutné vyčkat okolo 15 až 20 sekund kvůli kalibraci DMP čipu, sami uvidíte, že v prvních sekundách jsou výstupní hodnoty velice nestálé.

Poznámka: na rozdíl od většiny článků uvedených na tomto webu byla použita rychlost sériové linky větší než 9600 a to konkrétně 115200 baud. Tato změna byla nutná kvůli rychlejšímu vypisování informací ze zásobníku dat a je tedy nutné v Sériovém monitoru také změnit rychlost v pravém spodním rohu, jinak budete dostávat falešné znaky.

Seznam použitých komponent:

http://dratek.cz/arduino-vstupni-periferie/830-arduino-gyroskop-akcelerometr.html

http://dratek.cz/arduino/974-arduino-uno-r3-atmega328p-1424115860.html

http://dratek.cz/arduino/844-arduino-nepajive-pole.html

http://dratek.cz/arduino-kabelaz-propoje-rozsireni/1063-arduino-vodice-samec-samec-40-kusu.html