Dans cette partie, je montre comment lire et traiter les
données venant du Gyroscope. Plus précisément, nous allons
extraire les données du Gyro afin que le contrôleur de vol puisse
calculer les angles parcourus par le quadrotor pour les axes de mouvements
(Pitch, Roll et Yaw) en utilisant des opérations mathématiques
simples, puis les transformer en instructions algorithmiques adaptées
à Arduino.
Le Gyro est un capteur de taux d'angle à trois axes
(x, y et z), il mesure la vitesse angulaire en degrés par seconde de ces
axes en même temps. Les données d'entrées du Gyro sont
couplées linéairement avec sa sortie, signifie que la longueur de
l'impulsion de sortie change proportionnellement lorsque le Gyro est
tourné autour de ces axes. Pour connaître la valeur de sortie
exacte de l'MPU-6050, nous devons jeter un coup d'oeil sur la fiche technique
du Gyro MPU6050 mentionnée dans la page (12 of 52) de l'annexe de ce
mémoire. La sortie sera de 65,5 en décimal lorsque la vitesse
angulaire du Gyro est de 1°/sec, car nous utilisons 500°/sec comme
vitesse angulaire maximale pour les trois axes du Gyro.
Chapitre III Conception et mise en oeuvre du
fonctionnement
Par exemple si le Gyro fait tourner autour de son axe de lacet
(Yaw) et que nous terminons le tour complet (360°) exactement à une
minute (60 sec), la sortie du Gyro à n'importe quel moment de ce
parcourt sera de 6°/sec × 65.5 = 393, car à chaque seconde le
Gyro tourne 6°. Dans le programme Arduino il sera donc :
Angle°/sec = sortie Gyro / 65,5. Le résultat
« Angle°/sec » sera déclarée comme une variable
flottante et la sortie du Gyro est une valeur signée de 16 bits (la
sortie du gyroscope est une valeur de complément de 16 bits deux) qui
est similaire à un entier « Int » dans l'Arduino. Cela
signifie que la variable « sortie Gyro » doit être
déclarée comme un entier car elle peut atteindre 32750 si la
vitesse angulaire du Gyroscope atteint les 500°/sec (sortie Gyro =
500°/sec × 65.5 = 32750). La plupart des Gyro ont un petit
décalage moyen (offset) qui provoque la dérive du quadrotor donc
le Gyroscope doit être calibré avant d'utiliser ces données
de sortie. Pour obtenir les meilleurs résultats, la sortie du Gyro doit
être mise à zéro et ceci est simplement fait en prenant
l'offset pour plusieurs lectures et en les soustrayant des plusieurs valeurs de
la sortie du Gyro. Ceci est expliquer dans cet organigramme de boucle for (;;)
qui paresse dans le void setup :
Variables entiers 16 bits : Int_cal = 0
Initialiser les
Variables 8 bits : lowByte, highByte
Initialiser les Variables 64 bits :
gyro_pitch,
gyro_roll, gyro_yaw, cal_axe_x, cal_axe_y,
cal axe z
Int_cal < 2000 ?
Pour exécuter Non
Le code 2000 fois Oui
L'axe Pitch
L'axe Roll
L'axe Yaw
Ajouter la valeur lue gyro_pitch
de l'axe x du Gyro
à la variable
cal_axe_x
Ajouter la valeur lue gyro_roll
de l'axe y du Gyro à
la variable
cal_axe_y
Ajouter la valeur lue gyro_yaw
de l'axe z du Gyro à
la variable
cal_axe_z
Incrémenter par 1
Int cal + +
Sous-programme de lecture des
données du Gyro
MPU-6050
Tarder 3 ms
49
Diviser la variable
cal_axe_x par 2000 pour
obtenir
l'offset de l'axe x
Diviser la variable
cal_axe_y par 2000 pour
obtenir
l'offset de l'axe y
Diviser la variable
cal_axe_z par 2000 pour
obtenir
l'offset de l'axe z
Reste du programme.
Chapitre III Conception et mise en oeuvre du
fonctionnement
50
Cet organigramme calcule l'offset moyenne de 2000 lectures de
Gyro pour les 3 axes, puis il stocke chacune de ces valeurs d'offset dans les
variables (cal_axe_x, cal_axe_y et cal_axe_z). Après
cette étape l'offset est soustraite de chaque mesure du Gyro dans le
reste de la boucle du programme.
· Sous-programme de lecture des données du
Gyro :
L'Arduino Uno du contrôleur de vol lit les
données des angles parcouru par le Gyro à partir des registres de
sortie d'adresses 0x69 et 0xA8 qui sont à 16 bits en utilisant le
protocole de communication I2C. Par exemple pour lire l'axe «
Yaw » on lit les 8 bits du poids fort du registre 16 bit à part,
puis les 8 bits du poids faible et on fait un décalage pour avoir les 16
bit puis on affecte le résultat au variable « gyro_yaw ».
Toutes les autres parties intéressantes pour la
communication I2C avec le Gyro sont gérées par la
librairie "wire" qui nous facilitent la tâche.
L'organigramme qui explique le processus est situé dans
la page suivante.
Chapitre III Conception et mise en oeuvre du
fonctionnement
Commencer la communication I2C
avec le Gyro
(registre d'adresse 0x69)
Commencer à lire du registre
d'adresse 0xA8
Fin de transmission
Demander 6 octets du
registre d'adresse 0x69
Attendre jusqu'à les 6 octets Soient reçus
Octets valables < 6 ?
Non
Oui
Réception du 1ier
Byte du
poids
faible des données
angulaire lowByte
Compenser l'offset Après le calibrage Jusqu'à
Int_cal = 2000
Int_cal = 2000?
Non
Oui
gyro_roll =
cal_axe_y -
gyro_roll
Int_cal = 2000?
Oui Non
gyro_pitch =
cal_axe_x -
gyro_pitch
Non
Oui
gyro_yaw =
cal_axe_z -
gyro_yaw
Fin du
sous-
programm
Int_cal = 2000?
Réception du 2nd
Byte du poids
fort
des données
angulaire highByte
gyro_pitch =
(highByte X 256) +
lowByte
gyro_roll = (highByte X 256) + lowByte
Décalage de highByte
Par 8 positions
gyro_yaw = (highByte X 256) + lowByte
51
Après l'étape de calibrage, il est
nécessaire de calculer l'angle total parcouru par le Gyro afin que le
contrôleur de vol puisse reconnaître la position exacte du
quadrotor dans l'espace. Pour cela il suffit simplement d'intégrer au
fil de temps tous les valeurs de sortie Gyro (les données de la vitesse
angulaire des axes Pitch, Roll et Yaw) par exemple pour l'axe Yaw :
Chapitre III Conception et mise en oeuvre du
fonctionnement
52
Angle parcouruya11, =E sortie
Gyroya11, x At.
Dans le cas de notre contrôleur de vol, la
fréquence de rafraîchissement est de 250 Hz, on peut donc extraire
les angles parcouru par le Gyro pour chaque axe toutes les 4 ms au lieu de 1s.
Cela signifie également que nous devons diviser les données de
sortie Gyro sur 65,5 et ensuite diviser à nouveau sur 250 pour obtenir
l'angle parcouru par le quadrotor chaque 4 ms.
J contrôleur de vol = 250 Hz - At = 4 ms
Angle parcouruyau, = 1(sortie Gyroyau,
/(250/65.5))
= E(sortie Gyroyau, x 0.0000611)
L'intégrale en mathématique signifie la
sommation d'une fonction sur un intervalle donné, donc dans le code
Arduino on fait la somme de toutes les données gyroscopiques de sortie
pour chaque axe à part dans un intervalle de 4ms, par exemple pour l'axe
z « Yaw » se traduire par cette instruction :
Angle parcouruyau, est : Angle_yaw_entrée +=
gyro_z * 0.0000611;
C.à.d. : Angle_yaw_entrée =
Angle_yaw_entrée précédente +
(gyro_z x 0.0000611) Vérifions la
validité de cette relation avec le dernier exemple pour une rotation
complète (360°) du Gyro autour de son axe (Yaw) dans 60s, on aura
:
· La sortie du Gyro en décimal à n'importe
quel moment pendant la rotation est : sortie Gyroyau, = 393.
· Pendant la rotation, les données du Gyro sont
ajoutées 250 fois par seconde donnant un total de 15000 lectures :
250lecture P/s x 60s = 15000 lecture
· Pour une minute on aura donc :
Angle parcouruyau, = 15000 lecture x 393 x 0.0000611
= 360.18° 360°
Ce qui est assez proche pour un tour complet, d'où la
formule est juste.
L'inconvénient du Gyro MPU6050 c'est que le signal de
sortie est très bruyant Figure 3.27.
L'utilisation d'un simple filtre complémentaire permet
de réduire le bruit qui accompagne le signal
de sortie du Gyro, alors l'instruction précédente
devienne:
Angle_yaw_entrée = (Angle_yaw_entrée *
0.7) + (gyro_z * 0.0000611);
