Micro contrôleurs AVR/Travail pratique/Utilisation du MPU9250
Le MPU9250 est composé d'un MPU6050 (déjà présenté dans un TP précédent) et d'un magnétomètre AK8963. Il dispose donc des six axes du MPU6050 (3 pour l'accéléromètre et 3 pour le gyroscope) et des trois axes du magnétomètre. Un tel dispositif sera donc qualifié de 9 axes (9 Degree Of Freedom = 9 DOF).
L'objectif de ce TP est d'amener l'étudiant à comprendre les différences de comportements des accéléromètres, des gyroscopes et des magnétomètres en résolvant au mieux le problème de mesure de l'attitude c'est à dire la rotation des axes de cet objet MPU9250 par rapport à un repère d'inertie. Dans ce TP on considérera bien sûr la terre (c'est à dire votre salle de classe) comme un référentiel d'inertie.
Le MPU950 est déclaré EOL (End Of Life) fin de vie par son fabricant Invensense. Cela veut dire que vous n'avez pas intérêt à développer un nouveau produit avec lui. Gageons cependant qu'il sera disponible en Chine encore un certain temps.
Documentation[modifier | modifier le wikicode]
Une présentation en français de l' IMU9250 est disponible ICI. Elle a été réalisée par Vincent Kerhoas.
Introduction aux spécifications[modifier | modifier le wikicode]
Nous allons passer en revue un ensemble de spécifications sur le MPU 9250. Elles nous permettront de comprendre ce que l'on peut envisager de faire avec ce composant. Ces spécifications sont disponibles ICI (MPU9250 datasheet).
La tension d'alimentation du capteur est spécifiée comme comprise entre 2,4V et 3,6V ce qui en principe l'exclut du champ d'application des Arduino 5V. Cependant la majorité des modules vendus peuvent être alimentés en 5V et utilisés en 5V. Ils possèdent un régulateur de tension facilement reconnaissable. Mais le module de Sparkfun par exemple ne fonctionne qu'en 3,3V.
Spécifications du gyroscope[modifier | modifier le wikicode]
Nous donnons seulement un extrait des caractéristiques du gyroscope :
- Gyroscope MPU9250
Paramètre Conditions min Typique max Unités Pleine échelle FS_SEL=0 +/- 250 °/s FS_SEL=1 +/- 500 °/s FS_SEL=2 +/- 1000 °/s FS_SEL=3 +/- 2000 °/s Sensibilité FS_SEL=0 131 LSB/(°/s) FS_SEL=1 65.5 LSB/(°/s) FS_SEL=2 32.8 LSB/(°/s) FS_SEL=3 16.4 LSB/(°/s)
Dans le cas d'un choix FS_SEL à 0, la mesure sera donnée avec 131/(°/s) (lire 131 par [° par seconde]). Autrement dit, une mesure de 250 °/s donnera 131*250 soit 32750. Ceci montre que les données sont sur 16 bits et elles sont signées. Le type C pour les récupérer est donc soit "int" soit "int16_t". Cela montre qu'inversement, pour avoir votre vitesse angulaire en °/s il suffit de prendre le résultat envoyé par le capteur et de le diviser par 131.
Spécifications de l'accéléromètre[modifier | modifier le wikicode]
Nous donnons seulement un extrait des caractéristiques de l'accéléromètre :
- Accéléromètre MPU9250
Paramètre Conditions min Typique max Unités Pleine échelle AS_SEL=0 +/- 2 g AS_SEL=1 +/- 4 g AS_SEL=2 +/- 8 g AS_SEL=3 +/- 16 g Sensibilité AS_SEL=0 16384 LSB/g AS_SEL=1 8192 LSB/g AS_SEL=2 4096 LSB/g AS_SEL=3 2048 LSB/g
Dans le cas d'un choix de AS_SEL à 0, la mesure sera donnée avec 16384/g (lire 16384 par g et non pas 16384 divisé par g !!!). Autrement dit, une mesure d'un g donnera 16384 tandis que deux g donnera 32768. Ceci montre que les données sont sur 16 bits et elles sont signées. Le type C pour les récupérer est donc soit "int" soit "int16_t".
Spécification du magnétomètre[modifier | modifier le wikicode]
Nous donnons seulement un extrait des caractéristiques du magntomètre :
- Magnétomètre MPU9250
Paramètre Conditions min Typique max Unités Pleine échelle 4800 uT Nb bits CAN 14 bits Sensibilité 0.6 uT/LSB Tolérence initiale +/- 500 LSB
Faire fonctionner le MPU9250 en simple i2c[modifier | modifier le wikicode]
Installation de la librairie correspondante[modifier | modifier le wikicode]
Il y a plusieurs librairies utilisables pour le MPU9250. On vous demande d'installer bolderflight/MPU9250 de Brian R Taylor et de réussir à faire fonctionner l'exemple Basic_I2C. Il s'agit d'une simple lecture des données de l'accéléromètre, des données du gyroscope et des données du magnétomètre et de leur affichage à l'aide d'une liaison série dans le moniteur série de l'Arduino. Une fois que tout fonctionne vous êtes prêt pour réaliser l'application 1.
Application 1 : inclinomètre[modifier | modifier le wikicode]
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. En clair nous allons abandonner (momentanément) les données du gyroscope, du magnétomètre et de la température pour ne garder que celles de l'accéléromètre.
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.
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.
Application 2 : inclinomètre sur deux axes[modifier | modifier le wikicode]
Les deux axes en question dans le titre de cette section sont le tangage et le roulis (Tilt (Pitch) et Roll en anglais). Un cours sur le sujet est disponible dans Youtube en anglais. La leçon 6 nous intéresse particulièrement : 9-Axis IMU LESSON 6: Determine Tilt From 3-axis Accelerometer.
Ce que l'on vous demande de réaliser est tout simplement ce qu'il y a dans cette leçon 6 avec un matériel complètement différent :
- 9 axes MPU9250 d'Invensense au lieu du 9 axes BNO055 d'Adafruit
- bolderflight/Librairie MPU9250 de Brian R Taylor au lieu de la librairie Adafruit
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float theta;
float phi;
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
}
void loop() {
// read the sensor
IMU.readSensor();
// display the data
Serial.print(IMU.getAccelX_mss(),6);
Serial.print("\t");
Serial.print(IMU.getAccelY_mss(),6);
Serial.print("\t");
Serial.print(IMU.getAccelZ_mss(),6);
theta=-atan2(IMU.getAccelX_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
phi=-atan2(IMU.getAccelY_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
Serial.print("\t");
Serial.print(theta,6);
Serial.print("\t");
Serial.println(phi,6);
delay(100);
}
|
Si vous faites fonctionner le code de la solution ci-dessus avec un accéléromètre en main vous vous apercevrez très rapidement que ce code possède un inconvénient de taille : si vous faites trembler horizontalement l'accéléromètre il vous donnera un angle de roulis ou un angle de tangage (ou les deux) suivant vos tremblements... et ceci malgré le fait que vous restez horizontal ! Ceci s'explique facilement : vous ajoutez une accélération supplémentaire qui va détruire les beaux calculs trigonométriques qui sont bâtis autour de l'hypothèse que seule la gravitation entre en jeux ! C'est à cause de ce défaut qu'il nous faudra ajouter plus tard les données du gyroscope dans nos calculs. |
Application 3 : stabilisation[modifier | modifier le wikicode]
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.
Nous n'avions pas, au moment de l'écriture de cette solution, le matériel correspondant. Nous avons donc réalisé un essai partiel et la solution ci-dessous mérite donc tout simplement d'être corrigée et améliorée.
#include "MPU9250.h"
#include <Servo.h>
Servo myservo; // create servo object to control a servo
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
myservo.attach(9); // attaches the servo on pin 9 to the servo object
}
void loop() {
// read the sensor
IMU.readSensor();
float angle = atan2(1.0*IMU.getAccelZ_mss(),IMU.getAccelX_mss()) ;
myservo.write(-angle*60);
delay(100);
}
Voir aussi[modifier | modifier le wikicode]
- 9-Axis IMU LESSON 5: Calibrating the BNO055 9-axis Inertial Measurement Sensor (de Paul McWhorter) Cet article ne peut pas être appliqué tel quel à notre accéléromètre.
Exploration du gyroscope[modifier | modifier le wikicode]
Un gyroscope mesure des vitesses angulaires et non des angles. Ce qui nous intéresse pour le problème de l'attitude sont des angles. Le problème semble à priori simple : pour passer des vitesses angulaires aux angles il suffit d'une intégration.
Propriétés de l'intégration[modifier | modifier le wikicode]
Mathématiquement, l'intégration est une fonctionnelle : vous lui donnez à manger une fonction et elle vous retourne un scalaire si la fonction est accompagnée de ses bornes d'intégration. Qu'en est-il d'un point de vue pratique ? La beauté mathématique s'efface car vous allez faire les calculs sur des données discrètes qui ne seront que des approximations des mesures réelles. Tout ceci introduit un bruit aléatoire de mesure et l'intégration d'un bruit donne toujours une dérive (drift en anglais).
Quand on parle de dérive d'un gyroscope, on parle de la dérive de l'angle qui est donc donné après intégration des données du gyroscope. Ce qui fait la dérive, est donc l'opération d'intégration.
Un autre problème mathématique est que la vitesse angulaire est un vecteur mais pas l'angle obtenu par intégration ! Cela veut tout simplement dire que si vous intégrez chacune des vitesses angulaires sur chacun des axes, les angles obtenus ne sont pas des angles que l'on peut utiliser dans une matrice de rotation ! Ceci est naturellement vrai si les rotations sont 3D. Pour les rotations 2D il n'y a pas de problème.
L'intégration d'une vitesse angulaire vectorielle est un problème complexe et ne peut pas se faire suivant les trois axes de manière indépendante. Elle sera pourtant réalisée comme ceci dans la partie Application 4 et les suivantes. Dans ce cas les tests seront fait avec des quasi-rotations 2D (un axe à la fois).
Application 4 : Intégration des données du gyroscope pour trouver les angles[modifier | modifier le wikicode]
On vous demande de réaliser ce qui est expliqué dans la vidéo disponible dans cette page : 9-Axis IMU LESSON 8: Using Gyros for Measuring Rotational Velocity and Angle (de Paul McWhorter) et donc de montrer de manière pratique la dérive de l'angle en fonction du temps.
Indication : vous pouvez garder les angles calculés par l'accéléromètre pour cet exercice.
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float theta,thetaG=0.0;
float phi,phiG=0.0;
float dt;
unsigned long millisOld;
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
}
void loop() {
// read the sensor
IMU.readSensor();
theta=-atan2(IMU.getAccelX_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
phi=-atan2(IMU.getAccelY_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
// display the data
Serial.print(theta,6);
Serial.print("\t");
Serial.print(phi,6);
dt=(millis()-millisOld)/1000.;
millisOld=millis();
thetaG=thetaG-IMU.getGyroY_rads()*dt;
phiG=phiG+IMU.getGyroX_rads()*dt;
Serial.print("\t");
Serial.print(thetaG/2/3.141592654*360,6);
Serial.print("\t");
Serial.print(phiG/2/3.141592654*360,6);
Serial.print("\t");
Serial.print(thetaG/2/3.141592654*360-theta,6);
Serial.print("\t");
Serial.println(phiG/2/3.141592654*360-phi,6);
delay(100);
}
Nous avons ajouté l'affichage de la différence entre les angles accéléromètres et les angles gyroscope. Cela nous a permis d'ailleurs de changer les signes de l'intégration par rapport à la correction trouvé dans le document de la LESSON 8.
Voir aussi[modifier | modifier le wikicode]
Vidéos dédiées au filtrage du gyroscope pour drones en anglais[modifier | modifier le wikicode]
- Gyroscope and filtering part 1 : gyro noise sources
- Gyroscope and filtering part 2 : filtering basics
- Gyroscope and filtering part 3 : find those frequencies
- Gyroscope and filtering part 4 : INAV and Betaflight setup
Fusion de données accéléromètre gyroscope avec filtre complémentaire[modifier | modifier le wikicode]
L'objectif de cette section est simplement d'essayer d'utiliser les données de l'accéléromètre ainsi que celle du gyroscope pour en déduire les angles de roulis et de tangage. La sensibilité de l'accéléromètre aux hautes fréquences nous impose d'abord de réaliser un filtrage passe-bas de ses données.
Application 5 Filtrage passe-bas des angles de l'accéléromètre[modifier | modifier le wikicode]
On vous demande de réaliser ce qui est expliqué dans la vidéo disponible dans cette page : 9-Axis IMU LESSON 7: Understanding Low Pass Filters (de Paul McWhorter)
Indication : Vous devez partir du code donné dans la correction de l'application 4 pour cet exercice. Vous ne travaillerez certes que sur les données de l'accéléromètre mais la présence du gyroscope simplifiera la réalisation de l'Application 6.
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float thetaG=0.0;
float phiG=0.0;
float dt;
unsigned long millisOld;
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
}
void loop() {
// read the sensor
IMU.readSensor();
thetaM=-atan2(IMU.getAccelX_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
phiM=-atan2(IMU.getAccelY_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
// Low-pass filter
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;
// display the data
Serial.print(thetaFnew,6);
Serial.print("\t");
Serial.print(phiFnew,6);
// gyro data integration
dt=(millis()-millisOld)/1000.;
millisOld=millis();
thetaG=thetaG-IMU.getGyroY_rads()*dt;
phiG=phiG+IMU.getGyroX_rads()*dt;
Serial.print("\t");
Serial.print(thetaG/2/3.141592654*360,6);
Serial.print("\t");
Serial.print(phiG/2/3.141592654*360,6);
phiFold=phiFnew;
thetaFold=thetaFnew;
delay(100);
Application 6 : fusion de données par filtre complémentaire[modifier | modifier le wikicode]
On vous demande de réaliser ce qui est expliqué dans la vidéo disponible dans cette page : 9-Axis IMU LESSON 9: Accurate and Stable Tilt Using Accelerometers, Gyros and a Complementary Filter
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float theta,thetaG=0.0;
float phi,phiG=0.0;
float dt;
unsigned long millisOld;
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;
void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
}
void loop() {
// read the sensor
IMU.readSensor();
// get accelerometer data and angle computing
thetaM=-atan2(IMU.getAccelX_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
phiM=-atan2(IMU.getAccelY_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
// Low-pass filter
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;
// complementary filter
dt=(millis()-millisOld)/1000.;
millisOld=millis();
//thetaG=thetaG-IMU.getGyroY_rads()*dt;
//phiG=phiG+IMU.getGyroX_rads()*dt;
theta=(theta-IMU.getGyroY_rads()/3.141592654*180*dt)*.95+thetaM*.05;
phi=(phi+IMU.getGyroX_rads()/3.141592654*180*dt)*.95+ phiM*.05;
// display the data
Serial.print(theta,6);
Serial.print("\t");
Serial.println(phi,6);
phiFold=phiFnew;
thetaFold=thetaFnew;
delay(100);
}
Les coefficients 0.95 et 0.05 peuvent être ajustés. La seule contrainte est de garder une somme égale à 1.
Tout a l'air de se passer correctement pour les données globales qui sont données en degrés par ce code Arduino. L'application suivante va nous permettre de visualiser les données.
Application 7 : Visualisation graphique des données[modifier | modifier le wikicode]
Nous allons utiliser à partir de maintenant une visualisation des données en utilisant un programme Processing.org donné ci-dessous.
/**
* 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);
rotateX(radians(x_fil - x_rotation));
rotateY(radians(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]);
} 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");
}
}
La seule modification par rapport à l'application 6, consiste à changer la sortie par liaison série pour qu'elle soit conforme à ce qu'est capable d'interpréter le programme donné.
Il y a eu quelques ajustements par rapport à la solution de l'application 6 en plus de la gestion de la liaison série.
Voici donc le code tant attendu :
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float theta=0.0,thetaG=0.0;
float phi=0.0,phiG=0.0;
float dt;
unsigned long millisOld;
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;
void setup() {
// serial to display data
Serial.begin(38400);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
}
void loop() {
// read the sensor
IMU.readSensor();
// get accelerometer data and angle computing
thetaM=-atan2(IMU.getAccelX_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
phiM=-atan2(IMU.getAccelY_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
// Low-pass filter
phiFnew=.9*phiFold+.1*phiM;
thetaFnew=.9*thetaFold+.1*thetaM;
dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta-IMU.getGyroY_rads()/3.141592654*180*dt)*.75+thetaM*.25;
phi=(phi+IMU.getGyroX_rads()/3.141592654*180*dt)*.75+ phiM*.25;
thetaG=thetaG-IMU.getGyroY_rads()*dt;
phiG=phiG+IMU.getGyroX_rads()*dt;
// display the data
// 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(-phiFnew, 2);
Serial.print(F(","));
Serial.print(-thetaFnew, 2);
Serial.print(F(","));
Serial.print(0.0, 2);
Serial.print(F("#GYR:"));
Serial.print(-phiG/3.141592654*180, 2); //Gyroscope angle
Serial.print(F(","));
Serial.print(-thetaG/3.141592654*180, 2);
Serial.print(F(","));
Serial.print(0.0, 2);
Serial.print(F("#FIL:")); //Filtered angle
Serial.print(phi, 2);
Serial.print(F(","));
Serial.print(theta, 2);
Serial.print(F(","));
Serial.print(0.0, 2);
Serial.println(F(""));
phiFold=phiFnew;
thetaFold=thetaFnew;
delay(100);
}
Les coefficients de filtrage peuvent être ajustés.
L'exécution de ces deux codes (Processing et Arduino) vous montre :
- à gauche la rotation calculée à partir des données du gyroscope : il est possible de voir à l’œil la dérive de celui-ci.
- au centre la même chose à partir des données de l'accéléromètre qui sont filtrées d'où sa réponse assez lente.
- à droite les deux données précédentes passées par un filtre complémentaire. Les mouvements y sont assez réactifs. C'est le mieux que l'on puisse obtenir à partir du MPU9250 de manière relativement simple.
Pour faire mieux il faut utiliser le DMP (Digital Motion Processor). Avant de regarder cet aspect, nous allons nous intéresser au magnétomètre.
Ajout d'un compas (magnétomètre)[modifier | modifier le wikicode]
Arrivés jusqu'ici, nous avons réussi à fusionner les données de l'accéléromètre et du gyroscope pour trouver les angles de roulis et de tangage pour l'attitude. Il reste cependant un angle important : c'est le lacet. Cet angle n'est pas accessible à un accéléromètre (voir démonstration ICI). Il n'est pas non plus dérivable à partir des données du gyroscope à cause de la dérive. Pour calculer cet angle, nous allons introduire un capteur supplémentaire, le détecteur de champ magnétique terrestre.
Un magnétomètre doit être absolument calibré pour être utilisé correctement. Ceci représente une difficulté d'utilisation particulière. Le calibrage a comme objectif de prendre en compte deux types d'erreurs (en anglais):
- soft iron distortion
- hard iron distortion
La Hard Iron Distortion est due aux parties métalliques de votre Robot, aux courants électriques de l'électronique de votre Robot. Ainsi pour trouver les facteurs de correction il vous faudra positionner le magnétomètre exactement à sa place finale dans votre Robot.
La Soft Iron Distortion est quant à elle, plutôt liée aux défauts du champ magnétique local (quand votre Robot n'y est pas).
Ce lien propose un logiciel d'aide à la calibration utilisé dans ce lien Youtube.
Application 8 : Utilisation du magnétomètre pour trouver le lacet[modifier | modifier le wikicode]
On vous demande de réaliser ce qui est expliqué dans la vidéo disponible dans cette page 9-Axis IMU LESSON 10: Making a Tilt Compensated Compass with Arduino (de Paul McWhorter)
L'anglais "tilt-compensated-compass" peut être traduit par compas à inclinaison compensée en français.
Vous allez procéder comme dans la vidéo, en deux étapes :
- calcul de l'angle de lacet par rapport au nord magnétique avec qui permet de trouver où xm et ym sont les deux données du magnétomètre respectivement sur les axes x et y
- amélioration du calcul précédent avec le prise en compte du roulis et du tangage
Première étape[modifier | modifier le wikicode]
Le calcul présenté dans cette première étape est tellement naïf qu'il ne convient pas à résoudre le problème correctement. Nous le présentons quand même pour des raisons pédagogiques.
//Show combined data
pushMatrix();
translate(5*width/6, height/2, -50);
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(radians(y_fil));
rotateZ(radians(z_fil));
draw_rect(93, 175, 83);
popMatrix();
- Pour la partie Arduino il vous faut ajouter la partie suivante dans le code Arduino :
Xm = IMU.getMagX_uT();
Ym = IMU.getMagY_uT();
psi = atan2(Ym,Xm);
avec une modification de la partie envoi de données par la liaison série :
Serial.print(F("#FIL:")); //Filtered angle
//Serial.print(phi, 2);
Serial.print(0.0, 2);
Serial.print(F(","));
//Serial.print(theta, 2);
Serial.print(0.0, 2);
Serial.print(F(","));
Serial.print(psi/3.141592654*180, 2);
Serial.println(F(""));
Il est facile de voir à l'aide de ce code que l'inclinaison de tangage ou de roulis influent sur l'angle de lacet, ce qui n'est absolument pas souhaitable... d'où la deuxième étape. Avant de passer à la deuxième étape, chacun d'entre-vous aura remarqué que la rotation du magnétomètre selon l'axe vertical z n'est pas correctement reproduit par la figure 3D de processing.
Calibration du magnétomètre[modifier | modifier le wikicode]
Le seul coupable est le magnétomètre qu'il faut calibrer. Nous pourrions nous inspirer de l'article Simple and Effective Magnetometer Calibration pour cela, mais nous allons plutôt utiliser la fonction de calibration qui existe toute faite dans la librairie que l'on utilise. Cette fonction s'appelle calibrateMag() et il vous est demandé de réaliser une figure en forme de huit lors de son exécution. Une fois réalisée, nous allons afficher à l'aide de la liaison série les résultats trouvés que l'on peut trouver à l'aide des primitives toutes faites de la librairie, à savoir :
- float MPU9250::getMagBiasX_uT()
- float MPU9250::getMagScaleFactorX()
- float MPU9250::getMagBiasY_uT()
- float MPU9250::getMagScaleFactorY()
- float MPU9250::getMagBiasZ_uT()
- float MPU9250::getMagScaleFactorZ()
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
void setup() {
pinMode(13, OUTPUT);
// serial to display data
Serial.begin(38400);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
digitalWrite(13,HIGH);
IMU.calibrateMag();
digitalWrite(13,LOW);
}
void loop() {
// put your main code here, to run repeatedly:
Serial.print(IMU.getMagBiasX_uT(),6);
Serial.print("\t");
Serial.print(IMU.getMagBiasY_uT(),6);
Serial.print("\t");
Serial.print(IMU.getMagBiasZ_uT(),6);
Serial.print("\t");
Serial.print(IMU.getMagScaleFactorX(),6);
Serial.print("\t");
Serial.print(IMU.getMagScaleFactorY(),6);
Serial.print("\t");
Serial.println(IMU.getMagScaleFactorZ(),6);
delay(1000);
}
Nous allumons la led 13 dès que le calibrage est prêt à être réalisé (attendre quelques secondes pour voir cette led s'allumer).
Vous avez ensuite une trentaine de secondes pour faire vos figures en forme de huit dans toutes les directions. Une fois que la led 13 s'éteint la liaison série affiche les valeurs de correction trouvées.Les données affichées par la liaison série pour le programme ci-dessus sont : 11,066 - 27,122 - -60,339 - 1,089 - 1,063 - 0,876
Ces données sont très dépendantes de l'accéléromètre que vous utilisez et pour vous elles n'ont peu de chance de correspondre à celles-ci.
On peut imaginer négliger les trois facteurs d'échelle mais c'est à essayer pour le troisième sur l'axe des z.
Prise en compte de la calibration du magnétomètre[modifier | modifier le wikicode]
Ce n'est pas le tout de calibrer le magnétomètre, il faut positionner les valeurs que l'on a trouvé pour les offsets et les facteurs d'échelle de la section précédente et les utiliser pour le calcul du lacet. Voici le code Arduino correspondant.
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float theta=0.0,thetaG=0.0;
float phi=0.0,phiG=0.0;
float dt;
unsigned long millisOld;
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;
// magnetometer :
float Xm, Ym, psi;
float phiRad,thetaRad;
void setup() {
// serial to display data
Serial.begin(38400);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
// calibration propre du magnétomètre
IMU.setMagCalX(11.066,1.089);
IMU.setMagCalY(27.122,1.063);
IMU.setMagCalZ(-60.339,0.876);
}
void loop() {
// read the sensor
IMU.readSensor();
// get accelerometer data and angle computing
thetaM=-atan2(IMU.getAccelX_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
phiM=-atan2(IMU.getAccelY_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
// Low-pass filter
phiFnew=.9*phiFold+.1*phiM;
thetaFnew=.9*thetaFold+.1*thetaM;
dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta-IMU.getGyroY_rads()/3.141592654*180*dt)*.75+thetaM*.25;
phi=(phi+IMU.getGyroX_rads()/3.141592654*180*dt)*.75+ phiM*.25;
thetaG=thetaG-IMU.getGyroY_rads()*dt;
phiG=phiG+IMU.getGyroX_rads()*dt;
phiRad=phi/360*(2*3.14);
thetaRad=theta/360*(2*3.14);
Xm = IMU.getMagX_uT();
Ym = IMU.getMagY_uT();
//Xm=IMU.getMagX_uT()*cos(thetaRad)-IMU.getMagY_uT()*sin(phiRad)*sin(thetaRad)+IMU.getMagZ_uT()*cos(phiRad)*sin(thetaRad);
//Ym=IMU.getMagY_uT()*cos(phiRad)+IMU.getMagZ_uT()*sin(phiRad);
psi = atan2(Ym,Xm);
// display the data
Serial.print(F("DEL:")); //Delta T
Serial.print(5, DEC);
Serial.print(F("#ACC:")); //Accelerometer angle
Serial.print(-phiFnew, 2);
Serial.print(F(","));
Serial.print(-thetaFnew, 2);
Serial.print(F(","));
Serial.print(0.0, 2);
Serial.print(F("#GYR:"));
Serial.print(-phiG/3.141592654*180, 2); //Gyroscope angle
Serial.print(F(","));
Serial.print(-thetaG/3.141592654*180, 2);
Serial.print(F(","));
Serial.print(0.0, 2);
Serial.print(F("#FIL:")); //Filtered angle
//Serial.print(phi, 2);
Serial.print(0.0, 2);
Serial.print(F(","));
//Serial.print(theta, 2);
Serial.print(0.0, 2);
Serial.print(F(","));
Serial.print(psi/3.141592654*180, 2);
Serial.println(F(""));
phiFold=phiFnew;
thetaFold=thetaFnew;
delay(100);
}
A ce point, nous sommes assez avancé pour envisager maintenant de passer à la deuxième étape.
Si vous avez seulement l'intention d'utiliser le compas pour un robot mobile qui se déplace sur un plan parfaitement horizontal, alors les corrections réalisées dans la deuxième étape ne seront pas forcément nécessaires. En effet dans ce cas précis, vous êtes sûr qu'il n'y aura auncun tangage et aucun roulis.
Deuxième étape[modifier | modifier le wikicode]
Il s'agit de prendre en compte le tangage et le roulis pour le calcul du lacet, de telle manière que ces deux premiers angles ne perturbent pas ce calcul (du lacet).
#include "MPU9250.h"
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;
float theta=0.0,thetaG=0.0;
float phi=0.0,phiG=0.0;
float dt;
unsigned long millisOld;
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;
// magnetometer :
float Xm, Ym, Zm, psi;
float phiRad,thetaRad;
void setup() {
// serial to display data
Serial.begin(38400);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
// calibration propre du magnétomètre
IMU.setMagCalX(25.129,0.998);
IMU.setMagCalY(-3.122,0.978);
IMU.setMagCalZ(-3.69,1.024);
}
void loop() {
// read the sensor
IMU.readSensor();
// get accelerometer data and angle computing
thetaM=-atan2(IMU.getAccelX_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
phiM=-atan2(IMU.getAccelY_mss(),-IMU.getAccelZ_mss())/2/3.141592654*360;
// Low-pass filter
phiFnew=.9*phiFold+.1*phiM;
thetaFnew=.9*thetaFold+.1*thetaM;
dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta-IMU.getGyroY_rads()/3.141592654*180*dt)*.75+thetaM*.25;
phi=(phi+IMU.getGyroX_rads()/3.141592654*180*dt)*.75+ phiM*.25;
thetaG=thetaG-IMU.getGyroY_rads()*dt;
phiG=phiG+IMU.getGyroX_rads()*dt;
phiRad=phi/360*(2*3.14);
thetaRad=theta/360*(2*3.14);
Xm = IMU.getMagX_uT();
Ym = IMU.getMagY_uT();
Zm = IMU.getMagZ_uT();
// pitch/roll correction (tangage/roulis)
Xm = Xm*cos(thetaRad)-Ym*sin(phiRad)*sin(thetaRad)+Zm*cos(phiRad)*sin(thetaRad);
Ym = Ym*cos(phiRad)+Zm*sin(phiRad);
psi = atan2(Ym,Xm);
// display the data
Serial.print(F("DEL:")); //Delta T
Serial.print(5, DEC);
Serial.print(F("#ACC:")); //Accelerometer angle
Serial.print(-phiFnew, 2);
Serial.print(F(","));
Serial.print(-thetaFnew, 2);
Serial.print(F(","));
Serial.print(0.0, 2);
Serial.print(F("#GYR:"));
Serial.print(-phiG/3.141592654*180, 2); //Gyroscope angle
Serial.print(F(","));
Serial.print(-thetaG/3.141592654*180, 2);
Serial.print(F(","));
Serial.print(0.0, 2);
Serial.print(F("#FIL:")); //Filtered angle
//Serial.print(phi, 2);
Serial.print(0.0, 2);
Serial.print(F(","));
//Serial.print(theta, 2);
Serial.print(0.0, 2);
Serial.print(F(","));
Serial.print(psi/3.141592654*180, 2);
Serial.println(F(""));
phiFold=phiFnew;
thetaFold=thetaFnew;
delay(100);
}
|
|
|
Utilisation du DMP (Digital Motions Processor)[modifier | modifier le wikicode]
Comme le MPU6050, le MPU9250 est équipé d'un processeur de fusion de données appelé DMP. La seule librairie DMP pour MPU9250 que nous connaissons à ce jour est la librairie Sparkfun de Jim Lindblom. Elle peut être téléchargée ICI (Arduino library for the MPU-9250 enabling its digital motion process (DMP) features). Elle a cependant un gros inconvénient pour le monde Arduino, c'est qu'elle est destinée aux 32 bits Arduino ZERO.
Nous avons cependant essayé la bibliothèque de Jeff Rowberg destinée au MPU6050 avec succès. Ceci est à la fois une bonne et une mauvaise nouvelle :
- la bonne c'est qu'il n'y a pas à écrire une nouvelle librairie. Nous ne nous voyions pas porter une partie de la librairie de Jim Lindblom (Sparkfun) pour des microcontrôleurs 8 bits.
- la mauvaise c'est que le DMP ne fonctionne que comme dans le MPU6050 c'est à dire en fusion de données de l'accéléromètre et du gyroscope et qu'il laisse de côté le magnétomètre.
En conséquence, si vous voulez que la fusion de données soit complète, il vous faudra vous intéresser au successeur du MPU9250, l'ICM20948. Mais il vous en coûtera environ 8 € (au lieu de 3 € en 2019 pour le MPU9250)
|
|
Les prix donnés ci-dessus sont ceux qui ont précédé la pandémie COVID 19 ! En 2022 il faut plutôt compter 19 € pour un ICM20948 et 7 € pour un MPU9250 !!! |
Reprise du problème de stabilisation[modifier | modifier le wikicode]
Le problème de stabilisation déjà évoqué dans ce TP est repris ici à l'aide du DMP. Ce problème n'est pas assez complexe pour donner des résultats meilleurs que ce que nous avons déjà fait. Il n'est donc repris ici que pour des raisons pédagogiques.
Fusion de données DMP magnétomètre : est-ce possible à réaliser soi-même ?[modifier | modifier le wikicode]
La fusion de données roulis/tangage avec un magnétomètre a été traité dans la section Ajout d'un compas (magnétomètre). Nous allons reprendre ce travail en essayant de remplacer le calcul par filtre complémentaire par le DMP.
Voir aussi[modifier | modifier le wikicode]
- Capteurs d'accélération : Accéléromètres
- How to Implement an Inertial Measurement Unit (IMU) Using an Accelerometer, Gyro, and Magnetometer (Youtube)
- How to Calibrate a Magnetometer - Digikey Vidéo très pédagogique pour ceux qui comprennent l'anglais.
- YouTube : Magnetometer Errors and Calibration
- bolderflight/MPU9250
- sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library librairie réservée aux Arduino SAM 32 bits
- Affordable 9 DoF Sensor Fusion
- MPU9250 datasheet
- Le blog à Lulu
- Using the mpu9250 to get real time motion data
- Applications of Magnetic Sensors for Low Cost Compass Systems (Théorie)
- Tilt Compensated Compass
- Simple and Effective Magnetometer Calibration
- Advanced hard and soft iron magnetometer calibration for dummies
- Vidéos sur le sujet : Understanding Sensor Fusion and Tracking