Cours:Accelerometre

De troyesGEII
Aller à : navigation, rechercher
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.

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.

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

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 !!!

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

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 ° !

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

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.

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

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.

Utiliser le gyroscope pour retrouver l'attitude

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