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>
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;
}