Textes et figures sont mis à disposition sous contrat Creative Commons.
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 8 kHz. 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.
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 :
#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
// Arduino nano : A4=SDA, A5=SCL, D2=INT
La constante MAX_BUF_SIZE définit 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. 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;
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.
void data_ready_interrupt() {
data_ready = 1;
}
La fonction setup configure le port série et établit la communication avec le PC, initialise différentes variables et configure l'interruption matérielle. La vitesse du port série est basse car l'objectif et d'utiliser une liaison radio Xbee.
void setup() {
Wire.begin();
Serial.begin(19200);
while(!Serial);
indice_buffer = 0;
data_ready = 0;
acquisition = 0;
attachInterrupt(digitalPinToInterrupt(2),data_ready_interrupt,RISING);
mpuIntStatus = mpu.getIntStatus();
compteur = 0;
}
Il y a 4 calibres pour l'accélération (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 étalonnage 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 200 Hz, on aura intérêt à échantillonner à 1000 Hz et à filtrer avec une bande passante de 94 Hz, de manière à éviter le repliement des fréquences supérieures à 100 Hz. Le prix à payer pour ce filtrage est le léger retard du signal numérique par rapport au signal analogique. Pour 94 Hz, ce retard est de 3,0 ms. 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 :
où f0 est la fréquence d'échantillonnage de numérisation (avant filtrage), soit f0=8 kHz si DLPF_CFG=0 et f0=1 kHz si DLPF_CFG=1 à 6. Par exemple pour DLPF_CGF=2 et SMPLRTDIV=3, la fréquence d'échantillonnage de numérisation est 1000 Hz, celle de sortie est 250 Hz et la fréquence de coupure du filtre est 94 Hz. 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.
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);
mpuIntStatus = mpu.getIntStatus();
}
La fonction suivante permet d'arrêter l'acquisition :
void stop_acquisition() {
acquisition = 0;
}
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. Le démarrage de l'accéléromètre est effectuée si start==true. Le port série est testé pour savoir si un ordre est donné par le PC, puis, si un bloc de données est disponible (data_ready=1), on récupère les 6 nombres entiers (16 bits) : les 3 accélérations et les 3 vitesses angulaires, qui sont stockés dans le tampon. Lorsque le tampon est plein, il est 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.
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);
}
}
}
Le programme Python 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.
class Arduino():
def __init__(self,port):
self.SET_ACQUISITION = 100
self.STOP_ACQUISITION = 101
self.TAILLE_BLOC_INT16 = 180
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)
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]
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
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)
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
else : self.gyro = 0
self.ser.write((self.SET_ACQUISITION).to_bytes(1,byteorder='big'))
self.ser.write((accel_range).to_bytes(1,byteorder='big'))
self.ser.write((gyro_range).to_bytes(1,byteorder='big'))
self.ser.write((rate_div.to_bytes(1,byteorder='big')))
self.ser.write((dlpf_mode).to_bytes(1,byteorder='big'))
self.ser.write((self.TAILLE_BLOC_INT16).to_bytes(1,byteorder='big'))
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 10 Hz et le filtre a une coupure à 5 Hz.
import numpy
from matplotlib.pyplot import *
import matplotlib.animation as animation
from ArduinoMPU6050AccelGyro import *
ard = Arduino("COM3")
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 = 9
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,line1,line2, line3,line4,line5,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()
data = acquisition.data
data[0] = data[0] / echelle_accel
data[1] = data[1] / echelle_accel
data[2] = (data[2]+2109.5)/ echelle_accel
data[3] = data[3] / echelle_gyro
data[4] = data[4] / echelle_gyro
data[5] = data[5] / echelle_gyro
numpy.savetxt("accel-gyro-1.txt",data.T,delimiter="\t")
Textes et figures sont mis à disposition sous contrat Creative Commons.