Guide d'interfaçage d'un gyroscope et d'un accéléromètre avec un Raspberry Pi

Ce guide explique comment utiliser une unité de mesure inertielle (IMU) avec un Raspberry Pi. Ceci est un guide mis à jour et améliore l’ancien trouvé ici.

Dans ce guide, j’expliquerai comment obtenir des lectures de l’IMU et convertir ces lectures brutes en angles utilisables. Je montrerai également comment lire certaines des informations contenues dans les fiches techniques de ces appareils.

Ce guide se concentre sur le BerryIMU. Cependant, la théorie et les principes ci-dessous peuvent être appliqués à n’importe quelle IMU numérique, quelques modifications mineures doivent être apportées. Par exemple Pololu MinIMU, Adafruit IMU et IMU Sparkfun

Dépôt Git ici
Le code peut être extrait sur votre Raspberry Pi avec ;

pi@raspberrypi ~ $ git clone http://github.com/ozzmaker/BerryIMU.git

Le code de ce guide se trouve sous le gyro_accelerometer_tutorial01_angles annuaire.

Une note sur les gyroscopes et les accéléromètres

Lors de l’utilisation de l’IMU pour calculer les angles, les lectures du gyroscope et de l’accéléromètre sont nécessaires, qui sont ensuite combinées. En effet, l’utilisation de l’un ou l’autre seul entraînera des lectures inexactes. Et une note spéciale sur le lacet.

Voici pourquoi ;
Gyros – Un gyroscope mesure le taux de rotation, qui doit être suivi dans le temps pour calculer l’angle actuel. Ce suivi fait dériver le gyroscope. Cependant, les gyroscopes sont bons pour mesurer les mouvements brusques et rapides.

Accéléromètres – Les accéléromètres sont utilisés pour détecter à la fois l’accélération statique (par exemple la gravité) et dynamique (par exemple les démarrages/arrêts soudains). Ils n’ont pas besoin d’être suivis comme un gyroscope et peuvent mesurer l’angle actuel à tout moment. Les accéléromètres sont cependant très bruyants et ne sont utiles que pour suivre les angles sur une longue période de temps.

Les accéléromètres ne peuvent pas mesurer le lacet. Pour l’expliquer simplement, le lacet se produit lorsque l’accéléromètre se trouve sur une surface plane et qu’il est tourné dans le sens des aiguilles d’une montre ou dans le sens inverse. Comme les lectures de l’axe Z ne changeront pas, nous ne pouvons pas mesurer le lacet. Un gyroscope et un magnétomètre peuvent vous aider à mesurer le lacet. Cela fera l’objet d’un futur guide.

Voici un excellent tutoriel sur les accéléromètres et les gyroscopes.

Mise en place de l’IMU et de l’I2C

L’IMU utilisé pour ce guide est un BerryIMU qui utilise un LSM9DS0, qui se compose d’un gyroscope 3 axes, d’un accéléromètre 3 axes et d’un magnétomètre 3 axes.
La fiche technique est nécessaire si vous souhaitez utiliser cet appareil ;LSM9DS0

Cette IMU communique via l’interface I2C.

L’image ci-dessous montre comment connecter le BerryIMU à un Raspberry Pi

Ou BerryIMU peut s’asseoir juste au-dessus des broches GPIO sur un Raspberry Pi A, B, B+ et A+. Les 6 premiers GPIO sont utilisés comme indiqué ci-dessous.

Une fois connecté, vous devez activer I2C sur votre Raspberry Pi et installer les outils I2C. Ce guide vous montrera comment.

Une fois que vous avez activé i2c, vous devriez voir les adresses des composants sur l’IMU lorsque vous utilisez i2cdetect ;

pi@raspberrypi ~ $ sudo i2cdetect -y 1
0 1 2 3 4 5 6 7 8 9 abcdef
00 : — — — — — — — — — — — — —
10 : — — — — — — — — — — — — — — 1e —
20: — — — — — — — — — — — — — — — —
30 : — — — — — — — — — — — — — — — —
40 : — — — — — — — — — — — — — — — —
50 : — — — — — — — — — — — — — — — —
60 : — — — — — — — — — — — 6a — — — —
70 : — — — — — — — 77

Le gyroscope du LSM9DS0 utilise l’adresse 0x6a et l’accéléromètre et le magnétomètre utilisent 0x1e, cela peut être vérifié à la page 33 de la fiche technique.

Cité de la fiche technique avec l’adresse en surbrillance ;
« Les broches SDO/SA0 (SDO_XM/SA0_XM ou SDO_G/SA0_G) peuvent être utilisées pour modifier le bit le moins significatif de l’adresse de l’appareil. Si la broche SA0 est connectée à l’alimentation en tension, LSb vaut ‘1’ (ex. adresse 0011101b ) sinon si le plot SA0 est connecté à la masse, la valeur LSb est ‘0’ (ex. adresse 0011110b). »

Nous ne couvrirons pas le magnétomètre dans cet article.

Lire et écrire sur l’accéléromètre et le gyroscope

Nous lisons et écrivons des valeurs de différents registres dans les adresses ci-dessus pour faire différentes choses. Par exemple, allumez le gyroscope, réglez le niveau de sensibilité, lisez les valeurs de l’accéléromètre. etc..
Le référentiel git comprend un fichier d’en-tête pour le LSM9DS0 qui répertorie tous les registres utilisés par ce capteur.

Nous utilisons des appels ioctl pour lire et écrire des informations sur le bus I2C.

Bus I2C ouvert

Tout d’abord, vous devez ouvrir le fichier de périphérique de bus I2C, comme suit ;


        char filename[20];
        sprintf(filename, "/dev/i2c-%d", 1);
        file = open(filename, O_RDWR);
        if (file<0) {
                printf("Unable to open I2C bus!");
                exit(1);
        }

Comme nous utilisons sprintf, vous devrez inclure le fichier d’en-tête stdio.h

Sélection du gyroscope ou de l’accéléromètre
Avant de pouvoir commencer à écrire et à lire les valeurs des capteurs, nous devons sélectionner l’adresse du capteur sur lequel nous voulons écrire.

if (ioctl(file, I2C_SLAVE, <em>device_address</em>) < 0) {
        printf("Error: Could not select magnetometern");
}

adresse_périphérique est l’adresse du capteur que vous souhaitez sélectionner. Par exemple 0x6a pour le LSM9DS0.
I2C_SLAVE est défini dans i2c-dev.h

Vous devrez inclure les en-têtes de contrôle de fichier dans votre programme, fcntl.h.
Vous devrez également inclure les en-têtes pour le bus i2c – linux/i2c-dev.h

Si nous n’avions qu’un seul capteur, nous n’aurions besoin de le faire qu’une seule fois au début du programme. Comme nous en avons deux, nous devons sélectionner chacun des différents capteurs juste avant de vouloir lire ou écrire une valeur.

Nous devrions écrire une fonction pour faire cela ;

void selectDevice(int file, int addr)
{
 char device[3];

 if (ioctl(file, I2C_SLAVE, addr) < 0) {
 printf("Failed to select I2C device.");
 }
}

Lorsque nous appelons la fonction, nous passerons le pointeur de fichier et l’adresse du capteur que nous voulons sélectionner. Comme ça;

selectDevice(file,ACC_ADDRESS);

Activer l’accéléromètre

 // Enable accelerometer.
        writeAccReg(CTRL_REG1_XM, 0b01100111); //  z,y,x axis enabled, continuos update,  100Hz data rate
        writeAccReg(CTRL_REG2_XM, 0b00100000); // +/- 16G full scale

Pour activer l’accéléromètre, nous devons écrire un octet dans le registre CTRL_REG1_XM(0x20) sur le LSM9DS0. Cela peut être vu à la page 55 de la fiche technique, où vous trouverez également d’autres valeurs qui peuvent être utilisées.

Après cela, nous devons ensuite écrire un octet dans le registre CTRL_REG2_XM (0x21) pour définir notre sensibilité. Cette information se trouve à la page 56 de la fiche technique.

Activer le gyroscope

L’activation du gyroscope est très similaire à l’accéléromètre.

 // Enable Gyro
        writeGyrReg(CTRL_REG1_G, 0b00001111); // Normal power mode, all axes enabled
        writeGyrReg(CTRL_REG4_G, 0b00110000); // Continuos update, 2000 dps full scale

Les valeurs de CTRL_REG1_G et CTRL_REG4_G se trouvent aux pages 41 et 44 de la fiche technique.

Lecture des valeurs de l’accéléromètre et du gyroscope

Une fois que vous avez activé l’accéléromètre et le gyroscope, vous pouvez commencer à lire les valeurs brutes de ces capteurs.
Lecture de l’accéléromètre

void readACC(int  *a)
{
        uint8_t block[6];
        selectDevice(file,ACC_ADDRESS);
        readBlock(0x80 | OUT_X_L_A, sizeof(block), block);

        *a = (int16_t)(block[0] | block[1] << 8);
        *(a+1) = (int16_t)(block[2] | block[3] << 8);
        *(a+2) = (int16_t)(block[4] | block[5] << 8);
}

La fonction ci-dessus lira les valeurs brutes de l’accéléromètre, voici une description de ce qui se passe ;

Un tableau de 6 octets est d’abord créé pour stocker les valeurs.
L’adresse esclave I2C est sélectionnée pour l’accéléromètre, en passant l’adresse de l’accéléromètre ACC_ADDRESS ou 0x1E à la fonction selectDevice().
En utilisant la fonction readBlock() de i2c-dev.h, nous lisons 6 octets à partir de OUT_X_L_A (0x28). Ceci est indiqué à la page 61 de la fiche technique.
Les valeurs sont exprimées en complément à 2 (MSB pour le signe puis 15 bits pour la valeur) il faut donc combiner ;
bloquer[0] & bloquer[1] pour l’axe X
bloquer[2] & bloquer[3] pour l’axe Y
bloquer[4] & bloquer[5] pour l’axe Z

Lire le gyroscope


void readGYR(int *g)
{
	uint8_t block[6];

        selectDevice(file,GYR_ADDRESS);

	readBlock(0x80 | OUT_X_L_G, sizeof(block), block);

        *g = (int16_t)(block[0] | block[1] << 8);
        *(g+1) = (int16_t)(block[2] | block[3] << 8);
        *(g+2) = (int16_t)(block[4] | block[5] << 8)
}

La fonction ci-dessus lit les valeurs du gyroscope. Elle est très similaire à la fonction readACC().
Une fois que nous avons les valeurs brutes, nous pouvons commencer à les convertir en valeurs d’angle significatives.

Convertir les valeurs brutes en angles utilisables

Gyro

        //Convert Gyro raw to degrees per second
        rate_gyr_x = (float) gyrRaw[0] * G_GAIN;
        rate_gyr_y = (float) gyrRaw[1]  * G_GAIN;
        rate_gyr_z = (float) gyrRaw[2]  * G_GAIN;

Pour le gyroscope, nous devons déterminer à quelle vitesse il tourne en degrés par seconde (dps). Cela se fait facilement en multipliant la valeur brute par le niveau de sensibilité qui a été utilisé lors de l’activation du gyroscope. G_GAIN = 0,07, qui est basé sur un niveau de sensibilité de 2000 dps. En regardant le tableau à la page 13 de la fiche technique, nous pouvons voir que nous devons le multiplier par 0,07 pour obtenir des degrés par seconde. Le tableau l’indique à 70 mdps (milli degrés par seconde). Autre exemple : en regardant le tableau, si nous choisissons un niveau de sensibilité de 250 dps lors de l’activation du gyroscope, nous devrons multiplier les valeurs brutes par 0,00875.

Nous devons maintenant suivre les heures supplémentaires de ce gyroscope.

         //Calculate the angles from the gyro
         gyroXangle+=rate_gyr_x*DT;
         gyroYangle+=rate_gyr_y*DT;
         gyroZangle+=rate_gyr_z*DT;

DT = période de boucle. J’utilise une période de boucle de 20ms. donc DT = 0,02.
gyroXangle = est l’angle X actuel calculé à partir des données gyro X.
J’ai DT réglé sur 20ms. C’est le temps qu’il faut pour terminer un cycle de la boucle principale. Cette période de boucle doit être constante et précise, sinon votre gyroscope dérivera plus qu’il ne le devrait.

Accéléromètre

         //Convert Accelerometer values to degrees

        AccXangle = (float) (atan2(accRaw[1],accRaw[2])+M_PI)*RAD_TO_DEG;
        AccYangle = (float) (atan2(accRaw[2],accRaw[0])+M_PI)*RAD_TO_DEG;

Les angles peuvent être calculés en utilisant la trigonométrie et les valeurs brutes de l’autre axe de l’accéléromètre. Cela se fait à l’aide de la fonction Atan2 pour retourner la valeur principale de la tangente des autres angles, exprimée en radians. Nous ajoutons π aux radians pour obtenir un résultat compris entre 0 et 2. Nous convertissons ensuite les radians en degrés en multipliant les radians par 57,29578 (180/π).
M_PI = 3.14159265358979323846
RAD_TO_DEG = 57,29578 , 1 radian = 57,29578 degrés

Vous devrez inclure le fichier d’en-tête mathématique dans votre programme, math.h et lors de la compilation, vous devrez également lier la bibliothèque mathématique. ‘-lm’

Combinaison des angles de l’accéléromètre et du gyroscope

Une fois que vous avez les deux angles des deux capteurs, vous devrez les combiner pour surmonter la dérive du gyroscope et le bruit de l’accéléromètre.
Nous pouvons le faire en utilisant un filtre qui fera confiance au gyroscope pendant de courtes périodes et à l’accéléromètre pendant de plus longues périodes.

Il existe deux filtres que vous pouvez utiliser, le filtre de Kalman ou le filtre complémentaire. Nous utiliserons le filtre complémentaire car il est simple à comprendre et moins gourmand en CPU. Le filtre de Kalman est bien trop complexe et serait très gourmand en processeur.

Filtre complémentaire ;

Angle actuel = 98% x (angle actuel + taux de rotation du gyroscope) + (2% * angle de l’accéléromètre)

ou alors

        CFangleX=AA*(CFangleX+rate_gyr_x*DT) +(1 - AA) * AccXangle;
        CFangleY=AA*(CFangleY+rate_gyr_y*DT) +(1 - AA) * AccYangle;

CFangleX et CFangleY sont les angles finaux à utiliser.

Horaire

Le suivi gyroscopique ne sera pas précis si vous ne pouvez pas exécuter une boucle bien chronométrée.
Le code inclut la fonction mymillis() pour la synchronisation.

Au début de chaque boucle, nous obtenons l’heure actuelle ;

startInt = mymillis();

puis à la fin de la boucle, on s’assure qu’au moins 20ms se sont écoulées avant de continuer ;

//Each loop should be at least 20ms.
while(mymillis() - startInt < 20)
{
        usleep(100);
}

Si votre boucle met plus de 20ms à s’exécuter, 45ms par exemple. Mettez à jour la valeur ci-dessus pour vous assurer que chaque cycle de boucle s’exécute à la même vitesse et définissez cette valeur dans DT pour le suivi gyroscopique.

Convertir les lectures -/+ 180 Ddegrés

 AccXangle -= (float)180.0;
 if (AccYangle > 90)
       AccYangle -= (float)270;
 else
        AccYangle += (float)90;

Le code ci-dessus convertira les valeurs des axes X et Y sur l’accéléromètre afin que la valeur soit 0 lorsque l’IMU est debout. Ce n’est pas nécessaire, mais j’aime que tous les axes soient à 0 lorsque l’appareil est debout.

Afficher les valeurs dans la console

Le code ci-dessous imprimera les lectures sur la console avec un codage couleur pour faciliter la lecture des valeurs.

printf ("GyroX  %7.3f t AccXangle e[m %7.3f t 33[22;31mCFangleX %7.3f33[0mt GyroY  %7.3f t AccYangle %7.3f t 33[22;36mCFangleY %7.3ft33[0mn",gyroXangle,AccXangle,CFangleX,gyroYangle,AccYangle,CFangleY);