Cours:Accelerometre
Retour à la liste des Tps
Éléments de correction (page protégée)
Le but de ce projet est de comprendre et utiliser un capteurs moderne d'accélération.
Sommaire
Introduction aux capteurs IMU
IMU : Inertial Measurement Unit se traduit en français par Centrale à Inertie.
Le capteur que nous allons utiliser porte la référence GY-521 et est architecturé autour d'un MPU 6050 qui est composé de deux capteurs et 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
- Digital Motion Processor (DMP) capable de stocker des données et les restituer
La terminologie anglo-saxonne associée pour 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 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 différents ce qui rend leur utilisation délicate.
Nous allons nous contenter de six axes dans la suite de ce projet.
Ce que nous cherchons à réaliser dans ce projet 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. Elle est en général caractérisée par trois angles (roulis, tangage et cap ou lacet).
Ce mot d'attitude ne doit pas être confondu avec l'altitude ! Ces deux mots n'ont absolument rien en commun (à part l'orthographe voisine) !
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.
Partie Arduino
La première question à se poser est de savoir 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. L'adresse i2c écrite en dur dans le programme comme 0x68 nécessite que la broche appelée AD0 sur la carte soit reliée à la masse.
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. Vous en aurez absolument besoin pour la suite !
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 !!!
A 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.
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.
Partie Processing
On désire réaliser avec Processing une visualisation des données de l'exercice 2 précédent.
Exercice 3
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, de retirer le baratin "Phi = " 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;
char index;
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);
}
}
A 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 4
1°) Soit le programme Processing suivant :
/**
* 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 !
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). Dit autrement, c'est votre programme qui devra exécuter la rotation.
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 !
3°) Essayez de généraliser avec les formules présentées dans cette page. N'oubliez pas de réaliser tout en ° !
Voici encore une fois résumé sur une figure ce que nous avons réussi à réaliser à ce point.
Conséquences des résultats de l'exercice 4
Le travail de la question 3°) est facile à réaliser. Pourtant il n'est en aucun cas satisfaisant ! C'est ce que nous montrons sur la figure ci-contre.
Nous rappelons que la question à laquelle nous cherchons à répondre est :
- est-il possible avec la simple connaissance des trois composantes de g (gx,gy et gz) de retrouver l'orientation du repère dans lequel les mesures sont faites.
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 (qui est la diagonale du parallélogramme. 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 n'est plus le même. Ainsi la connaissance de gx, gy, gz ne suffira pas pour trancher parmi tous les repères possibles, lequel est le bon. Votre accéléromètre est insensible aux rotations verticales si l'on s'en tient aux mesures de l'accélération de pesanteur.
Voir aussi
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 (GYRO_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);
}
Peut être que certains d'entre vous auront remarqué qu'entre les adresses de l'accéléromètre et celles du gyroscope il y a deux octets. C'est en fait la température du composant qui s'y trouve mais nous ne nous y intéresserons pas dans ce projet !
Exercice 5
1°) Montrer que ce programme fonctionne.
ATTENTION 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.
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é tombé 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;
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 de de vitesse angulaire, il est légitime de se demander si l'utilisation de ces deux capteurs peut pas produire un capteur global de meilleure qualité. C'est ce que nous allons essayer d'examiner maintenant.
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);
}
Reprise du travail déjà réalisé
Nous avons déjà réalisé un certain nombre de calculs destinés au parallélépipède de gauche (intégration numérique des données du gyroscope exercice 5) et obtention de l'attitude à partir de l'accéléromètre (exercice 4).
Exercice 6
On vos demande de reprendre votre travail des exercices 4 et 5, et d'envoyer les sorties correspondantes sur les deux parallélépipèdes (gauche et centre).
Annexe
On donne ici quelques utilitaires de calcul :
/**************************************************/
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]){
float op[3];
for(int x=0; x<3; x++){
for(int y=0; y<3; y++){
for(int w=0; w<3; w++){
op[w]=a[x][w]*b[w][y];
}
mat[x][y]=0;
mat[x][y]=op[0]+op[1]+op[2];
float test=mat[x][y];
}
}
}
//Computes the dot product of two vectors
float Vector_Dot_Product(float vector1[3],float vector2[3]){
float op=0;
for(int c=0; c<3; c++){
op+=vector1[c]*vector2[c];
}
return op;
}
//Computes the cross product of two vectors
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]){
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
}
//Multiply the vector by a scalar.
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2){
for(int c=0; c<3; c++){
vectorOut[c]=vectorIn[c]*scale2;
}
}
void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]){
for(int c=0; c<3; c++){
vectorOut[c]=vectorIn1[c]+vectorIn2[c];
}
}
/**************************************************/
void Normalize(void){
float error=0;
float temporary[3][3];
float renorm=0;
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}
/**************************************************/
void Drift_correction(void){
float mag_heading_x;
float mag_heading_y;
float errorCourse;
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
// We make the gyro YAW drift correction based on compass magnetic heading
mag_heading_x = cos(MAG_Heading);
mag_heading_y = sin(MAG_Heading);
errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}
/**************************************************/
/*
void Accel_adjust(void){
Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
}
*/
/**************************************************/
void Matrix_update(void){
Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
Accel_Vector[0]=accel_x;
Accel_Vector[1]=accel_y;
Accel_Vector[2]=accel_z;
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
//Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
#if OUTPUTMODE==1
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
for(int x=0; x<3; x++) //Matrix Addition (update){
for(int y=0; y<3; y++){
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
void Euler_angles(void){
pitch = -asin(DCM_Matrix[2][0]);
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
}
Aussi sur Internet
- 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.
- gyroscope et accéléromètre dans une puce
- Tilt Sensing Using Linear Accelerometers
- The Balance Filter
