Programme de gestion du capteur MPU-6050

main.cpp

/****************************************************************************************

**** Gestion de la carte GY-521 avec un RaspBerry PI et la bibliothèque wiringPi

**** Norbert BRAUN

*** FS_SEL laissé par défaut (0) ce qui donne +/- 250 degré/s soit 131 LSB/degré/s

*** Code inspiré des codes de

                                          Andrew Birkett article Bitify du 7/11/2013

                                         Nahid Alam. <nahid.mahfuza.alam@gmail.com>

                                         WIKIVERSITE

                                        ELECTRICAL ENGINEERING Calculating Angles from MPU6050

                                         University of North Carrolina Asheville : Using the MPU-6050

*****************************************************************************************

*/

#include <stdio.h>

#include <wiringPi.h>

#include <wiringPiI2C.h>

#include <QDebug>

#include "math.h"

int adresse=0x68;//adresse du port I2C. Utiliser i2cdetect pour trouver cette adresse

long int timePrev, temps, timeStep;

double arx, ary, arz, gsx, gsy, gsz, gry, grz, grx, rx, ry, rz;

int fd;

int i=1;

static const double gyroScale = 131; //131 LSB/deg/s

static const double Rad2Degr=57.295779506;//PI=180°

int angleX, angleY, angleZ;

float angleX0, angleY0, angleZ0;

//***********************************************************************************

int conversionSigne(double valeur)

{//le registre contient des données signées (le LSB donne le signe)=>convertion signée

if (valeur>=0x8000)

return -((65535-valeur)+1);

else

return valeur;

}

//************************************************************************************

double getAccX()

{

double temp;

//lecture des deux octets accélération sur le bus I2C

temp=wiringPiI2CReadReg8(fd,0x3B)<<8|wiringPiI2CReadReg8(fd,0x3C);

return conversionSigne(temp);

}

//***************************************************************************************

double getAccY()

{

double temp;

temp=wiringPiI2CReadReg8(fd,0x3D)<<8|wiringPiI2CReadReg8(fd,0x3E);

return conversionSigne(temp);

}

//***************************************************************************************

double getAccZ()

{

double temp;

temp=wiringPiI2CReadReg8(fd,0x3F)<<8|wiringPiI2CReadReg8(fd,0x40);

return conversionSigne(temp);

}

//************************************************************************************

double getGyroX()

{

return wiringPiI2CReadReg8(fd,0x43)<<8|wiringPiI2CReadReg8(fd,0x44);

}

//************************************************************************************

double getGyroY()

{

return wiringPiI2CReadReg8(fd,0x45)<<8|wiringPiI2CReadReg8(fd,0x46);

}

//************************************************************************************

double getGyroZ()

{

return wiringPiI2CReadReg8(fd,0x47)<<8|wiringPiI2CReadReg8(fd,0x48);

}

//************************************************************************************

void lireTous()

{

//lecture de tous les registres du capteur MPU6050

short int temp;

for (int i=0x0D;i<0x76;i++)

{

temp=wiringPiI2CReadReg8(fd,i);

qDebug()<<temp<<endl;

}

}

//**************************************************************************************

float getAngleX()

{

double temp;

double aX, aY, aZ;

aX=getAccX()/16384;//16.384 LSB pour 1g d'après la doc

aY=getAccY()/16384;

aZ=getAccZ()/16384;

temp=sqrt( pow(aY,2)

                 + pow(aZ,2));

return Rad2Degr * atan(aX/temp);

}

//**************************************************************************************

float getAngleY()

{

double temp;

double aX, aY, aZ;

aX=getAccX()/16384;//16.384 LSB pour 1g d'après la doc

aY=getAccY()/16384;

aZ=getAccZ()/16384;

temp=sqrt( pow(aX,2)

               + pow(aZ,2));

return Rad2Degr * atan(aY/temp);

}

//**************************************************************************************

float getAngleZ()

{ //cette fonction n'a pas vraiment d'intérêt si le capteur est posé à plat

//car il est impossible de trouver l'angle de rotation Z

//étant donné que l'accélération se détermine à partir du vecteur gravitationnel

double temp;

double aX, aY, aZ;

aX=getAccX()/16384;//16.384 LSB pour 1g d'après la doc

aY=getAccY()/16384;

aZ=getAccZ()/16384;

temp=sqrt( pow(aX,2)

              + pow(aY,2));

return Rad2Degr*atan(temp/aZ);

}

//**************************************************************************************

void getAngles()

{ //cette fonction utilise le calcul d'angles par l'accélération

// puis intègre les données gyroscopiques par filtrage (filtre de Kalman) pour éviter les dérives

// liées à des chocs ou vibrations occasionnant des accélérations

timePrev = temps;//on mémorise le temps initial pour Dt

temps = millis();

timeStep = (temps - timePrev) / 1000; // time-step in s

//récup des valeurs

arx=getAngleX();

ary=getAngleY();

arz=getAngleZ();

gsx=getGyroX()/gyroScale;

gsy=getGyroY()/gyroScale;

gsz=getGyroZ()/gyroScale;

if (i==1)

{ //initialement les valeurs gyro=valeurs accel

grx=arx;

gry=ary;

grz=arz;

}

else

{

grx=grx+(timeStep*gsx);

gry=gry+(timeStep*gsy);

grz=grz+(timeStep*gsz);

}

delay(50);

//on filtre

angleX=(0.04*arx) +(0.96*grx);

angleY=(0.04*ary) +(0.96*gry);

angleZ=(0.04*arz) +(0.96*grz);

}

//*********************************************************************************

void initAngles()

{//détermine les valeurs lorsque le capteur est au repos

qDebug("Ne pas bouger et laisser le capteur en position d'équilibre");

double tx=0;

double ty=0;

double tz=0;

for (int i=0;i<100;i++)

{

getAngles();

tx +=angleX;

ty +=angleY;

tz +=angleZ;

}

angleX0=tx/100;

angleY0=ty/100;

angleZ0=tz/100;

}

//---------------------------------------------------------------------------------------

int main ()

{

temps = millis();

i=1;

wiringPiSetup () ;

fd=wiringPiI2CSetup (adresse) ;

if (fd==-1)

{

qDebug("Le port I2C n'est pas joignable. Vérifiez les pramètres et connexions.");

return -1;

}

else

{

wiringPiI2CWriteReg8(fd,0x6B,0x00);//réveille le capteur en gérant le power_mgmt

// lireTous();

initAngles();//récupère les valeurs de repos

while (1)

{

getAngles();

qDebug()<<"AngleX="<<angleX-angleX0<<" AngleY="<<angleY-angleY0<<endl;

delay(1000);

}

}

return 0;

}