Micro contrôleurs AVR/Travail pratique/Utilisation d'un Accéléromètre MPU6050

Début de la boite de navigation du travail pratique

La majeure partie de ce chapitre n'existerait pas sans la publication de Shane Colton du MIT (voir bibliographie en fin de chapitre).

Nous avons reclassé récemment (Janv.2020) ce TP en niveau 16 à cause de la présence des mathématiques sur les rotations. Vous pouvez cependant vous tourner vers le TP sur le 9250 qui reste beaucoup plus simple d'accès et qui fonctionne partiellement avec le MPU6050 si l'on enlève tous ce qui a rapport au magnétomètre.

Ce projet vise à comprendre et utiliser un capteur moderne d'accélération couplé à un gyroscope, deux capteurs aujourd'hui facilement trouvés ensemble dans un même composant à coût modeste (1 €-2 €).

Nous allons mettre en œuvre ce composant avec un Arduino, et le coupler à un programme Processing chargé de réaliser une visualisation 2D ou 3D de l'attitude de l'accéléromètre.

Certaines sections de ce travail pratique (sur les matrices de rotations) sont très théoriques. Elles ne sont utiles que si vous voulez comprendre le fondement des formules. Depuis la rédaction de ces exercices, nous avons réalisé un projet de robotique mobile en équilibre sur deux roues utilisant le même accéléromètre que ce TP (voir chapitre suivant) mais ne nécessite pas une connaissance approfondie des rotations 3D. Les rotations 2D, plus intuitives, permettent déjà de résoudre ce problème d'équilibre.

Nous réalisons aussi un projet de plate forme support d'une caméra comprenant deux servomoteurs. Nous y avons mis un accéléromètre et avons demandé à des étudiants de stabiliser la plate forme. Ils ont pu stabiliser le tangage avec l'accéléromètre. Reste à stabiliser le lacet, ce qui ne peut être fait avec un accéléromètre mais avec le gyroscope ; c'est le genre de détail qu'une bonne connaissance des rotations 3D permet de comprendre (les résultats détaillés seront publiés dans un futur proche).

Introduction aux capteurs IMU

IMU : Inertial Measurement Unit se traduit en français par Centrale à Inertie.

La référence du capteur que nous allons utiliser est GY-521 ; il est architecturé autour d'un MPU 6050 qui est composé de deux capteurs et d'un processeur :

• un capteur accéléromètre 3 axes (x,y et z) qui mesure une accélération ;
• un capteur gyroscope 3 axes qui mesure une vitesse angulaire ;
• un « Digital Motion Processor » (DMP) pouvant stocker et restituer des données.

Utiliser Processing pour visualiser des données d'un accéléromètre et d'un gyroscope.

La terminologie anglo-saxonne associée à ce genre de capteur est 6 DOF (Degree of Freedom). On devrait traduire par 6 degrés de liberté mais il est préférable de traduire par 6 axes (3 pour l'accéléromètre et 3 pour le gyroscope). Le meilleur de ce qui se fait actuellement pour les capteurs bon marché ( < 50 € ) comporte 9 axes. En général c’est un magnétomètre qui est ajouté pour essayer de repérer le Nord magnétique. Ces 9 axes sont en général suffisants pour réaliser, en combinant les mesures de ces trois capteurs, une centrale à inertie de coût modeste.

Mais ces trois capteurs ont des défauts rendant leur utilisation délicate.

Nous allons nous contenter de six axes dans la suite du présent projet dont l'objectif peut se résumer dans la figure ci-dessus : réaliser à l'aide d'un traitement des données des capteurs une visualisation avec Processing permettant de mettre en évidence les propriétés des capteurs accélérations et gyroscopes.

La notion d'attitude

L'attitude en robotique (et en astronautique) désigne la direction des axes de la pièce mobile du robot ; généralement caractérisée par trois angles (roulis, tangage et cap ou lacet).

Ce mot « attitude » ne doit pas être confondu avec « altitude » (ils n'ont en commun qu'une orthographe voisine) !

La notion d'attitude est très liée aux rotations en trois dimensions. Nous allons donc commencer par examiner ces rotations d'un point de vue mathématique.

Introduction : mathématiques des rotations

Il est difficile d'éviter le formalisme des matrices lorsqu'on aborde le sujet des rotations. Le chapitre Les Matrices d'un autre projet peut permettre d'aborder ce sujet. Ne le lisez pas de façon exhaustive, contentez-vous des concepts d'addition et de multiplication de matrices ainsi que de la notion de déterminant.

Les rotations en trois dimensions sont suffisamment complexes pour que nous leur accordions un peu de temps (voir aussi les articles Matrice de Rotation et Angles d'Euler dans wikipédia).

Cette partie mathématique sera agrémentée de code SCILAB pour essayer de découvrir quelques propriétés des rotations 3D.

Soient les matrices :

$R_{\mathbf {x} }(\phi )={\begin{pmatrix}1&0&0\\0&\cos \phi &-\sin \phi \\0&\sin \phi &\cos \phi \end{pmatrix}},\qquad R_{\mathbf {y} }(\theta )={\begin{pmatrix}\cos \theta &0&\sin \theta \\0&1&0\\-\sin \theta &0&\cos \theta \end{pmatrix}},\qquad R_{\mathbf {z} }(\psi )={\begin{pmatrix}\cos \psi &-\sin \psi &0\\\sin \psi &\cos \psi &0\\0&0&1\end{pmatrix}}.$

Elles nous permettent de définir les angles associés aux rotations autour des axes x,y et z.

Voici un code SCILAB permettant de définir ces trois matrices pour des valeurs particulières d'angle :

//******* SCILAB
phi=%pi/4;
theta=-%pi/4;
//theta=0;
psi=-%pi/3;
//psi=0.0;
// see https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf for rotations conventions
rotx=[1 0 0;0 cos(phi) sin(phi);0 -sin(phi) cos(phi)];
roty=[cos(theta) 0 -sin(theta);0 1 0;sin(theta) 0 cos(theta)];
rotz=[cos(psi) sin(psi) 0;-sin(psi) cos(psi) 0;0 0 1];

Euler 123

La notation 123 signifie que l’on fait d’abord la rotation 3 (autour de l'axe z) puis la rotation 2 (autour de l'axe y) pour finir par la rotation 1 (autour de l'axe x). La matrice de rotation associée est donc :

$R=R_{x}(\phi )\cdot R_{y}(\theta )\cdot R_{z}(\psi )$

Cette matrice vaut donc :

$R={\begin{pmatrix}\cos \theta \cdot \cos \psi &\cos \theta \cdot \sin \psi &-\sin \theta \\\cos \psi \sin \theta \sin \phi -\cos \phi \sin \psi &\cos \phi \cos \psi +\sin \theta \sin \phi \sin \psi &\cos \theta \sin \phi \\\cos \phi \cos \psi \sin \theta +\sin \phi \sin \psi &\cos \phi \sin \theta \sin \psi -\cos \psi \sin \phi &\cos \theta \cos \phi \end{pmatrix}}$

Toute rotation en dimension 3 est équivalente à un produit des trois rotations que l’on vient de définir. Cela signifie que trois paramètres suffisent pour définir une rotation (les trois Angles d'Euler).

Le code SCILAB permettant de réaliser le calcul de la matrice d'Euler 123 est :

//******* SCILAB
// construction matrice de rotation Euler 123
R=rotx*roty*rotz;
// construction matrice de rotation Euler 321
R2=rotz*roty*rotx;

Une rotation en dimension 3 peut aussi être définie par un axe de rotation et un angle (soit à priori 4 paramètres). Les quatre paramètres sont naturellement les trois composantes du vecteur qui définit l'axe et l'angle de rotation. Il est en fait possible d'économiser un paramètre en définissant l'axe à partir du centre d'une sphère ainsi que la longitude et latitude du point de rencontre de l'axe avec la sphère. Une autre manière d'économiser une donnée est de définir l'axe de rotation par un vecteur unitaire pour lequel trois coordonnées suffisent. Bref seulement 3 paramètres suffisent à définir une rotation.

Remarque

Euler123 désigne dans un autre article de la wikiversité : Euler 123 Sequence (en) ce que l'on appelle Euler321 ici . À ce jour nous donc rencontré deux définitions différentes. L'ambiguïté de la notation est liée à une propriété de la multiplication des matrices :

//******* SCILAB
// construction matrice de rotation Euler 123
R=rotx*roty*rotz;

Cette matrice R aura un vecteur argument à sa droite sur laquelle elle agira et comme "rotz" est à droite c'est elle agira en premier. Notre qualification 123 respecte donc l'ordre d'apparition des matrices dans le produit : rotx puis roty puis rotz (mais qui veut dire d'abord l'application de rotz) ! Pour éviter toute ambiguïté Il serait mieux d'appliquer les notations de Phil Kim (voir bibliographie) qui serait dans ce cas z->y->x (ou x<-y<-z) qui exprime bien le fait que l'on commence par la rotation autour de l'axe z pour terminer par celle autour de l'axe x. Les flèches expriment très intuitivement l'ordre d'application des matrices rotations.

On rappelle que l'ambiguïté que l'on vient d'exprimer est liée à la non commutativité des rotations 3D !

Angle de rotation

Nous venons de dire que toute rotation est équivalente à un angle de rotation autour d'un axe de rotation. Cependant, une façon naturelle de définir une matrice de rotation de manière pratique est simplement par les coordonnées de chacun des axes. En clair vous disposez d'un repère (${\overrightarrow {i}},{\overrightarrow {j}},{\overrightarrow {k}}$) et vous cherchez la matrice qui vous permet d'obtenir ce repère après rotation (${\overrightarrow {i'}},{\overrightarrow {j'}},{\overrightarrow {k'}}$). Cette matrice est définie par les coordonnées de chacun des vecteurs (${\overrightarrow {i'}},{\overrightarrow {j'}},{\overrightarrow {k'}}$) dans (${\overrightarrow {i}},{\overrightarrow {j}},{\overrightarrow {k}}$), soit 9 paramètres. La matrice contenant ces 9 paramètres est appelée DCM : Direction Cosine Matrix). La données de ces paramètres ne vous permettra pas de vous faire une idée intuitive de la rotation correspondante. En général, il faut donc savoir comment passer des 9 paramètres aux 3 Angles d'Euler ou (pour les quaternions) qui vous décriront de manière plus intuitive la rotation. Cherchons une description axe de rotation plus angle.

Commençons dans cette section par rechercher l'angle de rotation en question ; nous le noterons $\theta _{global}$ pour éviter toute confusion avec $\theta$ de la section précédente.

La trace d'une matrice est la somme de ses éléments sur sa diagonale. Elle est toujours liée à l'angle de rotation.

Le calcul de l'angle $\theta _{global}$ associé à une rotation est tout simple. Il se fait à l'aide de la formule :

$\operatorname {Tr} (R)=1+2\cos(\theta _{global})$

Le code SCILAB permettant le calcul d'angle est donc :

//******* SCILAB
// matrice R de rotation 3x3
angle= acos((trace(R)-1)/2)

Calcul de l'axe de rotation

Utilisation de SCILAB

L'axe de rotation est plus difficile à calculer si l'on ne dispose pas d'une puissance de calcul suffisante. Sur notre PC ce n’est pas un problème : il faut chercher le vecteur invariant par cette rotation. Techniquement cela revient à chercher le seul et unique vecteur propre associé à la valeur propre +1. Plutôt que d’en faire la théorie, nous préférons donner le code de SCILAB qui permet facilement ce genre de calcul :

//******* SCILAB
// première maniere
[Ab,X]=bdiag(R)

affiche :

X  =
 
    0.2850828  - 0.9324527  - 0.2219452  
  - 0.4494128  - 0.3345581    0.8283109  
    0.8466144    0.1363922    0.5144330  
 Ab  =
 
    0.3200597    0.9473974    0.  
  - 0.9473974    0.3200597    0.  
    0.           0.           1.  

La dernière colonne de X, celle en face du 1 comme valeur propre est l'axe de rotation. L'axe est donc défini par le vecteur ${\overrightarrow {u}}{\begin{pmatrix}{-0.2219452}\\0.8283109\\0.5144330\end{pmatrix}}$.

Il existe une autre méthode avec SCILAB :

//******* SCILAB
//deuxième manière
[al,be]=spec(R)

qui donne

 be  =
 
    0.3200597 + 0.9473974i    0                         0   
    0                         0.3200597 - 0.9473974i    0   
    0                         0                         1.  
 al  =
 
    0.6894709                 0.6894709               - 0.2219452  
    0.1333194 - 0.3730636i    0.1333194 + 0.3730636i    0.8283109  
    0.0827997 + 0.6006859i    0.0827997 - 0.6006859i    0.5144330  
 

qui permet de retrouver le même axe : ${\overrightarrow {u}}{\begin{pmatrix}{-0.2219452}\\0.8283109\\0.5144330\end{pmatrix}}$.

Pour la suite on supposera que notre calcul a été fait avec la première méthode et donc que l’on a calculé X, et que, si l’on a besoin de l'axe, il suffit de l'extraire de X.

Sans SCILAB

Les microcontrôleurs de type AVR (et même bien d'autres sont incapables de faire tourner SCILAB (ou OCTAVE/MATLAB). Il nous faut donc plutôt une formule pour trouver l'axe de rotation. Cette formule nécessite déjà le calcul de l'angle de rotation. La formule que nous avons donné précédemment peut être utilisée sur n'importe quel processeur, y compris sur un AVR.

Commençons par le calcul de l'angle de rotation :

$\operatorname {Tr} (R)=1+2\cos(\theta _{global})$

donne :

${\operatorname {Tr} (R)-1 \over 2}=\cos(\theta _{global})$

et finalement :

$\theta _{global}=\arccos({{\operatorname {Tr} (R)-1} \over 2})$

On peut démontrer que l'axe est caractérisé par le vecteur u :

${\overrightarrow {u}}{\begin{pmatrix}{R_{32}-R_{23} \over \sin(\theta _{global})}\\{R_{13}-R_{31} \over \sin(\theta _{global})}\\{R_{21}-R_{12} \over \sin(\theta _{global})}\end{pmatrix}}$

où $R_{32}$ désigne l'élément de la matrice de rotation de la 3ème ligne et de la deuxième colonne.

Remarque

Si vous n'avez pas besoin que le vecteur u soit unitaire, vous pouvez naturellement retirer le sinus thêta de votre formule.

Matrice de rotation à partir d'un axe et d'un angle

Nous cherchons maintenant à résoudre le problème inverse : nous connaissons l'axe de rotation et l'angle associé, comment trouver la matrice de rotation associée ?

On peut calculer la matrice R de rotation autour d'un axe dirigé par un vecteur unitaire ${\overrightarrow {u}}{\begin{pmatrix}{u_{x}}\\u_{y}\\u_{z}\end{pmatrix}}$ (donc avec $u_{x}^{2}+u_{y}^{2}+u_{z}^{2}=1$) et d'un angle $\theta _{global}$. La formule est :

$R={\begin{pmatrix}u_{x}^{2}(1-c)+c&u_{x}u_{y}(1-c)-u_{z}s&u_{x}u_{z}(1-c)+u_{y}s\\[3pt]u_{x}u_{y}(1-c)+u_{z}s&u_{y}^{2}(1-c)+c&u_{y}u_{z}(1-c)-u_{x}s\\[3pt]u_{x}u_{z}(1-c)-u_{y}s&u_{y}u_{z}(1-c)+u_{x}s&u_{z}^{2}(1-c)+c\end{pmatrix}}$

où

$c=\cos(\theta _{global}),\qquad s=\sin(\theta _{global})$

Si l'espace en 3 dimensions est orienté de façon conventionnelle, cette rotation se fera dans le sens inverse aux aiguilles d'une montre pour un observateur placé de telle sorte que le vecteur directeur ${\overrightarrow {u}}$ pointe dans sa direction (règle de la main droite).

Exemple en SCILAB

//******* SCILAB
// recupération de l'axe : troisième colonne de X
axis = X(:,3);
// construction de la matrice de rotation à partie de l'axe et de l'angle :
R2 = [cos(angle)+axis(1)*axis(1)*(1-cos(angle)) axis(2)*axis(1)*(1-cos(angle))-axis(3)*sin(angle) axis(3)*axis(1)*(1-cos(angle))+axis(2)*sin(angle);
      axis(1)*axis(2)*(1-cos(angle))+axis(3)*sin(angle)  cos(angle)+axis(2)*axis(2)*(1-cos(angle)) axis(3)*axis(2)*(1-cos(angle))-axis(1)*sin(angle);
      axis(1)*axis(3)*(1-cos(angle))-axis(2)*sin(angle) axis(2)*axis(3)*(1-cos(angle))+axis(1)*sin(angle) cos(angle)+axis(3)*axis(3)*(1-cos(angle))];

// calcul matrice rotation avec exponentiation 
Z=[0 -axis(3) axis(2);axis(3) 0 -axis(1);-axis(2) axis(1) 0];
R3=expm(angle*Z); // redonne R2

redonne exactement la même matrice que R pour les deux techniques de construction de R2 et R3.

Rotations du vecteur accélération de pesanteur

Tout accéléromètre est sensible au vecteur accélération de pesanteur. Cela en fait un très bon candidat pour mesurer l'attitude d'un solide pourvu que celui-ci ne soit pas soumis à une autre accélération (que l'accélération de pesanteur). Ceci est rarement le cas pour un robot mais l'accélération sera faible par rapport au vecteur d'accélération de pesanteur au moins tant qu’il n'y a pas de chocs.

Le vecteur accélération possède comme tout vecteur trois composantes :

${\overrightarrow {g}}{\begin{pmatrix}{g_{x}}\\g_{y}\\g_{z}\end{pmatrix}}$

S'il est exprimé dans un repère avec z vertical et vers le haut, et s'il est normalisé, ce vecteur devient :

${\overrightarrow {g}}{\begin{pmatrix}{0}\\0\\-1\end{pmatrix}}$

Après une rotation Euler 123, ce vecteur est transformé en

${\overrightarrow {g}}'=R_{x}(\phi )\cdot R_{y}(\theta )\cdot R_{z}(\psi )\cdot {\overrightarrow {g}}=R_{x}(\phi )\cdot R_{y}(\theta )\cdot R_{z}(\psi )\cdot {\begin{pmatrix}{0}\\0\\-1\end{pmatrix}}$

soit, tout calcul fait,

${\overrightarrow {g}}'={\begin{pmatrix}{\sin \theta }\\-\cos \theta \sin \phi \\-\cos \theta \cos \phi \end{pmatrix}}$

Cette formule montre qu’à partir d'une mesure de g dans le repère qui a tourné, il est possible de trouver $\theta$ et $\phi$ mais pas $\psi$ !!!

• En effet $g_{x}=\sin(\theta )$ permet de trouver $\theta$
• Puis ${g_{y} \over g_{z}}=\tan(\phi )$ permet de trouver $\phi$

Remarque

Nous proposons une autre façon plus géométrique d’aborder le problème de l'impossibilité de retrouver l'angle de rotation (angle $\psi$ précédent) autour de l'axe vertical.

Pourquoi la connaissance des composantes de l'accélération de pesanteur ne suffisent plus en 3D ?

Rappelons que ce que l’on cherche à faire est de retrouver les axes x, y et z à partir des seules connaissances des composantes gx, gy et gz du vecteur ${\vec {g}}$ (sachant que ${\vec {g}}$ est vertical). Mais regardez la figure attentivement pour vous convaincre de l'impossibilité d'une solution générale à ce problème.

Dans cette figure, nous avons dessiné un parallélépipède en vert, les axes x, y et z et le vecteur g d'accélération de pesanteur (diagonale du parallélépipède). Imaginez cet ensemble comme un solide et faites-le tourner autour de l'axe vertical d'un angle quelconque. Alors aucune composante de g ne change mais le repère (x,y,z) n'est plus le même puisqu’il a tourné. Ainsi la connaissance de gx, gy, gz ne suffira pas pour trancher parmi tous les repères possibles, lequel est celui que l’on cherche. Votre accéléromètre est insensible aux rotations par rapport à un axe vertical et sera donc insuffisant pour retrouver l'attitude.

Autre manière de calculer les angles

Cette propriété d'invariance de g par une rotation autour de l'axe vertical permet de trouver une autre relation pour le calcul de $\theta$ et de $\phi$. Il s'agit d'un calcul trouvé dans la note d'application AN4248 de Freescale Semiconcuctor intitulée Implementing a Tilt-Compensated eCompass using Accelerometer and Magnetometer Sensors.

Repartons de

${\overrightarrow {g}}'=R_{x}(\phi )\cdot R_{y}(\theta )\cdot R_{z}(\psi )\cdot {\overrightarrow {g}}=R_{x}(\phi )\cdot R_{y}(\theta )\cdot R_{z}(\psi )\cdot {\begin{pmatrix}{0}\\0\\-1\end{pmatrix}}$

Cette formule peut se simplifier en (pas d'effet de la matrice de rotation suivant l'axe z sur g) :

${\overrightarrow {g}}'=R_{x}(\phi )\cdot R_{y}(\theta )\cdot {\overrightarrow {g}}=R_{x}(\phi )\cdot R_{y}(\theta )\cdot {\begin{pmatrix}{0}\\0\\-1\end{pmatrix}}$

puis en faisant passer la rotation inverse dans le terme de gauche (avec changement de signe de l'angle pour inverser la matrice)

$R_{x}(-\phi )\cdot {\overrightarrow {g}}'=R_{y}(\theta )\cdot {\overrightarrow {g}}=R_{y}(\theta )\cdot {\begin{pmatrix}{0}\\0\\-1\end{pmatrix}}$

pour finalement donner avec la même opération que précédemment pour la rotation autour de l'axe y

$R_{y}(-\theta )\cdot R_{x}(-\phi )\cdot {\overrightarrow {g}}'={\overrightarrow {g}}={\begin{pmatrix}{0}\\0\\-1\end{pmatrix}}$

En remplaçant par les matrices correspondantes cela donne donc

$R_{\mathbf {x} }(-\phi )={\begin{pmatrix}1&0&0\\0&\cos \phi &\sin \phi \\0&-\sin \phi &\cos \phi \end{pmatrix}},\qquad R_{\mathbf {y} }(-\theta )={\begin{pmatrix}\cos \theta &0&-\sin \theta \\0&1&0\\\sin \theta &0&\cos \theta \end{pmatrix}}.$

d'où

${\begin{pmatrix}1&0&0\\0&\cos \phi &\sin \phi \\0&-\sin \phi &\cos \phi \end{pmatrix}}\cdot {\begin{pmatrix}\cos \theta &0&-\sin \theta \\0&1&0\\\sin \theta &0&\cos \theta \end{pmatrix}}\cdot {\overrightarrow {g}}'={\begin{pmatrix}1&0&0\\0&\cos \phi &\sin \phi \\0&-\sin \phi &\cos \phi \end{pmatrix}}\cdot {\begin{pmatrix}\cos \theta &0&-\sin \theta \\0&1&0\\\sin \theta &0&\cos \theta \end{pmatrix}}\cdot {\begin{pmatrix}{g'_{x}}\\g'_{y}\\g'_{z}\end{pmatrix}}={\overrightarrow {g}}={\begin{pmatrix}{0}\\0\\-1\end{pmatrix}}$

En effectuant le produit matriciel on trouve :

${\begin{pmatrix}\cos \theta &\sin \theta \cdot \sin \phi &\sin \theta \cdot \cos \phi \\0&\cos \phi &-\sin \phi \\-\sin \theta &\cos \theta \cdot \sin \phi &\cos \theta \cdot \cos \phi \end{pmatrix}}\cdot {\begin{pmatrix}{g'_{x}}\\g'_{y}\\g'_{z}\end{pmatrix}}={\overrightarrow {g}}={\begin{pmatrix}{0}\\0\\-1\end{pmatrix}}$

Les deux composantes de g à 0 en commençant par gy donnent

$g'_{y}\cdot \cos \phi -g'_{z}\cdot \sin \phi =0$

soit

$\tan \phi ={g'_{y} \over g'_{z}}$ ce qui permet de trouver $\phi$.

De même la composante suivant x donne :

$g'_{x}\cdot \cos \theta +g'_{y}\cdot \sin \theta \cdot \sin \phi +g'_{z}\cdot \sin \theta \cdot \cos \phi =0$

soit enfin

$\tan \theta ={-g'_{x} \over {g'_{y}\cdot \sin \phi +g'_{z}\cdot \cos \phi }}$

Vous avez ainsi deux relations qui vous permettent de calculer les angles en utilisant seulement la primitive atan2.

On retiendra donc :

calcul de l'angle de roulis et de l'angle de tangage à l'aide d'un accéléromètre

À partir des connaissances des trois composantes de g perçues par votre accéléromètre vous pouvez calculer les deux premiers angles de la rotation d' Euler123.

• Première méthode :
  • $g_{x}=\sin(\theta )$ permet de trouver $\theta$
  • ${g_{y} \over g_{z}}=\tan(\phi )$ permet de trouver $\phi$
• Deuxième méthode qui a notre préférence :
  • d'abord l'angle de roulis : $\tan \phi ={g_{y} \over g_{z}}$ ce qui permet de trouver $\phi$
  • puis à l'aide de ce résultat l'angle de tangage : $\tan \theta ={-g_{x} \over {g_{y}\cdot \sin \phi +g_{z}\cdot \cos \phi }}$

Axe de rotation du vecteur g

Le script suivant :

//******* SCILAB
// réalisation du produit vectoriel
exec("CrossProd.sci")
// valeur de g dans le repère avant rotation
g0 = [0;0;-1];
// valeur du vecteur g après rotation
g1 = R * g0;
// il est inutile de normaliser, mais au cas où ...
g1 = g1 / norm(g1);
// calcul de l'axe à partir du produit vectoriel : IL EST MONTRE PLUS LOIN QUE CETTE IDÉE SIMPLE EST FAUSSE
axis2=CrossProd(g0,g1);

nécessite l’utilisation de CrossProd.sci qui doit contenir :

//******* SCILAB
function [p] = CrossProd(u,v)
//Calculates the cross-product of two vectors u and v.
//Vectors u and v can be columns or row vectors, but
//they must have only three elements each.
[nu,mu] = size(u);
[nv,mv] = size(v);
if nu*mu <> 3 | nv*mv <> 3 then
error('Vectors must be three-dimensional only')
abort;
end
A1 = [ u(2), u(3); v(2), v(3)];
A2 = [ u(3), u(1); v(3), v(1)];
A3 = [ u(1), u(2); v(1), v(2)];
px = det(A1); py = det(A2); pz = det(A3);
p = [px, py, pz]
//end function

L'axe de rotation d'une rotation 3D ne peut pas être retrouvé à l'aide d'un produit vectoriel

La réalisation de ce programme nous montre que l'on ne retrouve absolument pas l'axe correctement. Ce code est donc laissé seulement pour information, pour découvrir les chemins tortueux employés par l'esprit de l'auteur.

Cette erreur a permis de tirer la propriété suivante :

Propriété

Nous avons donc découvert un autre point avec SCILAB : si l'axe de rotation d'une rotation 2D peut être retrouvé avec le produit vectoriel, ce n'est généralement pas le cas pour une rotations 3D. Il nous a fallu un peu de temps pour trouver une figure convaincante, celle ci-contre, que nous avons mis pour illustrer ce point. Le vecteur bleu tourne pour donner le vecteur noir autour de l'axe (vecteur rouge). Le produit vectoriel donne le vecteur orange qui n'est en aucun cas égal au vecteur rouge.

Les conséquences de cette propriété sont profondes. Elles montrent ainsi qu'il est impossible de retrouver une matrice de rotation à partir de la connaissance de deux vecteurs (le deuxième étant une rotation du premier) puisqu’il y a une infinité de rotations possibles. Vous pouvez, en effet, imaginer un tas de cercles passant par les extrémités des vecteurs bleu et noir. Ceci même avec la contrainte que l'axe de rotation passant par centre du cercle doit aussi passer par l'origine commune des deux vecteurs !

Passage en revue du travail de Kimberly Tuck (2007)

Nous avons eu l’occasion de commenter le travail Gyroscopes and Accelerometers on a Chip sur Internet et nous désirons expliquer les problèmes rencontrés ici.

Les raisonnements 3D nécessitent une bonne concentration. Nous avons nous-même commis des erreurs plusieurs fois (voir section précédente).

La note d'application AN3461 : Tilt Sensing Using Linear Accelerometers by: Kimberly Tuck (2007) est passée partiellement en revue maintenant.

Figure 8 de AN3461 modifiée pour expliquer les formules associées

Cette note comporte un point central : sa "Figure 8" qui nous a posé de sérieux problèmes. Il nous a été impossible de bien comprendre exactement sa partie dessin. Nous ne pouvons pas la publier ici (pour des raisons de copyright) ; nous avons donc refait une figure qui nous semble bien plus explicative que l'original, mais qui, du coup, est plus complexe : c’est la figure associée à cette section (ci-contre).

Les formules qui y apparaissent sont les mêmes que les formules originales (celles qui nous serviront pour l'exercice 4) mais nous nous sommes attachés à faire apparaître les triangles rectangles desquels on peut les déduire facilement si l’on se rappelle qu'une tangente est le rapport du côté opposé sur le côté adjacent.

De son côté, le texte de cette AN3461, nous définit les angles comme :

• $\phi$ est défini comme l'angle entre l'axe Y et le plan horizontal
• $\rho$ est défini comme l'angle entre l'axe X et le plan horizontal
• $\theta$ est défini comme l'angle entre l'axe Z et le vecteur accélération

Or sur la figure que nous proposons, seule la définition de $\theta$ saute immédiatement aux yeux. Pour les deux autres c’est un peu plus caché et cela demande un raisonnement géométrique : il vous faut remarquer que les angles cherchés ont leur côté perpendiculaires aux angles dessinés. Ils sont donc soit égaux soit supplémentaire. Le dessin faisant apparaître des angles plus petit que 90°, ils sont forcément égaux.

Par exemple, pour $\phi$, $A_{y}$ est perpendiculaire au côté du fond (l'angle droit est dessiné en rouge) et par définition le plan horizontal est perpendiculaire au vecteur accélération de pesanteur (gros vecteur vertical). Ainsi l'angle $\phi$ de la figure est aussi l'angle entre l'axe Y et le plan horizontal. CQFD.

Apartir de maintenant les Angles d'Euler sont notés avec des indices 123 pour éviter toute confusion avec les angles calculés par Kimberly Tuck.

Propriété

Les formules de la figure 8

$\tan \phi ={A_{y} \over {\sqrt {(A_{x}^{2}+A_{z}^{2})}}}$

$\tan \rho ={A_{x} \over {\sqrt {(A_{y}^{2}+A_{z}^{2})}}}$

$\tan \theta ={{\sqrt {(A_{x}^{2}+A_{y}^{2})}} \over A_{z}}$

ne sont pas des angles d'Euler et d'ailleurs Kimberly Tuck n'a jamais prétendu le contraire... mais aurait été avisée d'essayer de les relier ensembles.

L'utilisation de SCILAB nous a permis cependant de trouver une relation entre les deux. En effet le code :

anglex=%pi/4; //phi
angley=-%pi/4; //theta
anglez=-%pi/3; //psi
// see https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf for rotations conventions
rotx=[1 0 0;0 cos(anglex) sin(anglex);0 -sin(anglex) cos(anglex)];
roty=[cos(angley) 0 -sin(angley);0 1 0;sin(angley) 0 cos(angley)];
rotz=[cos(anglez) sin(anglez) 0;-sin(anglez) cos(anglez) 0;0 0 1];
// construction matrice de rotation Euler 123
R=rotx*roty*rotz;

//AN3461 Kimberly Tuck 2007
g0 = [0;0;-1];
g2 = R * g0;
g2 = g2 / norm(g2);
phi = atan(g2(2),sqrt(g2(1)*g2(1)+g2(3)*g2(3)));
rho = atan(g2(1),sqrt(g2(2)*g2(2)+g2(3)*g2(3)));
erreurx = (anglex-phi)*180/%pi
erreury= (angley-rho)*180/%pi

donne :

-->erreurx = (anglex-phi)*180/%pi
 erreurx  =
    75.  
-->erreury= (angley-rho)*180/%pi
 erreury  =
    0. 

Ce qui est surprenant est la valeur de "erreury" qui est nulle :

Propriété

Le deuxième angle de Euler 123, à savoir $\theta$, est identique au $\rho$ calculé par la formule de Kimberly Tuck. La démonstration de cette propriété est très simple : Si vous remplacez dans $\tan \rho ={g_{x} \over {\sqrt {(g_{y}^{2}+g_{z}^{2})}}}$ les composantes de g par leur valeurs après Euler 123, soit donc : ${\overrightarrow {g}}'={\begin{pmatrix}{\sin \theta _{123}}\\-\cos \theta _{123}\sin \phi _{123}\\-\cos \theta _{123}\cos \phi _{123}\end{pmatrix}}$ vous retrouvez $\tan \theta _{123}$.

Cela peut sembler de la petite histoire mais cette façon de trouver l'angle avec une tangente inverse donne un résultat entre $-\pi$ et $\pi$ (avec la primitive atan2) alors que l'autre méthode utilise un asin qui donne un angle entre $-\pi \over 2$ et $\pi \over 2$.

Est-il donc possible d’utiliser les angles calculés avec les formules de Kimerly Tuck pour trouver l'angle d'Euler manquant ? Nous avons déjà répondu par la négative à cette question mais nous pensons qu’il est utile de creuser la question : qu'est-ce qui fait que les calculs de Kimberly sont insensibles aux rotations autour d'un axe vertical ? Pour comprendre ce qui se passe on se pose la question suivante :

Nous vous donnons les trois angles de Kimberly , pouvez-vous amener la position du parallélépipède conformément à ces angles ?

Remarque

Nous laissons les lignes ci-dessous mais nous venons de découvrir dans un livre sur les rotations (p47-48) que :

• les rotations par rapport aux axes fixes ne sont pas commutatives donc problème ci-dessus n'aurait pas de solution puisqu'il ne nous donne pas l'ordre des rotations
• les rotations par rapport aux axes fixes sont quand même calculées par produit matriciel !!!

Prenez donc les lignes ci-dessous avec toutes les réserves qui s'imposent :

La réponse est simple si vous comprenez que vos rotations doivent avoir lieu par rapport à des axes fixes (toujours par rapport au repère initial). Vous tournez d’abord autour de l'axe X de $\phi$. Votre axe Y devient alors Y' mais c’est toujours autour de Y que vous tournez de $\rho$ (mathématiquement ceci n’est pas une multiplication matricielle !)... et comme on garde les axes de départ, la troisième rotation est autour de Z et donc ne peut changer les composantes du vecteur accélération. Pour la petite histoire l'angle $\theta$ de Kimberly ne correspond pas à une rotation autour de Z et n'apporte donc rien !

Malgré cette remarque qui sera retirée dès que les problèmes évoqués seront résolus, nous formulons la propriété suivante :

Propriété

À ce point, les meilleures formules que nous pouvons espérer pour résoudre le problème de l'attitude avec l'accéléromètre sont donc :

• $\tan \theta _{123}={g_{x} \over {\sqrt {(g_{y}^{2}+g_{z}^{2})}}}$
• $\tan \phi _{123}={g_{y} \over g_{z}}$

Ces deux angles font partie d'une rotation de Euler 123 et il est impossible de retrouver le troisième angle !

Le problème de la deuxième formule est son instabilité numérique. Une solution est examinée dans la nouvelle note d'application AN3461.

Passage en revue du travail de Mark Pedley (2013)

La note d'application AN3461 a été fortement remaniée : AN3461 : Tilt Sensing Using Linear Accelerometers by: Mark Pedley (2013). Seuls quelques points sont examinés maintenant.

• Les calculs correspondants à l'équation $\tan \phi _{123}={g_{y} \over g_{z}}$ sont instables quand gy et gz sont tous deux voisins de 0, c'est-à-dire que l'axe x se retrouve vertical (ou presque à cause du bruit des accéléromètres)
• L'équation $\tan \theta _{123}={g_{x} \over {\sqrt {(g_{y}^{2}+g_{z}^{2})}}}$ est toujours stable dans le champ de gravitation

Le problème de l'instabilité de calcul est résolu dans la note d'application AN3461 et nous renvoyons le lecteur intéressé à sa lecture. Nous nous contentons de donner le résultat final :

Propriété

Les meilleures formules que nous pouvons espérer pour résoudre le problème de l'attitude avec l'accéléromètre sont :

• $\tan \theta _{123}={g_{x} \over {\sqrt {(g_{y}^{2}+g_{z}^{2})}}}$
• $\tan \phi _{123}={g_{y} \over {sign(g_{z}){\sqrt {(g_{z}^{2}+\mu g_{x}^{2})}}}}$

La présence de $\mu$ introduit une erreur mais stabilise les calculs. Une valeur entre 0.01 et 0.1 donne une bonne plage d'utilisation.

Ces deux angles font partie d'une rotation de Euler 123 et il est impossible de retrouver le troisième angle !

Mark Pedley introduit dans une autre section le calcul d'angle entre deux lectures de l'accéléromètre. Nous rappelons au lecteur qui lirait cette section que l'angle qui y est calculé n’est pas forcément l'angle de rotation 3D. Mark Pedley ne prétend rien sur ces angles et n'utilise pas ses formules sur des données d'accéléromètres. Pour nous, il serait plus sage de faire disparaître cette section de cette note d'application pour lever toute ambiguïté.

Voir aussi

• AN3461 : Tilt Sensing Using Linear Accelerometers by: Kimberly Tuck (2007)
• AN3461 : Tilt Sensing Using Linear Accelerometers by: Mark Pedley (2013)
• AN-1057 : Using an Accelerometer for Inclination Sensing by Christopher J. Fisher

Utiliser l'accéléromètre seul pour trouver l'attitude

Nous espérons montrer dans cette section que l'accéléromètre est un capteur lent mais surtout qu’il est sensible à l'accélération de pesanteur. C'est cette propriété du capteur que l’on va utiliser pour trouver l'attitude. En effet on sait que cette pesanteur est toujours dirigée vers le bas.

Une autre manière de dire les choses est que si la seule accélération est celle de la pesanteur (pas d'autres forces que le poids) alors la projection de cette accélération sur les trois axes permet trouver l'attitude. On réalisera ce genre de calcul plus tard.

Utiliser un Arduino pour lire le capteur

Comment câbler l'Arduino au capteur qui nous intéresse ?

Câblage

Le câblage d'un Arduino UNO avec le GY-521 se trouve ICI. On ne reproduira donc pas ce schéma.

Exercice 1

1°) Nous allons commencer par le programme tout simple :

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true); // request a total of 6 registers
  AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.println(AcZ);
  delay(333);
}

Essayez ce programme sur la liaison série.

La documentation du MPU 6050 dit que la valeur en g de l'accélération se trouve en divisant par 16384 (voir page 13). Êtes-vous d'accord avec la documentation ? Préciser votre méthode pour répondre à la question.

2°) Vous allez reprendre maintenant la mise en œuvre de la première question pour calculer pour chacun des axes la correction d'amplification ainsi que celle de l'offset. La correction d'amplification se trouve en mesurant la valeur de g pour un axe puis en retournant l'axe. La différence des deux valeurs divisée par 2*16384 donne le facteur de correction.

• Si celui-ci est proche de 1 on ne fera aucune correction d'amplification.
• L'offset sera pris entre les deux valeurs mesurées, pour que chacune des deux valeurs soit le plus proche possible de 16384 (au signe près).

Les valeurs trouvées sont à noter quelque part. Elles sont très dépendantes des circuits utilisés. Vous trouverez donc les nôtres dans les extraits de code donnés dans la suite, mais vous devrez y adapter les vôtres pour un fonctionnement correct.

Exercice 2

On va maintenant se servir de la position de l'accélération de pesanteur par rapport à nos axes pour essayer de trouver l'attitude de notre repère.

On va d’abord simplifier le problème en le ramenant en deux dimensions. L'accéléromètre que vous utilisez a ses deux axes X et Y clairement indiqués. Naturellement l'axe Z est perpendiculaire à ces deux axes.

On va s'intéresser à une rotation autour de l'axe Y (tangage). L'accéléromètre devra donc rester horizontal par rapport à l'axe X !!!

Projection de l'accélération de pesanteur sur deux axes X et Z

À l'aide de la figure explicative ci-dessous, on vous demande de retrouver l'angle de tangage de votre repère. Utilisez les résultats de la question 2° de l'exercice 1 pour affiner vos résultats.

Indication : Quelle est la relation entre les coordonnées ax et az et l'angle ? Le calcul d'une tangente inverse est en général mieux réalisé par la primitive atan2 que atan. Chercher la documentation sur ce point sur Internet.

Note : Il est possible que vous considériez l'axe Z dans l'autre sens ! Tout dépend comment vous tenez l'accéléromètre.

Solution

 #include<Wire.h>
    const int MPU=0x68;  // I2C address of the MPU-6050
    int16_t AcX,AcY,AcZ;
    float accel,ax,az,phi;
    void setup(){
      Wire.begin();
      Wire.beginTransmission(MPU);
      Wire.write(0x6B);  // PWR_MGMT_1 register
      Wire.write(0);     // set to zero (wakes up the MPU-6050)
      Wire.endTransmission(true);
      Serial.begin(9600);
    }
    void loop(){
      Wire.beginTransmission(MPU);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU,6,true);  // request a total of 6 registers
      AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
      AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
      Serial.print("AcX = "); Serial.print(AcX);
      Serial.print(" | AcY = "); Serial.print(AcY);
      Serial.print(" | AcZ = "); Serial.println(AcZ);
      // AcZ = 15000 au repos AcZ= -18600 à l’envers
      az = (float)(AcZ + 1400);
      //AcX = -15900 au repos AcX =17000
      ax = (float)(AcX - 500);
      accel = sqrt(ax*ax+az*az);
      // test d'immobilité
      if ((accel > 15200) && (accel < 17400)) { // valeur normale 16384
        phi = atan2(ax,az);
        Serial.print("Phi = "); Serial.println(phi);
      }
      delay(333);
    }

Remarque

Ce code pose problème dans le cas où les composantes de g sont nulles sur les deux axes X et Z (cela peut arriver : accéléromètre sur la tranche). Dans ce cas, ce qui est retourné par atan2 est NaN (Not a Number) et ne peut pas être interprété par un logiciel de visualisation. Il vous faut donc gérer cette situation sauf si vous vous interdisez de mettre l'accéléromètre dans cette situation.

 

Exercice 3

On vous demande de réaliser ce que montre la vidéo de cette page (ou directement dans youtube). Cela nécessite un servomoteur et un accéléromètre.

Utiliser Processing pour visualiser

On désire réaliser avec Processing une visualisation des données de l'exercice 2 précédent.

Exercice 4

On va chercher à visualiser concrètement les résultats de l'exercice précédent. On va naturellement commencer par le plus simple, la 2D.

Construire un programme simple de visualisation pour la question 1°) de l'exercice 2. Vous pouvez pour simplifier commencer par n'envoyer que l'angle par la liaison série et plus du tout les composantes de l'accélération.

Indication : Voici un programme capable de récupérer la chaîne "Phi = -0.67" envoyée par l'Arduino et de la transformer en nombre réel :

import processing.serial.*;

Serial port; // The serial port
int lf = 10; // Linefeed in ASCII
int serialCount=0;
float phi;
String myString=null;

void setup() {
// ne fonctionne pas sous Windows !!!
  String portName = "/dev/ttyACM0";
  port = new Serial(this, portName, 9600);
  frameRate(4);
}

void draw() {
  String floatString=null;
  while (port.available() > 0) {
    myString = port.readStringUntil(lf);
  }
  if (myString != null) {
  // On retire les 6 premiers caractères
    floatString = myString.substring(6);
    print(floatString);print(" converti en : ");
  // la fonction float() fait tout le boulot
    phi = float(floatString);
    println(phi);
  }
}

Solution

import processing.serial.*;
 
Serial port; // The serial port
int lf = 10; // Linefeed in ASCII
int serialCount=0;
float phi;
String myString=null;
 
void setup() {
    //size(100, 100, P3D);
    // get the first available port (use EITHER this OR the specific port code below)
    String portName = "/dev/ttyACM0";
 
    port = new Serial(this, portName, 9600);
    //frameRate(10);
    // black background
    background(0);
    //rectMode(CENTER);
    //rect(50,50,80,80);
}
 
void draw() {
  String floatString=null;
  // black background
  background(0);
  while (port.available() > 0) {
     myString = port.readStringUntil(lf);
  }
  if (myString != null) {
       floatString = myString.substring(6);
       print(floatString);print(" converti en : ");
       phi = float(floatString);
       println(phi);
       // black background
       background(0);
       translate(width/2, height/2);
       rotate(phi);
       rect(-26, -13, 52, 26);
       //rect(50,50,60,40);
   }  
}

 

À partir de maintenant, la partie processing sera fournie. Elle sera bien plus sophistiquée que ce que l’on vient de faire puisqu'elle sera destinée à recevoir les données de notre capteur et d’en faire plusieurs représentations 3D.

Exercice 5

Soit le programme Processing suivant :

Programme Processing pour visualisation

/**
 * Show GY521 Data.
 * 
 * Reads the serial port to get x- and y- axis rotational data from an accelerometer,
 * a gyroscope, and comeplementary-filtered combination of the two, and displays the
 * orientation data as it applies to three different colored rectangles.
 * It gives the z-orientation data as given by the gyroscope, but since the accelerometer
 * can't provide z-orientation, we don't use this data.
 * 
 */
 
import processing.serial.*;
 
Serial myPort;
short portIndex = 1;
int lf = 10; //ASCII linefeed
String inString; //String for testing serial communication
int calibrating;
 
float dt;
float x_gyr; //Gyroscope data
float y_gyr;
float z_gyr;
float x_acc; //Accelerometer data
float y_acc;
float z_acc;
float x_fil; //Filtered data
float y_fil;
float z_fil;
 
 
void setup() { 
// size(640, 360, P3D); 
  size(1400, 800, P3D);
  noStroke();
  colorMode(RGB, 256); 
 
// println("in setup");
// String portName = Serial.list()[portIndex];
  String portName = "/dev/ttyACM1";
// println(Serial.list());
// println(" Connecting to -> " + Serial.list()[portIndex]);
  myPort = new Serial(this, portName, 38400);
  myPort.clear();
  myPort.bufferUntil(lf);
} 
 
void draw_rect_rainbow() {
  scale(90);
  beginShape(QUADS);
 
  fill(0, 1, 1); vertex(-1,  1.5,  0.25);
  fill(1, 1, 1); vertex( 1,  1.5,  0.25);
  fill(1, 0, 1); vertex( 1, -1.5,  0.25);
  fill(0, 0, 1); vertex(-1, -1.5,  0.25);
 
  fill(1, 1, 1); vertex( 1,  1.5,  0.25);
  fill(1, 1, 0); vertex( 1,  1.5, -0.25);
  fill(1, 0, 0); vertex( 1, -1.5, -0.25);
  fill(1, 0, 1); vertex( 1, -1.5,  0.25);
 
  fill(1, 1, 0); vertex( 1,  1.5, -0.25);
  fill(0, 1, 0); vertex(-1,  1.5, -0.25);
  fill(0, 0, 0); vertex(-1, -1.5, -0.25);
  fill(1, 0, 0); vertex( 1, -1.5, -0.25);
 
  fill(0, 1, 0); vertex(-1,  1.5, -0.25);
  fill(0, 1, 1); vertex(-1,  1.5,  0.25);
  fill(0, 0, 1); vertex(-1, -1.5,  0.25);
  fill(0, 0, 0); vertex(-1, -1.5, -0.25);
 
  fill(0, 1, 0); vertex(-1,  1.5, -0.25);
  fill(1, 1, 0); vertex( 1,  1.5, -0.25);
  fill(1, 1, 1); vertex( 1,  1.5,  0.25);
  fill(0, 1, 1); vertex(-1,  1.5,  0.25);
 
  fill(0, 0, 0); vertex(-1, -1.5, -0.25);
  fill(1, 0, 0); vertex( 1, -1.5, -0.25);
  fill(1, 0, 1); vertex( 1, -1.5,  0.25);
  fill(0, 0, 1); vertex(-1, -1.5,  0.25);
 
  endShape();
 
 
}
 
void draw_rect(int r, int g, int b) {
  scale(90);
  beginShape(QUADS);
 
  fill(r, g, b);
  vertex(-1,  1.5,  0.25);
  vertex( 1,  1.5,  0.25);
  vertex( 1, -1.5,  0.25);
  vertex(-1, -1.5,  0.25);
 
  vertex( 1,  1.5,  0.25);
  vertex( 1,  1.5, -0.25);
  vertex( 1, -1.5, -0.25);
  vertex( 1, -1.5,  0.25);
 
  vertex( 1,  1.5, -0.25);
  vertex(-1,  1.5, -0.25);
  vertex(-1, -1.5, -0.25);
  vertex( 1, -1.5, -0.25);
 
  vertex(-1,  1.5, -0.25);
  vertex(-1,  1.5,  0.25);
  vertex(-1, -1.5,  0.25);
  vertex(-1, -1.5, -0.25);
 
  vertex(-1,  1.5, -0.25);
  vertex( 1,  1.5, -0.25);
  vertex( 1,  1.5,  0.25);
  vertex(-1,  1.5,  0.25);
 
  vertex(-1, -1.5, -0.25);
  vertex( 1, -1.5, -0.25);
  vertex( 1, -1.5,  0.25);
  vertex(-1, -1.5,  0.25);
 
  endShape();
}
 
void draw() { 
 
  background(0);
  lights();
 
  // Tweak the view of the rectangles
  int distance = 50;
  int x_rotation = 90;
 
  //Show gyro data
  pushMatrix(); 
  translate(width/6, height/2, -50); 
  rotateX(radians(-x_gyr - x_rotation));
  rotateY(radians(-y_gyr));
  draw_rect(249, 250, 50);
 
  popMatrix(); 
 
  //Show accel data
  pushMatrix();
  translate(width/2, height/2, -50);
  rotateX(radians(-x_acc - x_rotation));
  rotateY(radians(-y_acc));
  draw_rect(56, 140, 206);
  popMatrix();
 
  //Show combined data
  pushMatrix();
  translate(5*width/6, height/2, -50);
  rotateX(radians(-x_fil - x_rotation));
  rotateY(radians(-y_fil));
  draw_rect(93, 175, 83);
  popMatrix();
 
  textSize(24);
  String accStr = "(" + (int) x_acc + ", " + (int) y_acc + ")";
  String gyrStr = "(" + (int) x_gyr + ", " + (int) y_gyr + ")";
  String filStr = "(" + (int) x_fil + ", " + (int) y_fil + ")";
 
 
  fill(249, 250, 50);
  text("Gyroscope", (int) width/6.0 - 60, 25);
  text(gyrStr, (int) (width/6.0) - 40, 50);
 
  fill(56, 140, 206);
  text("Accelerometer", (int) width/2.0 - 50, 25);
  text(accStr, (int) (width/2.0) - 30, 50); 
 
  fill(83, 175, 93);
  text("Combination", (int) (5.0*width/6.0) - 40, 25);
  text(filStr, (int) (5.0*width/6.0) - 20, 50);
 
} 
 
void serialEvent(Serial p) {
 
  inString = (myPort.readString());
 
  try {
    // Parse the data
    String[] dataStrings = split(inString, '#');
    for (int i = 0; i < dataStrings.length; i++) {
      String type = dataStrings[i].substring(0, 4);
      String dataval = dataStrings[i].substring(4);
    if (type.equals("DEL:")) {
        dt = float(dataval);
        /*
        print("Dt:");
        println(dt);
        */
 
      } else if (type.equals("ACC:")) {
        String data[] = split(dataval, ',');
        x_acc = float(data[0]);
        y_acc = float(data[1]);
        z_acc = float(data[2]);
        /*
        print("Acc:");
        print(x_acc);
        print(",");
        print(y_acc);
        print(",");
        println(z_acc);
        */
      } else if (type.equals("GYR:")) {
        String data[] = split(dataval, ',');
        x_gyr = float(data[0]);
        y_gyr = float(data[1]);
        z_gyr = float(data[2]);
      } else if (type.equals("FIL:")) {
        String data[] = split(dataval, ',');
        x_fil = float(data[0]);
        y_fil = float(data[1]);
        z_fil = float(data[2]);
      }
    }
  } catch (Exception e) {
      println("Caught Exception");
  }
}

C'est le programme que l’on utilisera systématiquement par la suite. Il peut être trouvé dans la partie à télécharger de cette page.

Ce programme est sensible au type de liaison série utilisé et particulièrement au système d'exploitation. La ligne en début de programme :

String portName = "/dev/ttyACM0"; // parfois "/dev/ttyACM1" chez moi

est propre à Linux et devra donc être adaptée !

Remarque

Ce programme utilise la représentation d'Euler des rotations. Ceci peut être vu avec l'extrait de code

rotateX(radians(-x_fil - x_rotation));
rotateY(radians(-y_fil));

La rotation autour de l'axe Z est donc supposée être nulle. Ceci semble assez normal pour l'accéléromètre mais on peut faire mieux pour les rotations (gyroscope). Nous serons donc éventuellement amenés à modifier ceci.

1°) Vous allez vous intéresser à "void serialEvent(Serial p)" qui est appelé chaque fois qu'un événement a lieu sur la liaison série. Son objectif est de recevoir et de séparer les données. Si l’on veut utiliser le programme tel quel, il faut lui fournir des données correctes qu’il interprétera. Pour cela il faut remarquer que trois parallélépipèdes sont dessinés :

• celui de gauche pour interpréter les données du gyroscope
• celui du milieu pour interpréter les données de l'accéléromètre
• celui de droite pour interpréter les données des deux capteurs à la fois (fusion de données)

Pour cette question, on vous demande d'envoyer toutes les données à zéro (nombres réels avec deux chiffres après la virgule) sauf celles de l'accéléromètre. Essayez de réaliser une rotation progressive du parallélépipède du centre (sans travailler sur l'accélération de pesanteur).

Solution

float accel_angle_x=0.0,accel_angle_y=0.0,accel_angle_z=0.0;
 
void setup()
{
  // put your setup code here, to run once:
  Serial.begin(38400);
}
 
void loop()
{
  // put your main code here, to run repeatedly:
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(5, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(accel_angle_x, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_z, 2);
  Serial.print(F("#GYR:"));
  Serial.print(0.0, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));
  accel_angle_x += 0.1;
  accel_angle_y += 0.1;
  accel_angle_z += 0.1;
  // Delay so we don't swamp the serial port
  //delay(5);
  delay(300);
}

 

2°) Construire ensuite le même programme en vous intéressant maintenant à la position de l'accélération de pesanteur. Pour simplifier commencez par faire fonctionner le programme Processing avec la résolution 2D que vous avez réalisé dans l'exercice précédant. Les données que vous allez envoyer au programme processing seront maintenant liées à votre position de l'accéléromètre par rapport à l'axe x. L'angle doit être fourni en ° et non en radians !

Solution

#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ;
float accel_angle_x=0.0,accel_angle_y=0.0,accel_angle_z=0.0;
float az,ax,accel;
void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(38400);
}
 
void loop()
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);  // request a total of 6 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  // AcZ = 15000 au repos AcZ= -18600 à l’envers
  az = (float)(AcZ + 1400);
  //AcX = -15900 au repos AcX =17000
  ax = (float)(AcX - 500);
  accel = sqrt(ax*ax+az*az);
  // test d'immobilité
  if ((accel > 15200) && (accel < 17400)) { // valeur normale 16384
        accel_angle_x = atan2(ax,az);
  }
 
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(500, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(accel_angle_x*180/3.14, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_z, 2);
  Serial.print(F("#GYR:"));
  Serial.print(0.0, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));
  delay(30);
}

 

Retrouver l'attitude avec l'accélération de pesanteur

3°) Essayez de généraliser pour trois dimensions avec les formules présentées dans cette section. Elles sont rappelées maintenant : $\tan \phi ={A_{y} \over {\sqrt {(A_{x}^{2}+A_{z}^{2})}}}$

$\tan \rho ={A_{x} \over {\sqrt {(A_{y}^{2}+A_{z}^{2})}}}$

$\tan \theta ={{\sqrt {(A_{x}^{2}+A_{y}^{2})}} \over A_{z}}$

Seules les deux premières seront utilisées.

N'oubliez pas de réaliser tout en ° ! C'est la figure ci-contre qui nous montre ce que l’on cherche à réaliser.

Solution

L'utilisation des deux premières formules peut laisser croire que le problème est résolu... mais une étude expérimentale attentive peut nous conduite à de fortes divergences entre la position de l'accéléromètre dans notre main et sa visualisation 3D avec Processing.

#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ;
float accel_angle_x=0.0,accel_angle_y=0.0,accel_angle_z=0.0;
float az,ay,ax,accel;
void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(38400);
}
 
void loop()
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);  // request a total of 6 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  // AcZ = 15000 au repos AcZ= -18600 à l’envers
  az = (float)(AcZ + 1400);
  //AcX = -15900 au repos AcX =17000
  ax = (float)(AcX - 500);
  // AcY 
  ay = (float)(AcY);
 
  accel = sqrt(ax*ax+az*az+ay*ay);
  // test d'immobilité à améliorer !!!! : en commentaire pour le moment
// if ((accel > 15200) && (accel < 17400)) { // valeur normale 16384
        accel_angle_x = atan2(ax,sqrt(az*az+ay*ay));
        accel_angle_y = atan2(ay,sqrt(az*az+ax*ax));
        accel_angle_z = atan2(sqrt(ay*ay+ax*ax),az);
    // on écrase la troisième valeur comme demandé
        accel_angle_z = 0.0;
// }
 
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(500, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(accel_angle_x*180/3.14, 2);
  Serial.print(F(","));
  Serial.print(-accel_angle_y*180/3.14, 2); //moins pour correspondre à l'affichage
  Serial.print(F(","));
  Serial.print(accel_angle_z*180/3.14, 2);
  Serial.print(F("#GYR:"));
  Serial.print(0.0, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));
  // Delay so we don't swamp the serial port
  //delay(5);
  delay(30);
}

 

Améliorer les données de l'accéléromètre avec un filtrage

Une amélioration possible consiste à réaliser un filtre passe-bas pour filter les données de l'accélération. Vous avez, en effet, certainement noté la "nervosité" de la réponse de l'accéléromètre et un filtrage moyenneur s'impose donc de lui-même.

4°) Réaliser le filtrage passe-bas en prenant les contraintes suivantes :

• réalisation de la boucle "loop()" environ 100 fois par seconde
• réalisation du filtre passe-pas avec "angle = (0.75)*(angle) + (0.25)*(nouveau_calcul);"

Remarque

Le 100 fois par seconde est associé aux coefficients 0.75 et 0.25 présents dans cette équation. Un changement ne pourra se faire que sur les deux valeurs associées et nécessite une connaissance sur les filtres numériques.

Solution

#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ;
float accel_angle_x=0.0,accel_angle_y=0.0,accel_angle_z=0.0;
float az,ay,ax,accel;
void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(38400);
}
 
void loop()
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);  // request a total of 6 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  // AcZ = 15000 au repos AcZ= -18600 à l’envers
  az = (float)(AcZ + 1400);
  //AcX = -15900 au repos AcX =17000
  ax = (float)(AcX - 500);
  // AcY 
  ay = (float)(AcY);
 
  accel = sqrt(ax*ax+az*az+ay*ay);
// avec filtrage
        accel_angle_x = 0.75*accel_angle_x+0.25*atan2(ax,sqrt(az*az+ay*ay));
        accel_angle_y = 0.75*accel_angle_y+0.25*atan2(ay,sqrt(az*az+ax*ax));
        accel_angle_z = 0.75*accel_angle_z+0.25*atan2(sqrt(ay*ay+ax*ax),az);
  // on écrase la troisième valeur comme demandé
        accel_angle_z = 0.0;
 
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(500, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(accel_angle_x*180/3.14, 2);
  Serial.print(F(","));
  Serial.print(-accel_angle_y*180/3.14, 2);  //moins pour correspondre à l'affichage
  Serial.print(F(","));
  Serial.print(accel_angle_z*180/3.14, 2);
  Serial.print(F("#GYR:"));
  Serial.print(0.0, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));
  // Delay so we don't swamp the serial port
  delay(9);
  //delay(30);
}

 

Complément sur l'exercice 5

la majorité du code dans cette section est liée à un optimisme excessif de l'auteur. Il comporte de grosses erreurs et finira par disparaître ou sera fortement modifié !

Nous donnons maintenant un code qui calcule au mieux l'attitude à partir des données de l'accéléromètre. Nous le donnons tout fait pour ceux qui voudraient l'essayer.

Code pour retrouver l'attitude avec l'accéléromètre

#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ;
float euler_x=0.0,euler_y=0.0,euler_z=0.0,accel_angle_x,accel_angle_y;
float a[3],accel,axis[3],angle,g0[3]={0,0,1};
// rotation matrix :
float R[3][3]; 

//get modulus of a 3d vector sqrt(x^2+y^2+y^2)
float vector3d_modulus(float* vector){
        static float R;  
        R = vector[0]*vector[0];
        R += vector[1]*vector[1];
        R += vector[2]*vector[2];
        return sqrt(R);
}

//convert vector to a vector with same direction and modulus 1
void vector3d_normalize(float* vector){
        static float R;  
        R = vector3d_modulus(vector);
        vector[0] /= R;
        vector[1] /= R;
        vector[2] /= R;  
}

//calcuate vector dot-product c = a . b
float vector3d_dot(float* a,float* b){
        return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}

//calcuate vector cross-product c = a x b
void vector3d_cross(float* a,float* b, float* c){
        c[0] = a[1]*b[2] - a[2]*b[1];
        c[1] = a[2]*b[0] - a[0]*b[2];
        c[2] = a[0]*b[1] - a[1]*b[0];
}

void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(38400);
}
 
void loop()
{
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);  // request a total of 6 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  // AcZ = 15000 au repos AcZ= -18600 à l’envers
  a[2] = (float)(AcZ + 1400);
  //AcX = -15900 au repos AcX =17000
  a[0] = (float)(AcX - 500);
  // AcY 
  a[1] = (float)(AcY);

//*********** anciens calculs Exercice 5
        accel_angle_x = atan2(a[0],sqrt(a[2]*a[2]+a[1]*a[1]))*180/3.14159;
        accel_angle_y = atan2(a[1],sqrt(a[2]*a[2]+a[0]*a[0]))*180/3.14159; 
//*********** fin anciens calculs

//*********** Notre méthode plus complexe mais plus rigoureuse mais fausse !!!!
  vector3d_normalize(a);
// axis calculation : c’est ici qu’il y a erreur comme montré dans la section mathématique
  vector3d_cross(g0,a,axis); 
  angle = atan2(vector3d_modulus(axis),vector3d_dot(a,g0));
  vector3d_normalize(axis);

// Construction de la matrice de rotation
    R[0][0] = cos(angle)+axis[0]*axis[0]*(1-cos(angle));
    R[1][0] = axis[1]*axis[0]*(1-cos(angle))+axis[2]*sin(angle);
    R[2][0] = axis[2]*axis[0]*(1-cos(angle))-axis[1]*sin(angle);
    R[0][1] = axis[0]*axis[1]*(1-cos(angle))-axis[2]*sin(angle);
    R[1][1] = cos(angle)+axis[1]*axis[1]*(1-cos(angle));
    R[2][1] = axis[2]*axis[1]*(1-cos(angle))+axis[0]*sin(angle);
    R[0][2] = axis[0]*axis[2]*(1-cos(angle))+axis[1]*sin(angle);
    R[1][2] = axis[1]*axis[2]*(1-cos(angle))-axis[0]*sin(angle);
    R[2][2] = cos(angle)+axis[2]*axis[2]*(1-cos(angle));
 
// Il nous faut retrouver Euler 321
  euler_y = asin(-R[0][2]); // -pi/2 < angle < +pi/2
  euler_z = atan2(R[0][1],R[0][0]); // -pi < angle < +pi
  euler_x = atan2(R[1][2],R[2][2]); // -pi < angle < +pi
 
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(500, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(-euler_y*180/3.14, 2);
  Serial.print(F(","));
  Serial.print(euler_x*180/3.14, 2); //moins pour correspondre à l'affichage
  Serial.print(F(","));
  Serial.print(euler_z*180/3.14, 2);
  Serial.print(F("#GYR:"));
  Serial.print(accel_angle_x, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));
  delay(5);
}

Le calcul de la matrice de rotation R doit être amélioré : tel qu’il est écrit ici il fait appel plusieurs fois à des cosinus et sinus identiques.

Ce code montre qu’il est possible de faire un peu mieux que ce l’on a fait dans l'exercice 4 mais de manière bien plus complexe.

Ce travail montre qu’il nous faut ajouter un capteur supplémentaire si l’on veut retrouver correctement l'attitude.

Deuxième complément sur l'exercice 5

On vous demande maintenant d'implanter ce qui est présenté dans cette section. Les formules mathématiques à implanter deviennent donc :

• $\tan \theta _{123}={g_{x} \over {\sqrt {(g_{y}^{2}+g_{z}^{2})}}}$
• $\tan \phi _{123}={g_{y} \over {sign(g_{z}){\sqrt {(g_{z}^{2}+\mu g_{x}^{2})}}}}$

Utiliser le gyroscope seul pour trouver l'attitude

Nous espérons montrer dans cette section que le gyromètre est un capteur rapide mais qu’il possède un inconvénient, c’est qu’il dérive !

Nous allons aussi garder la partie Processing de la section précédente puisque la représentation du problème graphique est la même.

Un programme Arduino pour commencer

On donne :

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain

#include<Wire.h>

const int MPU=0x68; // I2C address of the MPU-6050
int16_t GyX,GyY,GyZ;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // starting with register 0x43 (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true); // request a total of 14 registers
  GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Serial.print("GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
}

Exercice 6

1°) Montrer que ce programme fonctionne.

Un gyroscope est sensible à la vitesse de rotation (et non pas à l'angle) !!! D'où la nécessité de calcul de l'intégrale pour obtenir l'angle.

2°) Réaliser une intégration des vitesses angulaires (comment faire ?) pour trouver les angles.

Utiliser le gyroscope pour retrouver l'attitude

L'image ci-contre montre ce que nous cherchons à faire à ce stade.

Indications :

i) On rappelle que l'opérateur intégration est un opérateur qui peut poser un certain nombre de problèmes. Un des plus important est l'intégration de la moyenne (qui n'est jamais nulle même sur un bon GBF !!!) et qui finit par saturer la sortie. Il faudra à tout prix développer la phase de calibration que l’on a un peu laissé tomber jusqu'ici. Un moyen est qu'au départ du programme le GY-521 soit au repos et que l’on effectue une dizaine de mesures sur tous les axes pour en faire une moyenne. Cette moyenne sera considérée comme un décalage à soustraire.

ii) La sortie sera réalisée par quelque chose comme :

// Send the data to the serial port
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(dT, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#GYR:"));
  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_z, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));

si l’on veut rester compatible avec Processing.

iii) Réaliser l'intégration par la formule récursive : "angle = angle + gyro*dt" pour chacun des angles apparaissant dans le programme ci-dessus. "dt" sera obtenu avec la primitive "millis()" qui comme son nom l'indique retourne des millisecondes.

iv) L'obtention de la vitesse en °/s se fait par une division à l'aide d'une valeur

// Convert gyro values to degrees/sec
  float FS_SEL = 131;

Solution trouvée sur Internet mais fausse

#include<Wire.h>
 const int MPU=0x68; // I2C address of the MPU-6050
int16_t GyX,GyY,GyZ;
unsigned long last_read_time;
float last_gyro_x_angle=0; // Store the gyro angles to compare drift
float last_gyro_y_angle=0;
float last_gyro_z_angle=0;
float base_x_gyro;
float base_y_gyro;
float base_z_gyro;
float unfiltered_gyro_angle_x=0,unfiltered_gyro_angle_y=0;
float unfiltered_gyro_angle_z=0;
 
void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  calibrate_gyro();
  Serial.begin(38400);
  last_read_time = millis(); // utilisé dans loop
}

void calibrate_gyro() {
 float x_gyro = 0, y_gyro = 0, z_gyro = 0; 
 for (int i=0;i<10;i++) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43); // starting with register 0x43 (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU,6,true); // request a total of 14 registers
    GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
    x_gyro += (float) GyX;
    y_gyro += (float) GyY;
    z_gyro += (float) GyZ;
    delay(30);
 }
 base_x_gyro = x_gyro / 10.0;
 base_y_gyro = y_gyro / 10.0;
 base_z_gyro = z_gyro / 10.0;
}
   
void loop(){
  // Convert gyro values to degrees/sec
  float FS_SEL = 131;
  unsigned long t_now;
  float dT;
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // starting with register 0x43 (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true); // request a total of 14 registers
  GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  // calibration du temps
  t_now = millis();
  dT = (float)(t_now - last_read_time)/1000;
  last_read_time = t_now;
  // intégration récursive (une des plus mauvaise)
  unfiltered_gyro_angle_x += ((float) (GyX - base_x_gyro))*dT/FS_SEL;
  unfiltered_gyro_angle_y += ((float) (GyY - base_y_gyro))*dT/FS_SEL;
  unfiltered_gyro_angle_z += ((float) (GyZ - base_z_gyro))*dT/FS_SEL;
  // Send the data to the serial port
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(dT, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#GYR:"));
  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_z, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));
  delay(30);
}

 

Remarque

Cette page est en construction... et après plusieurs mois, nous venons de découvrir que la formule d'intégration utilisée dans le code ci-dessus est fausse. Elle n'est vraie qu'en deux dimensions mais pas en trois dimensions !!!

Nous avons commenté ce code dans la page de l'auteur (du code) qui n'a pas semblé être convaincu par les arguments que nous avons avancés.

Voici donc un nouveau programme qui respecte les mathématiques des rotations 3D. Il est destiné à comparer l'intégration naïve ci-dessus à celle que l’on est sensé faire.

Solution

#include<Wire.h>

//get modulus of a 3d vector sqrt(x^2+y^2+y^2)
float vector3d_modulus(float* vector){
        static float R;  
        R = vector[0]*vector[0];
        R += vector[1]*vector[1];
        R += vector[2]*vector[2];
        return sqrt(R);
}

//convert vector to a vector with same direction and modulus 1
void vector3d_normalize(float* vector){
        static float R;  
        R = vector3d_modulus(vector);
        vector[0] /= R;
        vector[1] /= R;
        vector[2] /= R;  
}

//calcuate vector dot-product c = a . b
float vector3d_dot(float* a,float* b){
        return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}


//calcuate vector cross-product c = a x b
void vector3d_cross(float* a,float* b, float* c){
        c[0] = a[1]*b[2] - a[2]*b[1];
        c[1] = a[2]*b[0] - a[0]*b[2];
        c[2] = a[0]*b[1] - a[1]*b[0];
}

//calcuate vector scalar-product n = s x a
void vector3d_scale(float s, float* a , float* b){
        b[0] = s*a[0];
        b[1] = s*a[1];
        b[2] = s*a[2];
}


//calcuate vector sum c = a + b
void vector3d_add(float* a , float* b, float* c){
        c[0] = a[0] + b[0];
        c[1] = a[1] + b[1];
        c[2] = a[2] + b[2];
}


//creates equivalent skew symetric matrix plus identity
//for v = {x,y,z} returns
// m = {{1,-z,y}
// {z,1,-x}
// {-y,x,1}}
void vector3d_skew_plus_identity(float *v,float* m){
        m[0*3+0]=1;
        m[0*3+1]=-v[2];
        m[0*3+2]=v[1];
        m[1*3+0]=v[2];
        m[1*3+1]=1;
        m[1*3+2]=-v[0];
        m[2*3+0]=-v[1];
        m[2*3+1]=v[0];
        m[2*3+2]=1;
}


const int MPU=0x68; // I2C address of the MPU-6050
int16_t GyX,GyY,GyZ;
unsigned long last_read_time;
float last_gyro_x_angle=0; // Store the gyro angles to compare drift
float last_gyro_y_angle=0;
float last_gyro_z_angle=0;
float base_x_gyro;
float base_y_gyro;
float base_z_gyro;
float unfiltered_gyro_angle_x=0,unfiltered_gyro_angle_y=0;
float unfiltered_gyro_angle_z=0;
 
void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  calibrate_gyro();
  Serial.begin(38400);
  last_read_time = millis(); // utilisé dans loop
}

void calibrate_gyro() {
 float x_gyro = 0, y_gyro = 0, z_gyro = 0; 
 for (int i=0;i<10;i++) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43); // starting with register 0x43 (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU,6,true); // request a total of 14 registers
    GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
    x_gyro += (float) GyX;
    y_gyro += (float) GyY;
    z_gyro += (float) GyZ;
    delay(30);
 }
 base_x_gyro = x_gyro / 10.0;
 base_y_gyro = y_gyro / 10.0;
 base_z_gyro = z_gyro / 10.0;
}

void dcm_orthonormalize(float dcm[3][3]){
        //err = X . Y ,  X = X - err/2 * Y , Y = Y - err/2 * X  (DCMDraft2 Eqn.19)
        float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
        float delta[2][3];
        vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0]));
        vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1]));
        vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0]));
        vector3d_add((float*)(dcm[1]),(float*)(delta[0]),(float*)(dcm[1]));

        //Z = X x Y  (DCMDraft2 Eqn. 20) ,
        vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2]));
        //re-nomralization
        vector3d_normalize((float*)(dcm[0]));
        vector3d_normalize((float*)(dcm[1]));
        vector3d_normalize((float*)(dcm[2]));
}

//rotate DCM matrix by a small rotation given by angular rotation vector w
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
void dcm_rotate(float dcm[3][3], float w[3]){
        //float W[3][3];        
        //creates equivalent skew symetric matrix plus identity matrix
        //vector3d_skew_plus_identity((float*)w,(float*)W);
        //float dcmTmp[3][3];
        //matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);
       
        int i;
        float dR[3];
        //update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
        for(i=0;i<3;i++){
                vector3d_cross(w,dcm[i],dR);
                vector3d_add(dcm[i],dR,dcm[i]);
        }              

        //make matrix orthonormal again
        dcm_orthonormalize(dcm);
}

float dcmGyro[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; //dcm matrix according to gyroscopes
//---------------
//dcmGyro
//---------------
float w[3]; 

   
void loop(){
  // Convert gyro values to degrees/sec
  float FS_SEL = 131;
  unsigned long t_now;
  float dT,euler_x,euler_y,euler_z;
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // starting with register 0x43 (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true); // request a total of 14 registers
  GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  // calibration du temps
  t_now = millis();
  dT = (float)(t_now - last_read_time)/1000;
  last_read_time = t_now;
  // Debra intégration 
  unfiltered_gyro_angle_x += ((float) (GyX - base_x_gyro))*dT/FS_SEL;
  unfiltered_gyro_angle_y += ((float) (GyY - base_y_gyro))*dT/FS_SEL;
  unfiltered_gyro_angle_z += ((float) (GyZ - base_z_gyro))*dT/FS_SEL;
  //New rotations matrix calculation
  w[0] = ((float) (GyX - base_x_gyro))*dT/FS_SEL;
  w[1] = ((float) (GyY - base_y_gyro))*dT/FS_SEL;
  w[2] = ((float) (GyZ - base_z_gyro))*dT/FS_SEL;
  // °/s to radian/s conversion
  w[0]= w[0] * 3.14159/180;
  w[1]= w[1] * 3.14159/180;
  w[2]= w[2] * 3.14159/180;
  dcm_rotate(dcmGyro,w);
  // Euler 321
  euler_y = asin(dcmGyro[0][2]); // -pi/2 < angle < +pi/2
  euler_z = atan2(-dcmGyro[0][1],dcmGyro[0][0]); // -pi < angle < +pi
  euler_x = atan2(-dcmGyro[1][2],dcmGyro[2][2]); // -pi < angle < +pi
  // Send the data to the serial port
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(dT, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F("#GYR:"));
  Serial.print(-unfiltered_gyro_angle_y, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_x, 2);
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_z, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(-euler_y, 2);
  Serial.print(F(","));
  Serial.print(euler_x, 2);
  Serial.print(F(","));
  Serial.print(euler_z, 2);
  Serial.println(F(""));
  delay(30);
}

Il a été testé avec le programme Processing très légèrement modifié pour l’occasion :

/**
 * Show GY521 Data.
 * 
 * Reads the serial port to get x- and y- axis rotational data from an accelerometer,
 * a gyroscope, and comeplementary-filtered combination of the two, and displays the
 * orientation data as it applies to three different colored rectangles.
 * It gives the z-orientation data as given by the gyroscope, but since the accelerometer
 * can't provide z-orientation, we don't use this data.
 * 
 */
 
import processing.serial.*;

Serial myPort;
short portIndex = 1;
int lf = 10; //ASCII linefeed
String inString; //String for testing serial communication
int calibrating;
 
float dt;
float x_gyr; //Gyroscope data
float y_gyr;
float z_gyr;
float x_acc; //Accelerometer data
float y_acc;
float z_acc;
float x_fil; //Filtered data
float y_fil;
float z_fil;

 
void setup() { 
// size(640, 360, P3D); 
  size(1400, 800, P3D);
  noStroke();
  colorMode(RGB, 256); 
 
// println("in setup");
// String portName = Serial.list()[portIndex];
  String portName = "/dev/ttyACM0";
// println(Serial.list());
// println(" Connecting to -> " + Serial.list()[portIndex]);
  myPort = new Serial(this, portName, 38400);
  myPort.clear();
  myPort.bufferUntil(lf);
} 

void draw_rect_rainbow() {
  scale(90);
  beginShape(QUADS);

  fill(0, 1, 1); vertex(-1,  1.5,  0.25);
  fill(1, 1, 1); vertex( 1,  1.5,  0.25);
  fill(1, 0, 1); vertex( 1, -1.5,  0.25);
  fill(0, 0, 1); vertex(-1, -1.5,  0.25);

  fill(1, 1, 1); vertex( 1,  1.5,  0.25);
  fill(1, 1, 0); vertex( 1,  1.5, -0.25);
  fill(1, 0, 0); vertex( 1, -1.5, -0.25);
  fill(1, 0, 1); vertex( 1, -1.5,  0.25);

  fill(1, 1, 0); vertex( 1,  1.5, -0.25);
  fill(0, 1, 0); vertex(-1,  1.5, -0.25);
  fill(0, 0, 0); vertex(-1, -1.5, -0.25);
  fill(1, 0, 0); vertex( 1, -1.5, -0.25);

  fill(0, 1, 0); vertex(-1,  1.5, -0.25);
  fill(0, 1, 1); vertex(-1,  1.5,  0.25);
  fill(0, 0, 1); vertex(-1, -1.5,  0.25);
  fill(0, 0, 0); vertex(-1, -1.5, -0.25);

  fill(0, 1, 0); vertex(-1,  1.5, -0.25);
  fill(1, 1, 0); vertex( 1,  1.5, -0.25);
  fill(1, 1, 1); vertex( 1,  1.5,  0.25);
  fill(0, 1, 1); vertex(-1,  1.5,  0.25);

  fill(0, 0, 0); vertex(-1, -1.5, -0.25);
  fill(1, 0, 0); vertex( 1, -1.5, -0.25);
  fill(1, 0, 1); vertex( 1, -1.5,  0.25);
  fill(0, 0, 1); vertex(-1, -1.5,  0.25);

  endShape();
  
  
}

void draw_rect(int r, int g, int b) {
  scale(90);
  beginShape(QUADS);
  
  fill(r, g, b);
  vertex(-1,  1.5,  0.25);
  vertex( 1,  1.5,  0.25);
  vertex( 1, -1.5,  0.25);
  vertex(-1, -1.5,  0.25);

  vertex( 1,  1.5,  0.25);
  vertex( 1,  1.5, -0.25);
  vertex( 1, -1.5, -0.25);
  vertex( 1, -1.5,  0.25);

  vertex( 1,  1.5, -0.25);
  vertex(-1,  1.5, -0.25);
  vertex(-1, -1.5, -0.25);
  vertex( 1, -1.5, -0.25);

  vertex(-1,  1.5, -0.25);
  vertex(-1,  1.5,  0.25);
  vertex(-1, -1.5,  0.25);
  vertex(-1, -1.5, -0.25);

  vertex(-1,  1.5, -0.25);
  vertex( 1,  1.5, -0.25);
  vertex( 1,  1.5,  0.25);
  vertex(-1,  1.5,  0.25);

  vertex(-1, -1.5, -0.25);
  vertex( 1, -1.5, -0.25);
  vertex( 1, -1.5,  0.25);
  vertex(-1, -1.5,  0.25);

  endShape();
  
  
}

void draw() { 
  
  background(0);
  lights();
    
  // Tweak the view of the rectangles
  int distance = 50;
  int x_rotation = 90;
  
  //Show gyro data
  pushMatrix(); 
  translate(width/6, height/2, -50); 
  rotateX(radians(-x_gyr - x_rotation));
  rotateY(radians(-y_gyr));
  rotateZ(radians(-z_gyr));
  draw_rect(249, 250, 50);
  
  popMatrix(); 

  //Show accel data
  pushMatrix();
  translate(width/2, height/2, -50);
  rotateX(radians(-x_acc - x_rotation));
  rotateY(radians(-y_acc));
  //rotateZ(radians(-z_acc));
  draw_rect(56, 140, 206);
  popMatrix();
  
  //Show combined data
  pushMatrix();
  translate(5*width/6, height/2, -50);
  // old:rotateX(radians(-x_fil - x_rotation));
  //rotateX(radians(-x_fil));
  //rotateY(radians(-y_fil));
  //rotateZ(radians(-z_fil));
  rotateX(x_fil- 3.14159/2.0);
  rotateY(y_fil);
  rotateZ(z_fil);
  draw_rect(93, 175, 83);
  popMatrix();
 
  textSize(24);
  String accStr = "(" + (int) x_acc + ", " + (int) y_acc + ")";
  String gyrStr = "(" + (int) x_gyr + ", " + (int) y_gyr + ")";
  String filStr = "(" + (int) x_fil + ", " + (int) y_fil + ")";
 

  fill(249, 250, 50);
  text("Gyroscope", (int) width/6.0 - 60, 25);
  text(gyrStr, (int) (width/6.0) - 40, 50);

  fill(56, 140, 206);
  text("Accelerometer", (int) width/2.0 - 50, 25);
  text(accStr, (int) (width/2.0) - 30, 50); 
  
  fill(83, 175, 93);
  text("Combination", (int) (5.0*width/6.0) - 40, 25);
  text(filStr, (int) (5.0*width/6.0) - 20, 50);

} 

void serialEvent(Serial p) {

  inString = (myPort.readString());
  
  try {
    // Parse the data
    String[] dataStrings = split(inString, '#');
    for (int i = 0; i < dataStrings.length; i++) {
      String type = dataStrings[i].substring(0, 4);
      String dataval = dataStrings[i].substring(4);
    if (type.equals("DEL:")) {
        dt = float(dataval);
        /*
        print("Dt:");
        println(dt);
        */
        
      } else if (type.equals("ACC:")) {
        String data[] = split(dataval, ',');
        x_acc = float(data[0]);
        y_acc = float(data[1]);
        z_acc = float(data[2]);
        /*
        print("Acc:");
        print(x_acc);
        print(",");
        print(y_acc);
        print(",");
        println(z_acc);
        */
      } else if (type.equals("GYR:")) {
        String data[] = split(dataval, ',');
        x_gyr = float(data[0]);
        y_gyr = float(data[1]);
        z_gyr = float(data[2]);
      } else if (type.equals("FIL:")) {
        String data[] = split(dataval, ',');
        x_fil = float(data[0]);
        y_fil = float(data[1]);
        z_fil = float(data[2]);
      }
    }
  } catch (Exception e) {
      println("Caught Exception");
  }
}

L'objectif a été de départager expérimentalement les deux méthodes. On bouge pendant environ 5s et on revient à la position de départ. Le gagant est celui qui s'est alors le moins éloigné de la position de départ et le gagnant n'a pas vraiement pu être départagé !

 

3°) Tester suffisamment longtemps pour montrer la dérive d'un gyroscope.

Solution

La dérive est visible même si l’on laisse le capteur GY-521 au repos : le parallélépipède tourne petit à petit. Ces ce défaut que nous allons nous attacher à corriger maintenant !

 

Coupler l'accéléromètre et le gyroscope

Puisqu'on a pu noter des qualités et défauts des deux capteurs d'accélération et de vitesse angulaire, il est légitime de se demander si l’utilisation de ces deux capteurs ne pourrait pas produire un capteur global de meilleure qualité. C'est ce que nous allons essayer d'examiner maintenant.

Ce couplage est très bien décrit pour le cas particulier deux dimensions dans The Balance Filter, mais en anglais bien sûr.

Un programme simple pour commencer

Nous donnons un programme très simple qui relève les donnée de l'accéléromètre et celles du gyroscope. Remarquez que vous avez en bonus la température.

// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain

#include<Wire.h>

const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

void setup(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true); // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
}

Si vous avez suivi jusque là, vous pouvez facilement déduire de ce programme qu’il n’est pas prévu pour des visualisations de données avec processing. En effet il fonctionne à 9600 bauds (au lieu des 38400 exigées). Les données ne peuvent être visibles qu’à l'aide du moniteur série.

Nous pourrions dans un premier temps nous intéresser au problème à deux dimensions car il y a un certain nombre de problèmes intéressants qui peuvent déjà y être associés. Tapez par exemple "self balancing bot" dans un moteur de recherche pour voir des exemples intéressants.

Mais puisque les exercices 4 et 5 travaillent déjà en 3 dimension, nous allons tout simplement les réunir.

Exercice 7 : Et si l’on regroupait les exercices 5 et 6 ensemble

Vous allez maintenant faire fonctionner l'accéléromètre et le gyroscope de manière indépendante sur le parallélépipède central et sur celui de gauche. Cela revient à réunir les codes des exercices 4 et 5. Vous en profiterez pour écrire ce code avec un peu plus de sous-programmes pour en augmenter sa lisibilité.

1°) Si nous mélangeons les données de l'accéléromètre et du gyroscope, nous aurons une sortie comme :

// Send the data to the serial port
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(dT, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(accel_angle_x, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_z, 2);
  Serial.print(F("#GYR:"));
  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_z, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));

On vous demande de réaliser un sous-programme appelé "envoiAProcessing()" ayant un paramètre de type :

struct data
  {
    int x_accel;
    int y_accel;
    int z_accel;
    int temperature;
    int x_gyro;
    int y_gyro;
    int z_gyro;
  };

qui regroupe donc toutes les données importantes et un autre paramètre de type float pour envoyer dt.

2°) Une fois le sous-programme réalisé, faire un sous-programme "aquisitionDonnees()" à partir des programmes d'exemples donnés et tester l'ensemble. Maintenant deux parallélépipèdes sont concernés (celui de gauche et celui du centre).

Solution

Voici une version sans sous-programme :

#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float accel_angle_x=0.0,accel_angle_y=0.0,accel_angle_z=0.0;
float az,ay,ax,accel;
unsigned long last_read_time;
float last_gyro_x_angle=0; // Store the gyro angles to compare drift
float last_gyro_y_angle=0;
float last_gyro_z_angle=0;
float base_x_gyro;
float base_y_gyro;
float base_z_gyro;
float unfiltered_gyro_angle_x=0,unfiltered_gyro_angle_y=0;
float unfiltered_gyro_angle_z=0;

void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  calibrate_gyro();
  Serial.begin(38400);
}

void calibrate_gyro() {
 float x_gyro = 0, y_gyro = 0, z_gyro = 0; 
 for (int i=0;i<10;i++) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43); // starting with register 0x43 (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU,6,true); // request a total of 14 registers
    GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
    x_gyro += (float) GyX;
    y_gyro += (float) GyY;
    z_gyro += (float) GyZ;
    delay(30);
 }
 base_x_gyro = x_gyro / 10.0;
 base_x_gyro = x_gyro / 10.0;
 base_x_gyro = x_gyro / 10.0;
}
 
void loop()
{
  // Convert gyro values to degrees/sec
  float FS_SEL = 131;
  unsigned long t_now;
  float dT;
  
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  
  // AcZ = 15000 au repos AcZ= -18600 à l’envers
  az = (float)(AcZ + 1400);
  //AcX = -15900 au repos AcX =17000
  ax = (float)(AcX - 500);
  // AcY 
  ay = (float)(AcY);
 
  accel = sqrt(ax*ax+az*az+ay*ay);
// avec filtrage
        accel_angle_x = 0.75*accel_angle_x+0.25*atan2(ax,sqrt(az*az+ay*ay));
        accel_angle_y = 0.75*accel_angle_y+0.25*atan2(ay,sqrt(az*az+ax*ax));
        accel_angle_z = 0.75*accel_angle_z+0.25*atan2(sqrt(ay*ay+ax*ax),az);
        
  // calibration du temps
  t_now = millis();
  dT = (float)(t_now - last_read_time)/1000;
  last_read_time = t_now;
  // intégration récursive (une des plus mauvaise)
  unfiltered_gyro_angle_x += ((float) (GyX - base_x_gyro))*dT/FS_SEL;
  unfiltered_gyro_angle_y += ((float) (GyX - base_y_gyro))*dT/FS_SEL;
  unfiltered_gyro_angle_z += ((float) (GyX - base_z_gyro))*dT/FS_SEL;
 
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(500, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(accel_angle_x*180/3.14, 2);
  Serial.print(F(","));
  Serial.print(-accel_angle_y*180/3.14, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_z*180/3.14, 2);
  Serial.print(F("#GYR:"));
  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_z, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.print(F(","));
  Serial.print(0.0, 2);
  Serial.println(F(""));
  delay(8);
}

 

Exercice 8 : apothéose finale

Utiliser un filtre balance pour retrouver l'attitude

Nous allons réaliser le filtre balance représenté dans la figure ci-contre.

Les formules utilisées sont rappelées dans la figure. La formule à droite peut être décomposée comme :

• angle = (angle + gyro * dt) est une intégration
• (0.02)*(x_acc) ajouté à l'intégration agit comme un filtre passe-bas des données de l'accéléromètre
• la multiplication de l'intégration par 0.98 agit comme un filtre passe-haut

Le fait que le filtre numérique passe-bas se retrouve encore dans ce calcul fait que l’on peut prendre directement les entrées de l'accéléromètre non filtrée comme entrée de notre filtre balance. Ceci ne correspond pas tout à fait à la figure présentée. Des tests nous permettrons de choisir entre les deux.

Solution

Voici une version d'essai qui ne fonctionne pas correctement, en tout cas pas mieux que la solution de l'exercice 6. Cette solution doit être corrigée au plus vite.

#include<Wire.h>
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float accel_angle_x=0.0,accel_angle_y=0.0,accel_angle_z=0.0;
float az,ay,ax,accel;
unsigned long last_read_time;
float last_gyro_x_angle=0; // Store the gyro angles to compare drift
float last_gyro_y_angle=0;
float last_gyro_z_angle=0;
float base_x_gyro;
float base_y_gyro;
float base_z_gyro;
float unfiltered_gyro_angle_x=0,unfiltered_gyro_angle_y=0;
float unfiltered_gyro_angle_z=0;
float filtered_gyro_angle_x=0,filtered_gyro_angle_y=0;
float filtered_gyro_angle_z=0;

void setup()
{
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  calibrate_gyro();
  Serial.begin(38400);
}

void calibrate_gyro() { // Programme qui a pour but de retirer les conditions initiales du gyroscope quand l'angle de départ est égal à 0 (accroît sa précision)
 float x_gyro = 0, y_gyro = 0, z_gyro = 0; 
 for (int i=0;i<10;i++) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43); // starting with register 0x43 (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU,6,true); // request a total of 14 registers
    GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
    x_gyro += (float) GyX; // Somme des 10 premières valeurs
    y_gyro += (float) GyY;
    z_gyro += (float) GyZ;
    delay(30);
 }
 base_x_gyro = x_gyro / 10.0; // Moyenne des 10 premières valeurs
 base_y_gyro = y_gyro / 10.0;
 base_z_gyro = z_gyro / 10.0;
}
 
void loop()
{
  // Convert gyro values to degrees/sec
  float FS_SEL = 131; // plage de fonctionnement de 250°/sec
  unsigned long t_now;
  float dT;
  
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  
  // AcZ = 15000 au repos AcZ= -18600 à l’envers
  az = (float)(AcZ + 1400);
  //AcX = -15900 au repos AcX =17000
  ax = (float)(AcX - 500);
  // AcY 
  ay = (float)(AcY);
 
  accel = sqrt(ax*ax+az*az+ay*ay);
// avec filtrage
        accel_angle_x = 0.75*accel_angle_x+0.25*atan2(ax,sqrt(az*az+ay*ay));
        accel_angle_y = 0.75*accel_angle_y+0.25*atan2(ay,sqrt(az*az+ax*ax));
        accel_angle_z = 0.75*accel_angle_z+0.25*atan2(sqrt(ay*ay+ax*ax),az);
        
  // calibration du temps
  t_now = millis();
  dT = (float)(t_now - last_read_time)/1000;
  last_read_time = t_now;

  // intégration récursive (une des plus mauvaise)
  unfiltered_gyro_angle_x += ((float) (GyX - base_x_gyro))*dT/FS_SEL;
  unfiltered_gyro_angle_y += ((float) (GyY - base_y_gyro))*dT/FS_SEL;
  unfiltered_gyro_angle_z += ((float) (GyZ - base_z_gyro))*dT/FS_SEL;
  // balanced filter
  filtered_gyro_angle_x = 0.98 * (filtered_gyro_angle_x + ((float) (GyX - base_x_gyro))*dT/FS_SEL) + 0.02 * accel_angle_x;
  filtered_gyro_angle_y = 0.98 * (filtered_gyro_angle_y + ((float) (GyY - base_y_gyro))*dT/FS_SEL) + 0.02 * accel_angle_y;
  filtered_gyro_angle_z = 0.98 * (filtered_gyro_angle_z + ((float) (GyZ - base_z_gyro))*dT/FS_SEL) + 0.02 * accel_angle_z;
 
  Serial.print(F("DEL:"));              //Delta T
  Serial.print(500, DEC);
  Serial.print(F("#ACC:"));              //Accelerometer angle
  Serial.print(accel_angle_x*180/3.14, 2);
  Serial.print(F(","));
  Serial.print(-accel_angle_y*180/3.14, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_z*180/3.14, 2);
  Serial.print(F("#GYR:"));
  Serial.print(unfiltered_gyro_angle_x, 2);        //Gyroscope angle
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(unfiltered_gyro_angle_z, 2);
  Serial.print(F("#FIL:"));             //Filtered angle
  Serial.print(filtered_gyro_angle_x, 2);
  Serial.print(F(","));
  Serial.print(filtered_gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(filtered_gyro_angle_z, 2);
  Serial.println(F(""));
  // Delay so we don't swamp the serial port
  delay(8);
  //delay(30);
}

 

Plus loin avec le Digital Motion Processor (DMP)

Ce qui fait la richesse de ce capteur MPU6050 c'est qu'il est associé avec un DMP. Il est donc possible d'utiliser ce processeur en lieu et place du filtre complémentaire (déjà rencontré) ou du filtre de Kalman.

Heureusement, la bibliothèque de Jeff Rowberg et en particulier un de ses exemples "MPU6050_DMP6.ino" fait tout le travail. L'exemple fonctionne avec le fichier "MPU6050_6Axis_MotionApps20.h" : lisez donc ce fichier pour vous convaincre du travail pour écrire cette librairie !

Plus loin avec Kalman

Le filtre de Kalman est tout indiqué pour ce que l’on est en train de réaliser. Son approche de manière théorique est pourtant difficile. Ce type de filtre est approprié pour :

• la fusion de données, c’est exactement ce que l’on est en train de faire : trouver des valeurs qui correspondent au mieux aux données bruitées provenant de deux capteurs
• le bruit dont on vient de parler doit être distribué suivant une loi normale ou gaussienne

Routines mathématiques 3D pour l'Arduino

Travail sur les vecteurs 3D

Voici un ensemble de routines permettant de réaliser un ensemble de calculs sur des vecteurs. Sont au menu :

• normalisation
• produit scalaire (dot product)
• produit vectoriel (cross product)

Elles nécessitent l’utilisation des vecteurs sous forme de tableau.

Routines mathématiques pour le calcul sur vecteurs

#ifndef VECTOR3D__H
#define VECTOR3D__H

//get modulus of a 3d vector sqrt(x^2+y^2+y^2)
float vector3d_modulus(float* vector){
        static float R;  
        R = vector[0]*vector[0];
        R += vector[1]*vector[1];
        R += vector[2]*vector[2];
        return sqrt(R);
}

//convert vector to a vector with same direction and modulus 1
void vector3d_normalize(float* vector){
        static float R;  
        R = vector3d_modulus(vector);
        vector[0] /= R;
        vector[1] /= R;
        vector[2] /= R;  
}

//calcuate vector dot-product c = a . b
float vector3d_dot(float* a,float* b){
        return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}

//calcuate vector cross-product c = a x b
void vector3d_cross(float* a,float* b, float* c){
        c[0] = a[1]*b[2] - a[2]*b[1];
        c[1] = a[2]*b[0] - a[0]*b[2];
        c[2] = a[0]*b[1] - a[1]*b[0];
}

//calcuate vector scalar-product n = s x a
void vector3d_scale(float s, float* a , float* b){
        b[0] = s*a[0];
        b[1] = s*a[1];
        b[2] = s*a[2];
}

//calcuate vector sum c = a + b
void vector3d_add(float* a , float* b, float* c){
        c[0] = a[0] + b[0];
        c[1] = a[1] + b[1];
        c[2] = a[2] + b[2];
}

//creates equivalent skew symetric matrix plus identity
//for v = {x,y,z} returns
// m = {{1,-z,y}
// {z,1,-x}
// {-y,x,1}}
void vector3d_skew_plus_identity(float *v,float* m){
        m[0*3+0]=1;
        m[0*3+1]=-v[2];
        m[0*3+2]=v[1];
        m[1*3+0]=v[2];
        m[1*3+1]=1;
        m[1*3+2]=-v[0];
        m[2*3+0]=-v[1];
        m[2*3+1]=v[0];
        m[2*3+2]=1;
}
#endif

Travail sur les matrices 3x3

Matrices avec un tableau à une dimension

Les matrices sont supposées être des tableaux à une seule dimension dans les routines proposées ci-dessous.

Routines mathématiques pour les matrices 3x3

#ifndef MATRIX_H
#define MATRIX_H

//transpose matrix A (m x n) to B (n x m)
void matrix_transpose(int m , int n, float* A, float* B){
        int i,j;
        for(i=0;i<m;i++)
                for(j=0;j<n;j++)
                        B[j*m+i] = A[i*n+j];
}

//multiply matrix A (m x p) by B(p x n) , put result in C (m x n)
void matrix_multiply( int m, int p, int n , float *A, float *B, float *C){
        int i,j,k;
        for(i=0;i<m;i++)                //each row in A
                for(j=0;j<n;j++){       //each column in B
                        C[i*n+j] = 0;
                        for(k=0;k<p;k++){//each element in row A & column B
                                C[i*n+j] += A[i*p+k]*B[k*n+j];
                                //printf("i=%d j=%d k=%d a=%f b=%f c=%f ",i,j,k,(double)(A[i*p+k]),(double)(B[k*n+j]),(double)(C[i*n+j]));
                        }
                        //printf("\n");
                }
};


void matrix_copy(int m, int n, float* A, float* B ){
        int i,j;
        for(i=0;i<m;i++)
                for(j=0;j<n;j++)
                        B[i*n+j] = A[i*n+j];    
}

//print a Matrix
void matrix_print(int m, int n, float* A){
        int i,j;
        for(i=0;i<m;i++){
                printf("{");
                for(j=0;j<n;j++){
                        if(j>0) printf(",");
                        printf("%f", (double)(A[i*n+j]));
                }
                printf("},\n");
        }
}


// Matrix Inversion Routine from http://www.arduino.cc/playground/Code/MatrixMath
// * This function inverts a matrix based on the Gauss Jordan method.
// * Specifically, it uses partial pivoting to improve numeric stability.
// * The algorithm is drawn from those presented in
// NUMERICAL RECIPES: The Art of Scientific Computing.
// * The function returns 1 on success, 0 on failure.
// * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
int matrix_inverse(int n, float* A)
{
        // A = input matrix AND result matrix
        // n = number of rows = number of columns in A (n x n)
        int pivrow;             // keeps track of current pivot row
        int k,i,j;              // k: overall index along diagonal; i: row index; j: col index
        int pivrows[n]; // keeps track of rows swaps to undo at end
        float tmp;              // used for finding max value and making column swaps
       
        for (k = 0; k < n; k++)
        {
                // find pivot row, the row with biggest entry in current column
                tmp = 0;
                for (i = k; i < n; i++)
                {
                        if (fabs(A[i*n+k]) >= tmp)      // 'Avoid using other functions inside abs()?'
                        {
                                tmp = fabs(A[i*n+k]);
                                pivrow = i;
                        }
                }
               
                // check for singular matrix
                if (A[pivrow*n+k] == 0.0f)
                {
                        //Inversion failed due to singular matrix
                        return 0;
                }
               
                // Execute pivot (row swap) if needed
                if (pivrow != k)
                {
                        // swap row k with pivrow
                        for (j = 0; j < n; j++)
                        {
                                tmp = A[k*n+j];
                                A[k*n+j] = A[pivrow*n+j];
                                A[pivrow*n+j] = tmp;
                        }
                }
                pivrows[k] = pivrow;    // record row swap (even if no swap happened)
               
                tmp = 1.0f/A[k*n+k];    // invert pivot element
                A[k*n+k] = 1.0f;                // This element of input matrix becomes result matrix
               
                // Perform row reduction (divide every element by pivot)
                for (j = 0; j < n; j++)
                {
                        A[k*n+j] = A[k*n+j]*tmp;
                }
               
                // Now eliminate all other entries in this column
                for (i = 0; i < n; i++)
                {
                        if (i != k)
                        {
                                tmp = A[i*n+k];
                                A[i*n+k] = 0.0f;  // The other place where in matrix becomes result mat
                                for (j = 0; j < n; j++)
                                {
                                        A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
                                }
                        }
                }
        }
       
        // Done, now need to undo pivot row swaps by doing column swaps in reverse order
        for (k = n-1; k >= 0; k--)
        {
                if (pivrows[k] != k)
                {
                        for (i = 0; i < n; i++)
                        {
                                tmp = A[i*n+k];
                                A[i*n+k] = A[i*n+pivrows[k]];
                                A[i*n+pivrows[k]] = tmp;
                        }
                }
        }
        return 1;
}
#endif

Pour ceux qui seraient frustrés d’utiliser un tableau à une dimension pour les matrices, on vous présente maintenant un autre code.

Matrice avec classe C++

Voici d’abord le fichier d'entête :

Routines mathématiques pour les matrices 3x3 (avec classe objet)

/*
 * matrixtools.h
 *
 * Created: 09/01/2012 12:49:34
 *  Authors: Loïc Kaemmerlen, Pascal Madesclair
 * 
 */ 


#ifndef MATRIXTOOLS_H_
#define MATRIXTOOLS_H_

#include <stdlib.h> 
#include <stdio.h>

typedef struct vector
{
//	float x, y, z;
	 float x, y, z;
} vector;

extern void vector_cross(const vector *a, const vector *b, vector *out);
extern float vector_dot(const vector *a, const vector *b);
extern void vector_normalize(vector *a);

class Matrix
{

	public:
	int row;
	int col;
	float **mat;
	
	// Constructor
	void init ();
	Matrix();
	Matrix(const Matrix& m);
	Matrix(int rownum , int colnum);
	
	// Destructor
	~Matrix();
	
	//Operator
	// Allocation
	Matrix& operator =(const Matrix& Other);
	// Addition
	friend Matrix operator+(Matrix const& a, Matrix const& b);
	// Substraction
	friend Matrix operator-(Matrix const& a, Matrix const& b);
	// Multiplication
	friend Matrix operator*(Matrix const& a, Matrix const& b);
	// Call for Matrix values (ex: A(0,1))
	float& operator() (unsigned row, unsigned col);
	
	
	// Functions
	// Returns the inverse of a 3x3 matrix
	Matrix invert_3x3 ();
	
	// Prints on serial the matrix
	void usart_Send_matrix (void);
	
};
#endif /* MATRIXTOOLS_H_ */

Et maintenant le code (Merci à Loïc Kaemmerlen et Pascal Madesclair)

Routines mathématiques pour les matrices 3x3

/*
 * matrixtools.cpp
 *
 * Created: 09/01/2012 12:49:45
 *  Authors: Loïc Kaemmerlen, Pascal Madesclair
 */ 
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stddef.h>

#include "mathstools.h"
#include "usart.h"


// Vector functions
void vector_cross(const vector *a, const vector *b, vector *out)
{
	out->x = a->y * b->z - a->z * b->y;
	out->y = a->z * b->x - a->x * b->z;
	out->z = a->x * b->y - a->y * b->x;
}

float vector_dot(const vector *a, const vector *b)
{
  return a->x * b->x + a->y * b->y + a->z * b->z;
}

void vector_normalize(vector *a)
{
	float mag = sqrt(vector_dot(a, a));
	a->x /= mag;
	a->y /= mag;
	a->z /= mag;
}

// Matrix functions

// Memory allocation function
void Matrix::init ()
{
	mat = (float**)malloc(sizeof(float*)*row);
	if (mat == NULL)
			USART_Send_string("fail\n");
	
	for (int i=0; i<row; i++)
	{
		mat[i] = (float*)malloc(sizeof(float)*col);
		if (mat[i] == NULL)
			USART_Send_string("fail2\n");
		for(int j=0; j<col; j++)
		{
			mat[i][j] = 0.0f;
		}
	}	
}

// Default constructor
Matrix::Matrix()
{
	//USART_Send_string(".");
	row=3;
	col=3;
	init();
	//USART_Send_string(",");
}

// Constructor copy
Matrix::Matrix(const Matrix& m)
{
	row= m.row;
	col= m.col;
	
	mat = (float**)malloc(sizeof(float*)*row);
	if (mat == NULL)
			USART_Send_string("fail\n");
	
	for (int i=0; i<row; i++)
	{
		mat[i] = (float*)malloc(sizeof(float)*col);
		if (mat[i] == NULL)
			USART_Send_string("fail\n");
		for(int j=0; j<col; j++)
		{
			(*this)(i, j) = m.mat[i][j];
		}
	}	
}

// User constructor
Matrix::Matrix(int rownum , int colnum)
{

	row=rownum;
	col=colnum;
	init();	
}

// Destructor
Matrix::~Matrix()
{
	//USART_Send_string("?");
	for(int i=0; i<row; i++)
	{
			free(this->mat[i]);
	}	
	free(this->mat);
	//USART_Send_string("!");
}

// Operators

// Allocation
Matrix& Matrix::operator =(const Matrix& Other)
{
	row=Other.row;
	col=Other.col;
	
    // Destruction de la ressource (ici un pointeur brut)
	for (int i=0; i<row; i++)
	{
		free (mat[i]);
	}
    free(this->mat);

    // Allocation d'une nouvelle
    init();

    // Copie de la ressource
    for (int i=0; i<this->row; i++)
	{
		// Handles columns of right Matrix
		for (int j=0; j<this->col; j++)
		{
			// Handles the multiplication 
				this->mat[i][j] =Other.mat[i][j];
		}
	}
    return *this;
} 

// Addition
Matrix operator+(Matrix const& a, Matrix const& b)
{
	Matrix copie;
    
	// Copie de la ressource
    for (int i=0; i<a.row; i++)
	{
		// Handles columns of right Matrix
		for (int j=0; j<a.col; j++)
		{
			// Handles the multiplication 
				copie.mat[i][j] = a.mat[i][j] + b.mat[i][j];
		}
	}
	
	
    return copie;
}

// Substraction
Matrix operator-(Matrix const& a, Matrix const& b)
{
	Matrix copie;
    
	// Copie de la ressource
    for (int i=0; i<a.row; i++)
	{
		// Handles columns of right Matrix
		for (int j=0; j<a.col; j++)
		{
			// Handles the multiplication 
				copie.mat[i][j] = a.mat[i][j] - b.mat[i][j];
		}
	}
	
	
    return copie;
}

// Multiplication
Matrix operator *(Matrix const& a, Matrix const& b)
{
	Matrix copie;
	int k;	

	// Handles rows of left Matrix
	for (int i=0; i<a.row; i++)
	{
		// Handles columns of right Matrix
		for (int j=0; j<b.col; j++)
		{
			// Handles the multiplication
			for (k=0, copie.mat[i][j]=0.0; k<a.col; k++)
			{
				copie.mat[i][j] += a.mat[i][k] * b.mat[k][j];
			}
		}
	}
	
	return copie;
}

float& Matrix:: operator() (unsigned row, unsigned col)
{
	return this->mat[row][col];
}

// Functions

// Returns the inverse of a 3x3 matrix
Matrix Matrix::invert_3x3 ()
{
	
	Matrix C (3,3);	
	float det=	this->mat[0][0]*this->mat[1][1]*this->mat[2][2]
				+ this->mat[0][1]*this->mat[1][2]*this->mat[2][0]
				+ this->mat[0][2]*this->mat[1][0]*this->mat[2][1]
				- this->mat[0][2]*this->mat[1][1]*this->mat[2][0]
				- this->mat[1][2]*this->mat[2][1]*this->mat[0][0]
				- this->mat[2][2]*this->mat[0][1]*this->mat[1][0];
				
	C.mat[0][0]=(this->mat[1][1]*this->mat[2][2] - this->mat[1][2]*this->mat[2][1])/det;
	C.mat[0][1]=(this->mat[0][2]*this->mat[2][1] - this->mat[0][1]*this->mat[2][2])/det;
	C.mat[0][2]=(this->mat[0][1]*this->mat[2][1] - this->mat[1][1]*this->mat[2][0])/det;
	
	C.mat[1][0]=(this->mat[1][2]*this->mat[2][0] - this->mat[1][0]*this->mat[2][2])/det;
	C.mat[1][1]=(this->mat[0][0]*this->mat[2][2] - this->mat[0][2]*this->mat[2][0])/det;
	C.mat[1][2]=(this->mat[0][2]*this->mat[1][0] - this->mat[0][0]*this->mat[1][2])/det;
	
	C.mat[2][0]=(this->mat[1][0]*this->mat[2][1] - this->mat[1][1]*this->mat[2][0])/det;
	C.mat[2][1]=(this->mat[0][1]*this->mat[2][0] - this->mat[0][0]*this->mat[2][1])/det;
	C.mat[2][2]=(this->mat[0][0]*this->mat[1][1] - this->mat[0][1]*this->mat[1][0])/det;	
	
	return C;
}

// Prints on serial the matrix
void Matrix::usart_Send_matrix (void)
{
	int i,j;
	
	USART_Send_string("\n"); 
		
	for (i=0; i<this->row; i++)
	{
		USART_Send_string("( ");
		for (j=0; j<this->col; j++)
		{		
			print_double(this->mat[i][j]);
			USART_Send_string(" ");			
		}
		USART_Send_string(")");
		USART_Send_string("\n");
	}
}

Quaternions

Voici un ensemble de routines utilisant les quaternions proposé par Jeff Rowberg.

Routines mathématiques pour Quaternions

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2012-06-05 - add 3D math helper file to DMP6 example sketch

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

#ifndef _HELPER_3DMATH_H_
#define _HELPER_3DMATH_H_

class Quaternion {
    public:
        float w;
        float x;
        float y;
        float z;
        
        Quaternion() {
            w = 1.0f;
            x = 0.0f;
            y = 0.0f;
            z = 0.0f;
        }
        
        Quaternion(float nw, float nx, float ny, float nz) {
            w = nw;
            x = nx;
            y = ny;
            z = nz;
        }

        Quaternion getProduct(Quaternion q) {
            // Quaternion multiplication is defined by:
            //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
            //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
            //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
            //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
            return Quaternion(
                w*q.w - x*q.x - y*q.y - z*q.z,  // new w
                w*q.x + x*q.w + y*q.z - z*q.y,  // new x
                w*q.y - x*q.z + y*q.w + z*q.x,  // new y
                w*q.z + x*q.y - y*q.x + z*q.w); // new z
        }

        Quaternion getConjugate() {
            return Quaternion(w, -x, -y, -z);
        }
        
        float getMagnitude() {
            return sqrt(w*w + x*x + y*y + z*z);
        }
        
        void normalize() {
            float m = getMagnitude();
            w /= m;
            x /= m;
            y /= m;
            z /= m;
        }
        
        Quaternion getNormalized() {
            Quaternion r(w, x, y, z);
            r.normalize();
            return r;
        }
};

class VectorInt16 {
    public:
        int16_t x;
        int16_t y;
        int16_t z;

        VectorInt16() {
            x = 0;
            y = 0;
            z = 0;
        }
        
        VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
            x = nx;
            y = ny;
            z = nz;
        }

        float getMagnitude() {
            return sqrt(x*x + y*y + z*z);
        }

        void normalize() {
            float m = getMagnitude();
            x /= m;
            y /= m;
            z /= m;
        }
        
        VectorInt16 getNormalized() {
            VectorInt16 r(x, y, z);
            r.normalize();
            return r;
        }
        
        void rotate(Quaternion *q) {
            // http://www.cprogramming.com/tutorial/3d/quaternions.html
            // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
            // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
            // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
        
            // P_out = q * P_in * conj(q)
            // - P_out is the output vector
            // - q is the orientation quaternion
            // - P_in is the input vector (a*aReal)
            // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
            Quaternion p(0, x, y, z);

            // quaternion multiplication: q * p, stored back in p
            p = q -> getProduct(p);

            // quaternion multiplication: p * conj(q), stored back in p
            p = p.getProduct(q -> getConjugate());

            // p quaternion is now [0, x', y', z']
            x = p.x;
            y = p.y;
            z = p.z;
        }

        VectorInt16 getRotated(Quaternion *q) {
            VectorInt16 r(x, y, z);
            r.rotate(q);
            return r;
        }
};

class VectorFloat {
    public:
        float x;
        float y;
        float z;

        VectorFloat() {
            x = 0;
            y = 0;
            z = 0;
        }
        
        VectorFloat(float nx, float ny, float nz) {
            x = nx;
            y = ny;
            z = nz;
        }

        float getMagnitude() {
            return sqrt(x*x + y*y + z*z);
        }

        void normalize() {
            float m = getMagnitude();
            x /= m;
            y /= m;
            z /= m;
        }
        
        VectorFloat getNormalized() {
            VectorFloat r(x, y, z);
            r.normalize();
            return r;
        }
        
        void rotate(Quaternion *q) {
            Quaternion p(0, x, y, z);

            // quaternion multiplication: q * p, stored back in p
            p = q -> getProduct(p);

            // quaternion multiplication: p * conj(q), stored back in p
            p = p.getProduct(q -> getConjugate());

            // p quaternion is now [0, x', y', z']
            x = p.x;
            y = p.y;
            z = p.z;
        }

        VectorFloat getRotated(Quaternion *q) {
            VectorFloat r(x, y, z);
            r.rotate(q);
            return r;
        }
};

#endif /* _HELPER_3DMATH_H_ */

Calibration du Invensense MPU6050

Le MPU6050 demande à être calibré parce qu'il n'est pas possible de le faire en usine. En effet cette calibration consiste à ajouter (ou retirer) des offsets qui sont dépendants de chacun de vos MPU6050. Si vous en avez donc plusieurs, il vous faudra automatiquement faire cette opération pour chacun d'eux.

Dans les codes d'exemple d'utilisation de l'accéléromètre, vous pouvez trouver des lignes du genre :

// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

et votre problème consiste à trouver toutes les valeurs correspondants à votre MPU6050. Pour cela vous pouvez utiliser du code que l'on trouve sur Internet.

Code de calibration trouvé sur Internet (luisrodenas)

article de luisrodenas

// Arduino sketch that returns calibration offsets for MPU6050 //   Version 1.1  (31th January 2014)
// Done by Luis Ródenas <luisrodenaslorda@gmail.com>
// Based on the I2Cdev library and previous work by Jeff Rowberg <jeff@rowberg.net>
// Updates (of the library) should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib

// These offsets were meant to calibrate MPU6050's internal DMP, but can be also useful for reading sensors. 
// The effect of temperature has not been taken into account so I can't promise that it will work if you 
// calibrate indoors and then use it outdoors. Best is to calibrate and use at the same room temperature.

/* ==========  LICENSE  ==================================
 I2Cdev device library code is placed under the MIT license
 Copyright (c) 2011 Jeff Rowberg
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 
 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.
 
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
 =========================================================
 */

// I2Cdev and MPU6050 must be installed as libraries
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

///////////////////////////////////   CONFIGURATION   /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000;     //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
int acel_deadzone=8;     //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
int giro_deadzone=1;     //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;
MPU6050 accelgyro(0x68); // 100 && i<=(buffersize+100)){ //First 100 measures are discarded
      buff_ax=buff_ax+ax;
      buff_ay=buff_ay+ay;
      buff_az=buff_az+az;
      buff_gx=buff_gx+gx;
      buff_gy=buff_gy+gy;
      buff_gz=buff_gz+gz;
    }
    if (i==(buffersize+100)){
      mean_ax=buff_ax/buffersize;
      mean_ay=buff_ay/buffersize;
      mean_az=buff_az/buffersize;
      mean_gx=buff_gx/buffersize;
      mean_gy=buff_gy/buffersize;
      mean_gz=buff_gz/buffersize;
    }
    i++;
    delay(2); //Needed so we don't get repeated measures
  }
}

void calibration(){
  ax_offset=-mean_ax/8;
  ay_offset=-mean_ay/8;
  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;
  gy_offset=-mean_gy/4;
  gz_offset=-mean_gz/4;
  while (1){
    int ready=0;
    accelgyro.setXAccelOffset(ax_offset);
    accelgyro.setYAccelOffset(ay_offset);
    accelgyro.setZAccelOffset(az_offset);

    accelgyro.setXGyroOffset(gx_offset);
    accelgyro.setYGyroOffset(gy_offset);
    accelgyro.setZGyroOffset(gz_offset);

    meansensors();
    Serial.println("...");

    if (abs(mean_ax)<=acel_deadzone) ready++;
    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;
    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;
    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;
    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;
    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;
    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6) break;
  }
}

Remplacer

Serial.println("...");

par

Serial.print(mean_ax);
Serial.print("\t");
Serial.print(mean_ay);
Serial.print("\t");
Serial.print(mean_az);
Serial.print("\t");
Serial.print(mean_gx);
Serial.print("\t");
Serial.print(mean_gy);
Serial.print("\t");
Serial.println(mean_gz);

pour avoir des données lisibles.

Le code précédent n'est pas complet. Une page en français sur le sujet de la calibration existe aussi avec un code complet.

Bibliographie

• Phil Kim "Rigid Body Dynamics for Beginners Euler angles and Quaternions" (2013)

Aussi sur Internet

• Capteurs d'accélération : Accéléromètres
• Le GY-521 architecturé autour d'un MPU 6050 peut être trouvé pour 3 €25 chez Amazon
• MPU-6050 Accelerometer + Gyro page qui contient un programme d'exemple.
• Gyroscopes and Accelerometers on a Chip site qui est à l'origine de ce TP avec la théorie de celui-ci : the Balance Filter (Shane Colton).
• GitHub: bibliothèque de Jeff Rowberg pour Arduino. L'exemple IMU_Zero permet la calibration.
• AN3461 : Tilt Sensing Using Linear Accelerometers by: Kimberly Tuck (2007) ou sur ce lien différent AN3461 : Tilt Sensing Using Linear Accelerometers by: Kimberly Tuck (2007).
• AN3461 : Tilt Sensing Using Linear Accelerometers by: Mark Pedley (2013) (la note d'application AN3461 a donc été remise à jour récemment)
• AN-1057 : Using an Accelerometer for Inclination Sensing by Christopher J. Fisher
• Dossier sur le filtre de Kalman en français
• (en) Kalman_filter
• Fichier imu.h destiné à un PIC24F mais qui peut être adapté à nos propos. Il a d'ailleurs été mis dans la partie "routines mathématiques 3D" de ce chapitre.
• Youtube : How to Calibrate an Accelerometer