Table des matières

Accéléromètre et gyromètre I2C

1. Introduction

Ce document montre la mise en œuvre d'un accéléromètre/gyromètre à liaison série I2C, le MPU6050 (InvenSense). Il s'agit d'un circuit intégré comportant un accéléromètre trois axes et un gyromètre trois axes avec un convertisseur A/N 16 bits. Il possède aussi une unité de traitement du signal pour filtrer les signaux et pour faire des calculs de position et d'orientation (Digital Motion Processor). Les données sont transmises à un microcontrôleur par liaison série I2C. Les plages d'accélérations vont de 2g à 16g, celles des vitesses de rotation de 250 à 2000 degrés par seconde et la fréquence d'échantillonnage peut atteindre 8kHz. Un filtrage passe-bas est généralement appliqué avant de réduire la fréquence d'échantillonnage, ce qui garantit un signal exempt de repliement spectral.

Le MPU6050 est vendu soudé sur une platine de test par DFROBOT.

Nous allons voir comment programmer le MPU6050 depuis un Arduino (UNO, NANO ou MEGA) afin d'effectuer une acquisition échantillonnée des accélérations et des vitesses de rotation, avec une transmission des données à un ordinateur. Nous verrons aussi comment réaliser une platine avec une liaison radio Xbee.

L'accès au MPU6050 depuis l'arduino se fait avec la bibliothèque de fonctions I2Cdevlib/MPU6050. I2Clibdev est une collection d'interfaces entre divers microcontrôleurs et des périphériques I2C. Il est possible de télécharger toute la bibliothèque I2Clibdev. Il suffira de recopier le dossier i2cdevlib-master/Arduino/MPU6050 et le dossier i2cdevlib-master/Arduino/I2Cdev dans le dossier Library de l'IDE Arduino.

2. Programme Arduino

Le MPU6050 peut être programmé pour effectuer des numérisations échantillonnées à une fréquence donnée. Il possède une sortie INT sur laquelle un front montant est généré à chaque fois que les données demandées ont été numérisées (accélérations et vitesses angulaires). Cette sortie est branchée sur l'entrée D2 de l'arduino, afin de générer une interruption permettant de lire ces données.

Le port I2C utilise les ports SDA et SCL de l'arduino (respectivement les bornes A4 et A5 sur l'arduino Nano).

On commence par les fichiers à inclure et les constantes codées en dur :

arduinoMPU6050.ino
#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#define SET_ACQUISITION 100
#define STOP_ACQUISITION 101
#define MAX_BUF_SIZE 180 // doit être divisible par 6
#define INTERRUPT_PIN 2 
#define TTL_OUT 9 // sortie TTL
#define DIV_TTL_OUT 4 // diviseur de fréquence
// Arduino nano : A4=SDA, A5=SCL, D2=INT
    		

La constante MAX_BUF_SIZE définie la taille maximale du tampon d'entiers 16 bits qui permettra de stocker les 3 accélérations et les 3 vitesses angulaires. Elle doit être divisible par 6. Un signal TTL synchrone avec l'échantillonnage sera généré sur la sortie D9 (pour contrôle ou pour piloter un autre système). Voici les variables globales :

MPU6050 mpu;
int16_t ax, ay, az;
int16_t buffer[MAX_BUF_SIZE];
uint16_t indice_buffer;
volatile uint8_t data_ready;
uint8_t acquisition;
uint8_t gyro;
uint8_t mpuIntStatus;
uint8_t buffer_size; // doit être divisible par 6
int8_t compteur;
char flag;
uint16_t ct;
    		 

La fonction suivante sera appelée lorsqu'une interruption matérielle est déclenchée par un front montant sur l'entrée D2. Elle met à 1 la variable data_ready, pour signaler qu'un bloc de 6 nombres entiers (accélérations et vitesses angulaires) est disponible et qu'il faut les lire au plus vite. Elle fait aussi une bascule de la sortie TTL_OUT à une fréquence égale à la fréquence d'échantillonnage divisée par DIV_TTL_OUT (ce qui donne un signal de fréquence deux fois plus faible).

void data_ready_interrupt() {
  data_ready = 1;
  ct += 1;
  if (ct==DIV_TTL_OUT) {
    ct = 0;
    if (flag==1) {
      flag=0;
      digitalWrite(TTL_OUT,LOW);
    }
    else {
      flag = 1;
      digitalWrite(TTL_OUT,HIGH);
    }
  }
}    	   		  
    		  

La fonction setup configure le port série et établit la communication avec le PC, initialise différentes variables et configure les interruptions matérielles. La vitesse du port série est basse car l'objectif et d'utiliser une liaison radio Xbee.

void setup() {
  char c;
  Wire.begin();
  Serial.begin(19200);
  Serial.setTimeout(0);
  c = 0;
  Serial.write(c);
  c = 255;
  Serial.write(c);
  c = 0;
  Serial.write(c);
  indice_buffer = 0;
  data_ready = 0;
  acquisition = 0;
  mpu.setIntDataReadyEnabled(false);
  attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),data_ready_interrupt,RISING);
  mpuIntStatus = mpu.getIntStatus();
  pinMode(TTL_OUT,OUTPUT);
  digitalWrite(TTL_OUT,LOW);
  flag = 0;
}    		   
    		   

Il y a 4 calibres pour l'accélérations (calibre 0 : +/-2g, calibre 1 : +/- 4g, calibre 2 : +/- 8g, calibre 4 : +/- 16g). Par exemple, le calibre 2g donnera une valeur 0 pour une accélération -2g, 65535 pour une accélération +2g et 32768 pour une accélération nulle (en principe car un étallonnage et un réglage du zéro seront nécessaires). Le calibre de l'accéléromètre se configure avec la fonction setFullScaleAccelRange.

Il y a 4 calibres pour le gyromètre (calibre 0 : +/- 250 deg/s, calibre 1 : +/- 500 deg/s, calibre 2 : +/- 1000 deg/s, calibre 3 : +/- 2000 deg/s). Le calibre du gyromètre se configure avec la fonction setFullScaleGyroRange.

Le filtre passe-bas numérique intégré dans le MPU6050 peut être configuré avec différentes bandes passantes :

         |   ACCELEROMETER    |           GYROSCOPE
 DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
 ---------+-----------+--------+-----------+--------+-------------
 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz  		    
    		    

Si l'on souhaite par exemple stocker et traiter les données à une fréquence de 200Hz, on aura intérêt à échantillonner à 1000Hz et à filtrer avec une bande passante de 94Hz, de manière à éviter le repliement des fréquences supérieures à 100Hz. Le prix à payer pour ce filtrage est le léger retard du signal numérique par rapport au signal analogique. Pour 94Hz, ce retard est de 3,0ms. Si l'on souhaite synchroniser ces données avec des images vidéo, ce retard est acceptable pour une cadence d'image jusqu'à 120 img/s. Le filtre passe-bas se configure avec la fonction setDLPFMode.

La fréquence d'échantillonnage de sortie (après filtrage) est obtenue par division de la fréquence indiquée dans le tableau ci-dessus. SMPLRTDIV est un nombre entier 8 bits (de 0 à 255). La fréquence d'échantillonnage est :

f0 est la fréquence d'échantillonnage de numérisation (avant filtrage), soit f0 = 8kHz si DLPF_CFG=0 et f0 = 1kHz si DLPF_CFG=1 à 6. Par exemple pour DLPF_CGF=2 et SMPLRTDIV=3, la fréquence d'échantillonnage de numérisation est 1000Hz, celle de sortie est 250Hz et la fréquence de coupure du filtre est 94Hz. En principe, la fréquence de sortie doit être égale au moins à deux fois la fréquence de coupure du filtre (d'après la condition de Nyquist-Shannon). Il faudra néanmoins que l'arduino utilisé soit capable de lire les données à cette cadence.

La fonction lecture_acquisition effectue la lecture des paramètres envoyés par le PC et configure en conséquence le MPU6050. L'appel setIntDataReadyEnabled(true) permet d'activer la génération d'un signal d'interruption sur la sortie INT lorsque des données sont disponibles.

void lecture_acquisition() {
    uint32_t c1,c2,c3,c4;
    uint8_t accel_range;
    uint8_t gyro_range;
    uint8_t rate_div;
    uint8_t dlpf_mode;
    
    while (Serial.available()<5) {};
    accel_range = Serial.read();
    gyro_range = Serial.read();
    rate_div = Serial.read();
    dlpf_mode = Serial.read();
    buffer_size = Serial.read();
    if (gyro_range<4) {gyro = 1;} else gyro = 0;
    mpu.initialize();
    mpu.setFullScaleAccelRange(accel_range);
    if (gyro) mpu.setFullScaleGyroRange(gyro_range);
    mpu.setDLPFMode(dlpf_mode);
    mpu.setRate(rate_div);
    data_ready = 0;
    acquisition = 1;
    indice_buffer = 0;
    mpu.setIntDataReadyEnabled(true);
    compteur = 0;
    ct = 0;
}    		     
    		     

La fonction suivante permet de signaler l'arrêt demandé de l'acquisition :

void stop_acquisition() {
  acquisition = 0;
  mpu.setIntDataReadyEnabled(false);
}   		     
    		     

Voici enfin la fonction loop. On rappelle que cette fonction est exécutée par le microcontrôleur de manière répétitive, tant qu'il n'a rien d'autre à faire. Cette fonction teste le port série pour savoir si un ordre est donné par le PC, puis, si un bloc de données est disponible (data_ready=1), récupère les 6 nombres entiers (16 bits) donnant les 3 accélérations et les 3 vitesses angulaires avant de les stocker dans le tampon. Lorsque le tampon est plein, il envoyé au PC par le port série. Le choix de la taille du tampon est un compromis : si le tampon est plus grand, la vitesse de transmission par octet est plus grande, mais si le tampon est trop grand sa transmission est plus longue et peut donc faire échapper certaines données envoyées par le MPU6050. Nous avons constaté qu'un tampon de taille 180 fonctionne bien.

void loop() {
   char com;
   if ((compteur==0)&&Serial.available()>0) {
        com = Serial.read();
        if (com==SET_ACQUISITION) lecture_acquisition();
        if (com==STOP_ACQUISITION) stop_acquisition();
   }
   compteur += 1;
    if ((data_ready)&&(acquisition)) {
        data_ready = 0;
        delayMicroseconds(100);
        if (gyro) {
          mpu.getMotion6(&buffer[indice_buffer],&buffer[indice_buffer+1],&buffer[indice_buffer+2],&buffer[indice_buffer+3],&buffer[indice_buffer+4],&buffer[indice_buffer+5]);
          indice_buffer += 6;
        }
        else {
          mpu.getAcceleration(&buffer[indice_buffer],&buffer[indice_buffer+1],&buffer[indice_buffer+2]);
          indice_buffer += 3;
        }
        if (indice_buffer == buffer_size) {
            indice_buffer = 0;
            Serial.write((uint8_t *)buffer,buffer_size*2);
        }
    }
}   		      
    		      

3. Programme Python

Le programme Python (python 3.x) permet de récupérer les données et de tracer accélérations et vitesses angulaires. Le fichier suivant contient la définition de la classe Arduino, qui gère les échanges avec le programme arduino décrit ci-dessus.

Le constructeur définit des constantes et établit la communication série avec l'arduino. La taille du tampon est ici fixée à 180, ce qui représente 30 échantillons d'accélérations et de vitesses angulaires, ou bien 60 échantillons d'accélération si le gyromètre n'est pas lu.

arduinoMPU6050.py
import numpy
import serial
import time
import threading

class Arduino():
    def __init__(self,port):
        self.SET_ACQUISITION = 100
        self.STOP_ACQUISITION = 101
        self.TAILLE_BLOC_INT16 = 180 # taille du tampon (<= à la taille max)
        self.TAILLE_BLOC_INT8 = self.TAILLE_BLOC_INT16*2
        self.TAILLE_BLOC_ACCEL = int(self.TAILLE_BLOC_INT16/3)
        self.TAILLE_BLOC_ACCEL_GYRO = int(self.TAILLE_BLOC_INT16/6)
        # calibres de l'accéléromètre
        self.ACCEL_FS_2 = 0
        self.ACCEL_FS_4 = 1
        self.ACCEL_FS_8 = 2
        self.ACCEL_FS_16 = 3
        self.accel_sensitivity = [16384.0,8192.0,4096.0,2048.0]
        # calibres du gyromètre
        self.GYRO_FS_250 = 0
        self.GYRO_FS_500 = 1
        self.GYRO_FS_1000 = 2
        self.GYRO_FS_2000 = 3
        self.gyro_sensitivity = [131,65.5,32.8,16.4]
        self.GYRO_NONE = 4 # pas de lecture du gyromètre
        # configuration du filtre passe-bas numérique 
        # bande passante pour l'accéléromètre, celle du gyromètre est légèrement différente.
        self.DLPF_BW_260 = 0
        self.DLPF_BW_184 = 1
        self.DLPF_BW_94 = 2
        self.DLPF_BW_44 = 3
        self.DLPF_BW_21 = 4
        self.DLPF_BW_10 = 5
        self.DLPF_BW_5 = 6
        self.gyro = 0
        self.ser = serial.Serial(port,baudrate=19200)
        c_recu = self.ser.read(1)
        while ord(c_recu)!=0:
            c_recu = self.ser.read(1)
        c_recu = self.ser.read(1)
        while ord(c_recu)!=255:
            c_recu = self.ser.read(1)
        c_recu = self.ser.read(1)
        while ord(c_recu)!=0:
            c_recu = self.ser.read(1)
            
    def close(self):
        self.ser.close()
    		 

Les trois fonctions suivantes permettent d'envoyer à l'arduino des nombres entiers, respectivement de 8 bits, 16 bits et 32 bits :

    def write_int8(self,v):
    	char = int(v&0xFF) # nécessaire pour les nombres négatifs
    	self.ser.write((char).to_bytes(1,byteorder='big'))
    	
    def write_int16(self,v):
        v = numpy.int16(v)
        char1 = int((v & 0xFF00) >> 8)
        char2 = int((v & 0x00FF))
        self.ser.write((char1).to_bytes(1,byteorder='big'))
        self.ser.write((char2).to_bytes(1,byteorder='big'))
        
    def write_int32(self,v):
        v = numpy.int32(v)
        char1 = int((v & 0xFF000000) >> 24)
        char2 = int((v & 0x00FF0000) >> 16)
        char3 = int((v & 0x0000FF00) >> 8)
        char4 = int((v & 0x000000FF))
        self.ser.write((char1).to_bytes(1,byteorder='big'))
        self.ser.write((char2).to_bytes(1,byteorder='big'))
        self.ser.write((char3).to_bytes(1,byteorder='big'))
        self.ser.write((char4).to_bytes(1,byteorder='big'))    		 
    		 

La fonction suivante lance l'acquisition avec les différentes paramètres (voir plus haut pour les explications) :

    def lancer_acquisition(self,accel_range,gyro_range,rate_div,dlpf_mode):
        if gyro_range<4: self.gyro = 1 # lecture du gyromètre
        else : self.gyro = 0
        self.write_int8(self.SET_ACQUISITION)
        self.write_int8(accel_range)
        self.write_int8(gyro_range)
        self.write_int8(rate_div)
        self.write_int8(dlpf_mode)
        self.write_int8(self.TAILLE_BLOC_INT16)
    		 

La fonction suivante renvoie la fréquence d'échantillonnage de lecture (après filtrage) en fonction de la confuguration du filtre et du facteur de division :

    def fechant(self,rate_div,dlpf_mode):
        if dlpf_mode==self.DLPF_BW_260:
            fechant = 8000/(1+rate_div)
        else:
            fechant = 1000/(1+rate_div)
        return fechant    		 
    		 

La fonction suivante renvoie, en fonction du calibre de l'accéréromètre, le facteur par lequel il faut diviser le nombre entier pour obtenir l'accélération en g.

    def echelle_accel(self,accel_range):
        return self.accel_sensitivity[accel_range] 		  
    		  

La fonction suivante renvoie, en fonction du calibre du gyromètre, le facteur par lequel il faut diviser le nombre entier pour obtenir la vitesse angulaire en deg/s.

    def echelle_gyro(self,gyro_range):
        return self.gyro_sensitivity[gyro_range]
    		   

La fonction suivante informe l'arduino qu'il faut stopper l'acquisition :

    def stopper_acquisition(self):
     	self.write_int8(self.STOP_ACQUISITION)
    		   

La fonction suivante effectue la lecture d'un bloc de données envoyé par l'arduino et renvoie le tableau correspondant. Chaque ligne du tableau correspond à une donnée (accélération sur un axe ou vitesse de rotation autour d'un axe). Si le gyromètre est utilisé, il y a 6 lignes dans ce tableau. S'il n'est pas utilisé, il y a 3 lignes.

    def lecture(self):
        buf = self.ser.read(self.TAILLE_BLOC_INT8)
        if self.gyro :
            ndata = 6
            N = self.TAILLE_BLOC_ACCEL_GYRO
        else :
            ndata = 3
            N = self.TAILLE_BLOC_ACCEL
        data = numpy.zeros((ndata,N),dtype=numpy.int16)
        j = 0
        for i in range(N):
            for k in range(ndata):
                data[k,i] = buf[j]+0x100*buf[j+1]
                j += 2
        return data    		    
    		    

Nous allons faire la lecture des données envoyées par l'arduino sur un fil d'exécution parallèle, qui se charge de la lecture du port série (qui est bloquante). On doit pour cela définir une classe qui hérite de threading.Thread. Voici le constructeur de cette classe :

class AcquisitionThread(threading.Thread):
    def __init__(self,arduino,accel_range,gyro_range,rate,dlpf_mode,nblocs):
        threading.Thread.__init__(self)
        self.arduino = arduino
        self.accel_range = accel_range
        self.gyro_range = gyro_range
        self.rate = rate
        self.dlpf_mode = dlpf_mode
        self.nblocs = nblocs # nombre de blocs maximal
        self.running = False
        if gyro_range<4:
            ndata = 6
            self.taille_bloc = self.arduino.TAILLE_BLOC_ACCEL_GYRO
        else:
            ndata = 3
            self.taille_bloc = self.arduino.TAILLE_BLOC_ACCEL
        self.data = numpy.zeros((ndata,self.taille_bloc*self.nblocs),dtype=numpy.float32)    		    
    		    

L'argument nblocs indique le nombre de blocs à acquérir, chaque bloc correspondant au contenu du tampon, soit 30 échantillons si on lit les accélérations et vitesses angulaires.

La fonction run déclenche l'acquisition puis lit les blocs de données envoyés par l'arduino et les enregistre dans le tableau self.data.

    def run(self):
        self.arduino.lancer_acquisition(self.accel_range,self.gyro_range,self.rate,self.dlpf_mode)
        self.fechant = self.arduino.fechant(self.rate,self.dlpf_mode)
        self.running = True
        self.indice_bloc = 0
        while self.running:
            i = self.indice_bloc*self.taille_bloc
            j = i+self.taille_bloc
            self.data[:,i:j] = self.arduino.lecture()
            self.indice_bloc += 1
            if self.indice_bloc==self.nblocs:
                self.stop()  		      
    		      

La fonction suivante renvoie un paquet de d'échantillons d'une certaine longueur. Elle renvoie le tableau des instants, le tableau des échantillons et la durée correspondante. Si le nombre d'échantillons demandé n'est pas encore disponible, elle renvoie les valeurs -1.

    def paquet(self,longueur):
        j = self.indice_bloc*self.taille_bloc
        i = j-longueur
        if i<0: i=0
        if j-i<=0: return (-1,-1,-1)
        temps = numpy.arange(j-i)/self.fechant
        tmax = longueur/self.fechant
        return (temps,self.data[:,i:j],tmax)    		      
    		      

La fonction suivante arrête l'acquisition.

    def stop(self):
        self.running = False
        self.join()
        self.arduino.stopper_acquisition()    		      
    		      

Voici un script qui fait une acquisition tout en traçant les accélérations et les vitesses angulaires. La fréquence d'échantillonnage est 50Hz et le filtre a une coupure à 21Hz.

testAcquisitionAnimateMPU6050.py
import numpy
from matplotlib.pyplot import *
import matplotlib.animation as animation
from arduinoMPU6050 import *


print("Presser RESET sur l'arduino si liaison Xbee")
ard = Arduino("COM12")
accel_range = ard.ACCEL_FS_2
g_max = 2
#g_max = 32000
gyro_range = ard.GYRO_FS_500
r_max = 500
dlpf_mode = ard.DLPF_BW_21
echelle_accel = ard.echelle_accel(accel_range)
#echelle_accel = 1
echelle_gyro = ard.echelle_gyro(gyro_range)
nblocs = 1000
N = nblocs*ard.TAILLE_BLOC_ACCEL_GYRO
rate_div = 19
fechant = ard.fechant(rate_div,dlpf_mode)
print("fe = %f"%fechant)
delai = 0.2
longueur_fenetre = 1000
temps = numpy.arange(longueur_fenetre)
a = numpy.zeros(longueur_fenetre)
fig,ax = subplots(2,1)
line0, = ax[0].plot(temps,a,"r",label="ax")
line1, = ax[0].plot(temps,a,"g",label="ay")
line2, = ax[0].plot(temps,a,"b",label="az")
line3, = ax[1].plot(temps,a,"r",label="rx")
line4, = ax[1].plot(temps,a,"g",label="ry")
line5, = ax[1].plot(temps,a,"b",label="rz")
ax[0].grid()
ax[0].set_xlabel("t (s)")
ax[0].set_ylabel("g")
ax[0].axis([0,longueur_fenetre,-g_max,g_max])
ax[0].legend(loc="lower left")
ax[1].grid()
ax[1].set_xlabel("t (s)")
ax[1].set_ylabel("deg/sec")
ax[1].axis([0,longueur_fenetre,-r_max,r_max])
ax[1].legend(loc="lower left")

acquisition = AcquisitionThread(ard,accel_range,gyro_range,rate_div,dlpf_mode,nblocs)
acquisition.start()


def animate(i):
    global line0,line_1,line2_,acquisition,temps,ax
    (temps,data,tmax) = acquisition.paquet(longueur_fenetre)
    if isinstance(data,int)==False:
        line0.set_ydata(data[0]/echelle_accel)
        line1.set_ydata(data[1]/echelle_accel)
        line2.set_ydata((data[2]+2109.5)/echelle_accel)
        line0.set_xdata(temps)
        line1.set_xdata(temps)
        line2.set_xdata(temps)
        ax[0].axis([0,tmax,-g_max,g_max])
        line3.set_ydata(data[3]/echelle_gyro)
        line4.set_ydata(data[4]/echelle_gyro)
        line5.set_ydata(data[5]/echelle_gyro)
        line3.set_xdata(temps)
        line4.set_xdata(temps)
        line5.set_xdata(temps)
        ax[1].axis([0,tmax,-r_max,r_max])
        
ani = animation.FuncAnimation(fig,animate,100,interval=delai*1000)
show()
acquisition.stop()
ard.close()
numpy.savetxt("accel-gyro-1.txt",acquisition.data.T,delimiter="\t")
    		      
Creative Commons LicenseTextes et figures sont mis à disposition sous contrat Creative Commons.