Gyorsulás mérő és giroszkóp MPU6050-el

Ebben a projektben kiegészítettem a Gyro szenzort egy accelerometer-el, így már nagyon pontos szöget lehet mérni.

Viszont csak két írányban(előre, hátra és jobbra balra dőlés), A teljes lefedettséghez ez még mindíg nem elég. Egy geometer is szükséges lenne ahoz, hogy az elfordulást is meg lehessen mérni. Az itt bemutatott példában ±180°-os dőlést érzékel az eszköz. A spectrum analyzer cikkben bemutatott kijelzőn megjeleníti az elfordulást, valamint bluetooth modulon keresztül a PC-nek is elküldi, és ott pedig egy programmal megjeleníti azt.

 

Az MPU6050

Az eszköz A giroszkóp része ugyan az, mint a másik cikkben bemutatott IC. A gyorsulás mérő szenzor 4 féle méréshatárral rendelkezik, ±2g, ±4g, ±8g, ±16g. A szögméréshez önmagában azért nem elég a gyorsulás mérő, mert nagyon érzékeny az apró ütésekre, egy kis rázkódástól már hatalmas elfordulásokat kapnánk. A giroszkóp nem érzékeny az ütésekre, viszont a „drift” miatt pontatlan. A két szenzort egy úgynevezett complementary filter vagy Kalman-filter segítségével tökéletes mérésre használhatjuk. Én az egyszerűsége miatt a complementary filtert használtam fel a projektben.

 

Számolási elmélet

A földön minden álló testre 1g erő hat lefele irányban.

A Trigonometrikus függvények inverzei közül az arkusz-tangensfüggvényre lesz szükségünk:

Viszont így csak ±90°-ban tudunk szöget mérni, és ha túlhaladunk a 90°-on akkor össze vissza fog forogni a testünk a monitoron. Ahoz hogy ±180°-ot tudjunk mérni a következő képletre lesz szükségünk:

A complementary filter

A gyro szenzort integráljuk az idővel, a gyorsulásmérővel megkapott adatokból kiszámoljuk a szöget és behelyettesítjük a képletbe. A 0.98 és 0.02 értékekkel lehet hangolni a filtert. Megvalósítása:

#define ACCELEROMETER_SENSITIVITY 16384.0 //Az adatlapból
#define x 0 //A kapott tömbben az irányok sorszámai
#define y 1
#define z 2
#define M_PI 3.14159265359f	    
 
#define dt 0.005 // 5 ms sample rate, amit az MPU6050-ben beállítottunk  
 
void ComplementaryFilter(short accData[3], short gyrData[3], float *pitch, float *roll)
{
    float pitchAcc, rollAcc;               
 
    // Gyroszkóp adatok integrálása az idővel
    *pitch += ((float)gyrData[0] / GYROSCOPE_SENSITIVITY) * dt; // X-axis
    *roll -= ((float)gyrData[1] / GYROSCOPE_SENSITIVITY) * dt;  //  Y-axis
 
    // Drift kompenzálás
    // Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
    int forceMagnitudeApprox = abs(accData[x]) + abs(accData[y]) + abs(accData[z]);
    if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32767)
    {
	// A filter megvalósítása X tengelyen
        pitchAcc = atan2f((float)accData[y], sqrtf((float)accData[z] * 
		(float)accData[z] +(float)accData[x] * 
		(float)accData[x])) * 180 / M_PI;

        *pitch = *pitch * 0.98f + pitchAcc * 0.02f;
 
	// A filter megvalósítása Y tengelyen
        rollAcc = atan2f((float)accData[x], sqrtf((float)accData[z] *
 		 (float)accData[z] +(float)accData[y] *
	 	 (float)accData[y])) * 180 / M_PI;

        *roll = *roll * 0.98f + rollAcc * 0.02f;
    }
} 

 

A bluetooth

A bluetooth átvitel egy RN42 modullal történik. A bluetooth – PIC kapcsolatot UART-on keresztül történik. A bluetooth parancsait az adatlapban találjuk meg. A sebességen kívűl mást nem szükséges állítani, az alapértelmezett beállítások teljesen megfelelőek.  A PC-n ha párosítottuk az eszközt, szintén soros portként érjük el. Csatlakozás után mehet egyből az adatküldés.

Az UART és bluetooth init kódja:

unsigned char BTCMDMode[] = {"$$$"};            //Command mode kódja
unsigned char BTCMDModeOff[] = {"---\n"};       //Command mód kilépés
unsigned char BTSpeed [] = {"SU,11\n"};         //Baudrate Beállítás, 110k
unsigned char BTReset[] = {"R,1\n"};            //Reset kód
unsigned char *ptr;        

void UARTInit()					//UART init
{
    U1BRG = 416;                //Baudrate 9600 
    U1MODEbits.UARTEN = 1;      //UART Enable
    U1STAbits.UTXEN = 1;        //UART TX Enable
    RPOR1bits.RP36R = 0b000001;	//UART TX on RP36/RB4 pin



}

void BTInit()
{
    __delay_ms(100);
    BTRESET = 1;            //Engedélyezzük a bluetooth-ot
    __delay_ms(1000);        //Megvárjuk,míg feléled a BT
    ptr = BTCMDMode;
    while(*ptr != 0)        //Command módba rakjuk a Bluetoothot
    {
        while(U1STAbits.UTXBF);
        U1TXREG = *ptr;
        ptr++;    
    }
    __delay_ms(100);    
    ptr = BTSpeed;    
    while(*ptr != 0)        //Beállítjuk a Bluetooth sebességét 110k-ra
    {
        while(U1STAbits.UTXBF);
        U1TXREG = *ptr;
        ptr++;    
    }        
    __delay_ms(100);
    BTRESET = 0;            //BT SPEED aktiválás(Reset után lép életbe)
    __delay_ms(100);   
    BTRESET = 1;                        
    U1BRG = 35;                //Beállítjuk a PIC-et is az új sebességre
}

A kapcsolási rajz

Nyákot nem készítettem, csak a videón látható próbapanelen lett összeállítva. Egy elvi kapcsolás a következő ábrán látható. Az MPU6050-et modulként vásároltam meg, így annak csak a táp és I2C kivezetései találhatóak meg a rajzon: