Než se pustíme k tomu zajímavému, z minulé diskuse mohlo vyplynout, že jsem dostatečně nezdůraznil, že stavba kvadrokoptéry a vývoj softwaru pro ni mohou být nebezpečné. Doufal jsem, že projevy motoru s vrtulí jsou dostatečně výrazné na to, aby do něj lidé nestrkali ruce. Pro jistotu tedy napíši, že to není dobrý nápad a že následování postupů v tomto seriálu rozhodně není bez zapojení vlastní obezřetnosti bezpečné. Autor ani Root.cz nenese zodpovědnost za to, co se vám během stavby vaší kvadrokoptéry stane, ať už se rozhodnete jít vlastní cestou nebo se inspirujete těmito řádky. Během pokusů dbejte na to, aby byla kvadrokoptéra dobře uchycena a neuletěla nějakým nečekaným směrem.

V seznamu v prvním článku můžete narazit na gyro MPU-6050. Jde o levný integrovaný obvod s podporou I2C/SPI sběrnice, tříosým gyroskopem, třiosým akcelerometrem a teploměrem. Kromě toho má na sobě ještě Digital Motion Processor™ (DMP), který umožňuje provádět výpočty s hodnotami z gyroskopu, akcelerometru a také z externě připojeného magnetometru. Integrovaný teploměr je spíše pro hrubé měření, protože více než teplotu okolí vrací teplotu čipu. Takže když máte v místnosti 18 °C, ochotně vrátí hodnotu 25 °C.

Klíčový senzor pro naši kvadrokoptéru je akcelerometr. Ten měří gravitační zrychlení, takže ho můžeme využít k měření:

• Náklonu
• Rychlosti
• Otřesů

Zajímá nás především náklon, ale ať už chceme jednu, druhou nebo třetí hodnotu, ostatní dvě nám budou do získaných čísel zasahovat. Pro přesnější měření proto kombinujeme několik senzorů najednou. MPU-6050 obsahuje kromě akcelerometru ještě gyroskop. Stejně jako akcelerometr, i gyroskop umí měřit ve třech osách a se správným nastavením jsme schopni získat údaje o tom, o kolik se od posledního měření změnil úhel desky se senzorem.

MPU-6050 je zařízení mnoha možností a díky štědré komunitě, jmenovitě Jeffu Rowbergovi, nemusíme znát všechny jeho záludnosti, ani tu matematiku, která se za získáváním klíčových hodnot schovává, abychom dostali to, co naše kvadrokoptéra potřebuje.

Zapojení

Ale nebudu předbíhat. Pokud máte na stole Arduino Nano a MPU-6050 jako já, není nic jednoduššího, než vzít 4 vodiče a připojit s nimi GND gyra na GND Arduina, napájení gyra dát na 5V pin Arduina a piny SDA a SCL na gyru připojit na piny A4 a A5 na Arduinu. Lepší přehled vám dá tato tabulka:

V tabulce ještě pin INT, který připojíme na D2 na Arduinu. Jedná se o externí přerušení, o kterém bude řeč níže.

Toto gyro je schopné pracovat i na 3,3 V, takže ho můžete bez problémů připojit také k Raspberry Pi, například podle návodu na astromik.org.

Získávání surových dat

S MPU-6050 lze komunikovat dvěma způsoby. Ten jednodušší je získávání surových dat. Ten komplikovanější je povídání si s integrovaným DMP, který dokáže přepočítat ledasco sám, aniž by bylo třeba s tím zatěžovat Arduino. Naštěstí pro oba způsoby existuje jedna knihovna, která je součástí balíku i2cdevlib. To je komunitní projekt, v rámci kterého různí vývojáři dávají dohromady podporu pro různá I2C zařízení. Nás zajímá hlavně naše MPU-6050. Zdrojový kód i příklady jsou dobře komentované a komunikace s gyrem pomocí této knihovny je velmi snadná. Knihovnu stačí stáhnout a nakopírovat k Arduino IDE do adresáře „libraries“.

V obou následujících ukázkách použiji mírně upravené příklady, které jsou součástí knihovny, jen je doplním o české komentáře a případně o nějaké důležité detaily. Začneme získáváním čistých dat.

# Knihovna I2Cdev využívá knihovnu Wire
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

// Výchozí I2C adres je 0x68, ale pokud máte jinou, zde ji můžete uvést
// AD0 low = 0x68
// AD0 high = 0x69
MPU6050 accelgyro;

// získané hodnoty
int16_t ax, ay, az;
int16_t gx, gy, gz;

#define LED_PIN 13
bool blinkState = false;

void setup() {
// připojení k I2C sběrnici, I2Cdev to sama nedělá
Wire.begin();

// Inicializace UART komunikace
Serial.begin(38400);

// Inicializace gyra
Serial.println("Initializing I2C devices...");
accelgyro.initialize();

// Test komunikace s gyrem
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

// Nastavíme diodu na výstupu D13
pinMode(LED_PIN, OUTPUT);
}

void loop() {
// Přečtení hodnot z gyra a akcelerometru
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

// Ke stejnému účelu můžete použít tyto dvě metody
//accelgyro.getAcceleration(&ax, &ay, &az);
//accelgyro.getRotation(&gx, &gy, &gz);

// Zobrazení jednotlivých hodnot (oddělené taby)
Serial.print("a/g:\t");
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);

// Zablikáme diodou
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
} 

Jak vidíte, použití s MPU-6050 je s touto knihovnou hračka. Výchozí nastavení senzorů nemusí vyhovovat pro daný účel a proto je ještě možné měnit citlivost jak gyroskopu, tak akcelerometru. Citlivost gyroskopu můžeme nastavit pomocí:

accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250); 

Kde možnými hodnotami jsou:

MPU6050_GYRO_FS_250   pro  ±250°/sec
MPU6050_GYRO_FS_500   pro  ±500°/sec
MPU6050_GYRO_FS_1000  pro ±1000°/sec
MPU6050_GYRO_FS_2000  pro ±2000°/sec 

Jednotka °/sec je úhlové zrychlení a čím větší číslo se před ní nachází, tím rychlejší otáčení je schopen gyroskop měřit, ale za cenu snížené přesnosti u otáček menších. U akcelerometru můžeme použít:

accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); 

Kde možnými hodnotami jsou:

MPU6050_ACCEL_FS_2   pro  ±2g
MPU6050_ACCEL_FS_4   pro  ±4g
MPU6050_ACCEL_FS_8   pro  ±8g
MPU6050_ACCEL_FS_16  pro  ±16g 

Čím větší hodnotu nastavíme, tím menší vliv bude mít změna náklonu na měření akcelerometru.

Využití Digital Motion Processoru

To nejzajímavější, co se na MPU-6050 nachází, je Digital Motion Processor. Ten dokáže provádět výpočty s hodnotami ze senzorů, které tak nezatěžují naše pomalé Arduino. Údaje z akcelerometru a gyroskopu nelze použít přímo pro regulaci otáček motorů. I když například informace z akcelerometru mohou vytvářet zdání, že jde o náklon desky se senzorem, stačí o ni trochu zavadit a do údajů se promítne setrvačnost. Za tímhle vším se schovává složitá matematika, o kterou jsem nezavadil ani na vysoké škole, takže jsem byl vcelku rád, že výše zmíněná knihovna i2cdevlib si poradí jak s DMP, tak s výpočty, které potřebujeme pro stabilizaci. Zavoláním několika metod nám z DMP začnou padat úhly, ve kterých se senzory nachází a jediné, co musíme udělat, je, že je včas přečteme.

Z předchozího příkladu byste měli mít správně spojené Arduino s MPU-6050. Pro následující příklad musíte mít propojený pin INT gyra s pinem A2 na Arduinu. MPU-6050 přes něj říká Arduinu, že má k dispozici nové výsledky výpočtů a že si je máme vyzvednout. Samotný kód vypadá takto:

// Knihovna I2Cdev využívá knihovnu Wire
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

// Výchozí I2C adresa MPU-6050 je 0x68, ale pokud máte jinou, zde ji můžete uvést
// AD0 low = 0x68
// AD0 high = 0x69
MPU6050 mpu;

#define LED_PIN 13 // číslo pinu s integrovanou LED diodou

bool dmpReady = false;  // když je DPM připraveno, obsahuje true
uint8_t mpuIntStatus;   // stav externího přerušení z DPM
uint8_t devStatus;      // stav poslední operace
uint16_t packetSize;    // velikost paketu z DPM
uint16_t fifoCount;     // počet bytů ve FIFO zásobníku
uint8_t fifoBuffer[64]; // FIFO zásobník

// proměnné důležité pro výpočet
Quaternion q;           // [w, x, y, z]         kvaternion
VectorFloat gravity;    // [x, y, z]            vektor setrvačnosti
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll úhly

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // true pokud DMP vyvolalo přerušení
void dmpDataReady() {
mpuInterrupt = true;
}

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
pinMode(LED_PIN, OUTPUT);

// připojíme se na I2C sběrnici
Wire.begin();

// inicializujeme UART
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately

// inicializujeme MPU-6050
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();

// ověříme připojení k MPU-6050
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

// incializujeme DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();

// ujistíme se, že funguje
if (devStatus == 0) {
    // zapneme DMP
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // externí přerušení Arduina nabindujeme na funkci dmpDataReady
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // zjistíme si, jak velké pakety DMP vrací
    packetSize = mpu.dmpGetFIFOPacketSize();
} else {
    // Když dojde k chybě, tak:
    // 1 = selhala úvodní komunikace s DMP
    // 2 = selhala aktualizace nastavení DMP
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
}

digitalWrite(LED_PIN, LOW);
}

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
// pokud není DMP připravené, nebudeme dělat nic
if (!dmpReady) return;

// zde provádíme náš kód, cyklus ověřuje, zda nemá DMP připravena nějaká data
while (!mpuInterrupt && fifoCount < packetSize) {
    //
}

// resetujeme proměnnou informující o přerušení vyvolané z DMP a získáme INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// získáme velikost FIFO zásobníku
fifoCount = mpu.getFIFOCount();

// zjistíme, zda nedošlo k přetečené zásobníku
// pokud k němu dojde, je třeba optimalizovat kód v cyklu výše,
// případně přerušit provádění mezi delšími výpočty, je-li to třeba
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // vyčistíme zásobník
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));

// pokud je vše v pořádku, zpracujeme data z DMP
} else if (mpuIntStatus & 0x02) {
    // čekání na správnou délku dat
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // přečteme paket ze zásobníku
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // pokud je v zásobníku víc než jeden paket, tímto zajistíme, že bude přečten v dalším cyklu
    fifoCount -= packetSize;

    // naplnění proměnných s vypočítanými hodnotami
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    //Výpis YAW/PITCH/ROLL
    Serial.print("ypr\t");
    Serial.print(ypr[0] * 180/M_PI);
    Serial.print("\t");
    Serial.print(ypr[1] * 180/M_PI);
    Serial.print("\t");
    Serial.println(ypr[2] * 180/M_PI);
}
} 

Jde o ukázkový kód, který najdete v knihovně i2clibdev, ořezaný o pro nás zatím zbytečné části a také doplněný českými komentáři. Když ho zkompilujete do svého Arduina, dostanete výpis podobný tomuto:

Initializing I2C devices...
Testing device connections...
MPU6050 connection successful
Initializing DMP...
Enabling DMP...
Enabling interrupt detection (Arduino external interrupt 0)...
DMP ready! Waiting for first interrupt...
[...]
ypr -169.34 51.40   -4.06
ypr -168.96 48.65   -2.35
ypr -168.55 45.82   -0.83
ypr -168.13 42.98   0.48
ypr -167.69 40.19   1.61
ypr -167.30 37.46   2.59
ypr -166.95 34.79   3.48
ypr -166.59 32.20   4.31
ypr -166.24 29.73   5.17
[...] 

Čísla jsou oddělena tabelátorem a jsou to úhly na osách X,Y a Z desky MPU-6050. U letadel a koptér se používá označení yaw, pitch a roll. Lepší představu získáte z následujícího obrázku:

Zdroj: Wikipedia

MPU-6050 je nutné na kvadrokoptéru umístit rozumně, to znamená, aby osy senzorů kopírovaly právě yaw, pitch a roll. Je vcelku jedno, zda bude yaw osou X, Y nebo Z na MPU-6050, to snadno vyladíte během procesu, který bude následovat v dalších dílech.

Až budete mít vše funkční, všimněte si, že chvilku trvá, než se naměřené úhly stabilizují a je třeba několik sekund počkat. Poté, když nakloníte gyro do libovolného úhlu, tak se tento nebude měnit v závislosti ani na vibracích, ani na změnách rychlosti, kterou se pohybuje.

Shrnutí

V dnešním díle jsme z MPU-6050 dostali nějaká rozumná čísla, která nyní stačí předhodit PID regulátoru, který se postará o to, aby quadrokoptéra stála rovně ve vzduchu. Nyní je správný čas, kdy bychom měli doladit detaily na celé konstrukci a vyřešit propojení mezi Raspberry Pi a Arduinem. Další fáze bude spočívat ve vyladění PID regulátoru, resp. v jemném nastavení regulačních konstant a není pohodlné kvůli jejich změnám pokaždé nahrávat do Arduina celý program. To tedy bude téma příštího dílu.