Mesurer des angles avec un Arduino - drone ch.7

1. Introduction

Salut les makers,

Aujourd'hui je vous propose un article sur le capteur MPU6050 qui est un gyroscope/accéléromètre 3 axes.

On a déjà vu dans le chapitre 5 comment utiliser le MPU6050. Pourquoi un autre chapitre là-dessus ?

Et bien parce que après réflexion, il s'avère que la librairie utilisée dans le chapitre 5 ne permet pas d'avoir une lecture des mesures à une fréquence suffisamment élevée pour notre drone.

Quoi qu'il en soit, toute la partie théorique sur le fonctionnement des gyroscopes/accéléromètres et la présentation du capteur MPU6050 restent valables. Aussi je vous invite à les lire si ce n'est déjà fait.

Nous allons aujourd'hui voir comment mettre en place un IMU : Inertial Measurement Unit.

2. Câblage

Le MPU6050 utilise le protocole de communication I²C dont la connexion est réalisée par l'intermédiaire de deux lignes :

  • SDA (Serial Data Line) : ligne de données bidirectionnelle
  • SCL (Serial Clock Line) : ligne d'horloge de synchronisation bidirectionnelle

Dans le monde Arduino, il existe une librairie dédiée à ce protocole : Wire.

En lisant la doc de la librairie Wire Arduino, on voit que sur un Arduino Uno ce sont les entrées analogiques A4 et A5 qui correspondent respectivement à SDA et SCL. On cable donc tout ça ensemble.

Le MPU a besoin d'être alimenté en 3.3V, on relie donc la broche Vcc à la broche 3.3V de l'Arduino et le GND au GND de l'Arduino.

Ce qui nous donne au final ce câblage :

Wiring
Cablage du MPU6050 avec Arduino Uno

3. Configurer le MPU6050

3.1 Sensibilité du gyroscope

Le capteur MPU6050 embarque son propre processeur (Digital Motion Processor) et qui dit processeur dit datasheet, comme pour l'ATMega de notre Arduino Uno.

En fouillant dans la datasheet des registres du MPU6050, on peut voir page 14 la description du registre 27 "Gyroscope Configuration", de son petit nom GYRO_CONFIG. On peut y lire que le gyroscope peut être utilisé avec différentes précisions en fonction du facteur de sensibilité FS_SEL :

SSF Gyro
Sensibilités du gyroscope

Dans notre cas, une précision de ±500°/sec est largement suffisante, ce qui nous amène à utiliser le facteur FS_SEL = 1.

Gyro Config register
Structure du registre GYRO_CONFIG

On voit que ce registre a une taille de 8 bits et que FS_SEL est codé sur les bits 3 et 4. La valeur 1 en binaire s'écrit donc 01.
Les bits 0, 1 & 2 sont réservés, on les laisse donc à 0.
Les bits 5, 6 & 7 servent à effectuer un auto-test des axes X, Y & Z. Ça ne nous intéresse pas, on les laisse donc à 0.

Auto test params
Paramètres d'auto-test du gyroscope

Tout ceci nous donne au final 00001000 en binaire, soit 0x08 en hexadécimal.
Enfin, l'adresse du registre est 0x1B.

Ne me croyez pas sur parole, allez fouiller dans la datasheet. C'est toujours très instructif et formateur smiley

Nous avons maintenant tous les éléments pour configurer ce registre à l'aide de la librarie Wire :


Wire.beginTransmission(0x68); // Start communication with MPU
Wire.write(0x1B);             // Request the GYRO_CONFIG register
Wire.write(0x08);             // Apply the desired configuration to the register : +-500°/s
Wire.endTransmission();       // End the transmission

3.2 Sensibilité de l'accéléromètre

Même tarif pour l'accéléromètre : le registre ACCEL_CONFIG permet de configurer la sensibilité de l'accéléromètre.

FS_SEL accéléromètre
Sensibilités de l'accéléromètre

Une précision de ±8g est suffisante pour notre drone, ce qui nous amène à utiliser le facteur AFS_SEL = 2.

Acc config register
Structure du registre ACCEL_CONFIG

La paramètre AFS_SEL est codé sur les bits 3 & 4. La valeur 2 en binaire s'écrit alors 10.
Les bits 0, 1 & 2 sont réservés, on les laisse donc à 0.
Les bits 5, 6, 7 servent à effectuer un auto-test des axes X Y Z respectivement. Ça ne nous intéresse pas, on les laisse donc à 0.

Auto test params
Paramètres d'auto-test de l'accéléromètre

Ce qui nous donne : 00010000 en binaire, soit 0x10 en hexadécimal.
Enfin, l'adresse du registre ACCEL_CONFIG est 0x1C en hexadécimal.

Ce qui nous donne le code Arduino suivant :


Wire.beginTransmission(0x68); // Start communication with MPU
Wire.write(0x1C);             // Request the ACCEL_CONFIG register
Wire.write(0x10);             // Apply the desired configuration to the register : +-8g
Wire.endTransmission();       // End the transmission

3.3 Fréquence d'horloge

Plus loin dans la datasheet, on arrive sur la page du registre Power Management. Ce registre nous permet de choisir la source d'horloge que le MPU va utiliser au travers du paramètre CLKSEL.

CLKSEL
Sources d'horloge

Nous allons utiliser ici son horloge interne de 8MHz, soit CLKSEL = 0.

Power register
Structure du registre PWR_MGMT_1

Là encore, on voit que ce registre est codé sur 8 bits.
CLKSEL est codé sur les bits 0, 1 & 2. La valeur 0 en binaire s'ecrit alors 000.
Le bit 3 TEMP_DIS (temparture disabled) sert à désactiver la lecture de la température lorsqu'il est à 1. On va garder cette information, on laisse donc ce bit à 0.
On laisse le reste à 0, ce qui nous donne au final 00000000 en binaire, soit 0x00 (hex).
Enfin, l'adresse du registre est 0x6B en hexadécimal.

Ce qui nous donne le code Arduino suivant :


Wire.beginTransmission(0x68); // Start communication with MPU
Wire.write(0x6B);             // Request the PWR_MGMT_1 register
Wire.write(0x00);             // Apply the desired configuration to the register
Wire.endTransmission();       // End the transmission

3.4 Filtre passe-bas

Allez courage, plus qu'un dernier registre à configurer smiley

Le dernier registre, de son petit nom CONFIG, va nous servir à configurer la fréquence de coupure du filtre passe-bas du gyroscope et de l'accéléromètre au travers du paramètre DLPF_CFG.

DLPF
Digital Low Pass Filter

On va partir sur une fréquendce d'environ ~43Hz, ce qui nous amène à DLPF_CNG = 3.

CONFIG_REG
Structure du registre CONFIG

Ce dernier est codé sur les bits 0 à 2. La valeur 3 en binaire s'écrit alors 011.
On laisse les autres bits à 0, ce qui nous amène à 00000011 en binaire, soit 0x03 en héxadécimal.
Enfin, l'adresse du registre est 0x1A en héxadécimal.

Ce qui nous amène au code Arduino suivant :


Wire.beginTransmission(0x68); // Start communication with MPU
Wire.write(0x1A);             // Request the CONFIG register
Wire.write(0x03);             // Apply the desired configuration to the register : DLPF about 43Hz
Wire.endTransmission();       // End the transmission

3.5 Configuration complète

Notre capteur est maintenant configuré au poil de cul, mais comme on aime le code bien structuré, on range tout ça dans une fonction, ce qui donne le code tout propre suivant :


#define MPU_ADDRESS 0x68  // I2C address of the MPU-6050

/**
 * Configure la précision du gyroscope et de l'accéléromètre :
 *  - accéléromètre: +/-8g
 *  - gyroscope: 500°/sec
 *
 * @return void
 */
void setupMpu6050Registers() {
    // Configure power management
    Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
    Wire.write(0x6B);                    // Request the PWR_MGMT_1 register
    Wire.write(0x00);                    // Apply the desired configuration to the register
    Wire.endTransmission();              // End the transmission
  
    // Configure the gyro's sensitivity
    Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
    Wire.write(0x1B);                    // Request the GYRO_CONFIG register
    Wire.write(0x08);                    // Apply the desired configuration to the register : ±500°/s
    Wire.endTransmission();              // End the transmission
  
    // Configure the acceleromter's sensitivity
    Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
    Wire.write(0x1C);                    // Request the ACCEL_CONFIG register
    Wire.write(0x10);                    // Apply the desired configuration to the register : ±8g
    Wire.endTransmission();              // End the transmission
  
    // Configure low pass filter
    Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
    Wire.write(0x1A);                    // Request the CONFIG register
    Wire.write(0x03);                    // Set Digital Low Pass Filter about ~43Hz
    Wire.endTransmission();              // End the transmission
}

4. Lire les données brutes du MPU6050

Maintenant que le MPU6050 est câblé et configuré, il ne reste plus qu'à lire les données brutes du capteur.

On constate dans la table des registres que les registres 59 à 72 stockent les valeurs de sorties de l'accéléromètre, de la température et du gyroscope et qu'ils commencent à l'adresse 0x3B.

Registers map
Table des registres

On va donc simplement requêter le contenu de ces 14 registres, en commençant à l'adresse 0x3B : 


#include <Wire.h>

#define MPU_ADDRESS 0x68  // I2C address of the MPU-6050

int acc_raw[3] = {0,0,0};
int gyro_raw[3] = {0,0,0};
int temperature = 0;

void readSensor() {
    Wire.beginTransmission(MPU_ADDRESS);// Start communicating with the MPU-6050
    Wire.write(0x3B);                   // Send the requested starting register
    Wire.endTransmission();             // End the transmission
    Wire.requestFrom(MPU_ADDRESS,14);   // Request 14 bytes from the MPU-6050

    // Wait until all the bytes are received
    while(Wire.available() < 14);

    acc_raw[X]  = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[X] variable
    acc_raw[Y]  = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Y] variable
    acc_raw[Z]  = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Z] variable
    temperature = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the temperature variable
    gyro_raw[X] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[X] variable
    gyro_raw[Y] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Y] variable
    gyro_raw[Z] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Z] variable
}

5. Mesures angulaires avec le gyroscope

5.1 Conversion des données brutes

Maintenant que l'on est capable de lire des données brutes du capteur, reste à les interpréter.

On a configuré le facteur de sensibilité du gyroscope FS_SEL = 1 pour une précision de ±500°/sec. On constate alors dans la doc que la valeur brute retournée est $65.5$ lorsque la vitesse angulaire est de 1°/s.

SSF gyroscope
Sensitivity Scale Factor gyroscope

AInsi, pour obtenir la valeur réelle il suffit de diviser la valeur brute par $65.5$.

Exemple
Si le gyro fait un tour complet en 1 minute, ça signifie qu'il fait 360° en 60s, soit 6°/s en moyenne. La valeur en sortie du gyro pour une telle vitesse angulaire est de 393. Si on la divise par le facteur de sensibilité :

$\dfrac{393}{65.5} = 6°.s^{-1}$

On retombe bien sur la valeur de 6°/s !

Si maintenant le gyro tourne à une vitesse de 500°/s (ce qui sera notre limite max), on obtient en sortie une valeur de $500 \times 65.5 = 32750$
Cette valeur correspond pratiquement à la valeur maximum d'un integer (32767). On sait déjà qu'on pourra stocker les valeurs brutes du gyroscope dans des variables de type integer.

5.2 Calcul des offsets

Nous sommes maintenant capables d'interpréter les valeurs lues.

Voici un relevé de mesures brutes du gyro alors que ce dernier ne bouge pas et est horizontal.

gyro raw X gyro raw Y gyro raw Z
Relevés brutes des axes X, Y et Z

Alors que les valeurs devraient tourner autour de 0, elles sont décalées par rapport à la réalité. Il convient alors d'estimer ce décalage pour le compenser.

Pour faire ça, on va simplement prendre un lot de mesures et calculer leur valeur moyenne. Ça nous donnera l'offset moyen à appliquer sur chaque axe.

Et voici comment l'implémenter en langage Arduino:


// On déclare quelques constantes pour la lisibilité
#define X 0
#define Y 1
#define Z 2

float gyro_offset[3] = {0,0,0};
int   gyro_raw[3]    = {0,0,0};

/**
 * Calibrage du MPU6050 : on prend 2000 échantillons et on calcule leur valeur moyenne.
 * Durant cette étape, le drone doit rester sur une surface horizontal sans bouger.
 *
 * @return void
 */
void calibrateMpu6050()
{
    int max_samples = 2000;

    for (int i = 0; i < max_samples; i++) {
        readSensor();

        gyro_offset[X] += gyro_raw[X];
        gyro_offset[Y] += gyro_raw[Y];
        gyro_offset[Z] += gyro_raw[Z];

        // Just wait a bit before next loop
        delay(3);
    }

    // Calculate average offsets
    gyro_offset[X] /= max_samples;
    gyro_offset[Y] /= max_samples;
    gyro_offset[Z] /= max_samples;
}

Il ne reste alors qu'à soustraire ces offsets aux valeurs brutes avant de les diviser par le facteur de sensibilité:


void loop()
{
	readSensor();

	real_value[X] = (gyro_raw[X] - gyro_offset[X]) / SSF_GYRO;
	real_value[Y] = (gyro_raw[Y] - gyro_offset[Y]) / SSF_GYRO;
	real_value[Z] = (gyro_raw[Z] - gyro_offset[Z]) / SSF_GYRO;
}

C'est bien joli tout ça mais comment on fait pour avoir des valeur en degrés directement ?

C'est vrai que le pour le moment on a du degré/seconde or on veut bel et bien des degrés tout court !

5.3 Conversion en degrés

Reprenons notre exemple d'un tour complet en 1 minute, soit 6°/s.

rotation

On a en sortie une valeur brute de 393.

Si chaque seconde (à intervalles réguliers donc) on additionne les valeurs relevées :
$393 \times 60 = 23580$

Si maintenant on divise cette valeur par le coefficient trouvé dans la datasheet :
$\dfrac{23580}{65.5} = 360°$
Et bim, on retombe sur notre valeur de 360° !

Heu... on vient de faire quoi là exactement ?

Le fait de sommer des échantillons à intervalles réguliers s'appelle intégrer. Il s'agit de la même fonction intégrale que l'on voit en Terminale S.
Cool, les maths de lycée ne servent pas complètement à rien !

La vitesse angulaire est la dérivée, par rapport au temps, de la position angulaire. Du coup sa semble logique qu'en intégrant la vitesse on retombe sur la position angulaire smiley

En sommant toutes les secondes, on est capable de connaitre l'angle du gyroscope toutes les secondes. On commence à toucher au but mais ça n'est pas très précis. Avec une donnée par seconde, le drone va avoir le temps de partir en vrille avant qu'on ait effectué la moindre correction !

Du coup, on va tabler sur une fréquence de rafraichissement de 250Hz. En dessous de 200Hz, notre asservissement risque d'être inefficace.
On parle quand même de faire tenir un machin dans les airs, ça implique nécessairement un asservissement balaise !

250Hz signifie une période de 4ms ce qui nous permet d'intégrer toutes les 4ms au lieu de toutes les 1 seconde. Concrètement, ça veut dire qu'on a une mesure angulaire toutes les 4ms.
Ça signifie également qu'on doit alors diviser la valeur brute du gyro par 250.

En langage mathématique, ça se traduit comme ceci

$$\sum_{i=1}^{n} \dfrac{raw_i - offset}{SSF \times 250}$$

Intégrer dans le domaine continue se fait avec le symbole $\int$ et intégrer dans le domaine discret (numérique) se fait avec le symbole $\sum$, mais ils représentent au final la même chose.

Et en langage Arduino, ça se traduit comme ça:


angle[PITCH] += (gyro_raw[X] - gyro_offset[X]) / (250 * SSF_GYRO);
angle[ROLL]  += (gyro_raw[Y] - gyro_offset[Y]) / (250 * SSF_GYRO);

Application numérique

Vérifions tout ça avec notre exemple de rotation en 1 minute.

Ça prend 60 secondes pour faire 360°. Durant cette rotation, on effectue 250 lectures par seconde, donc $250_{lectures} \times 60_{secondes} = 15000_{lectures}$ durant ces 60 secondes.
Le gyroscope nous donne une valeur brute de 393. Du coup, on déduit l'angle parcouru de la sorte :
$$\dfrac{15000 \times 393}{250 \times 65.5} = 360°$$

Impec, ça fonctionne !

Et vous pouvez tester avec 30sec, 5sec, 0.5sec si ça vous chante, vous obtiendrez alors la position angulaire correspondante.

5.4 Transfert de rotation de l'axe Z

Il nous reste encore un problème à résoudre.

Lorsque je positionne l'IMU sur un plan incliné à 45° comme ci-dessous, il mesure bien 45° sur son axe de roulis :

Z rotate 1
IMU posé sur un plan incliné

Maintenant, si on le fait pivoter sur son axe Z (lacet/yaw), l'axe de roulis est maintenant parallèle au sol, il devrait donc mesurer un angle de 0° et pourtant la mesure indique toujours 45° !

Z rotate 2
IMU après rotation de l'axe Z

Et pour cause, l'axe de roulis n'a subi aucune accélération, le gyroscope n'a donc mesuré aucun changement de vitesse angulaire.

Pour prendre en compte ce phénomène, il faut effectuer un transfert d'angle en utilisant simplement la fonction $sin()$ :


pitch_angle += roll_angle * sin(yaw_raw)

En appliquant ce dernier correctif, ça nous donne le code suivant :


#include <Wire.h>

#define MPU_ADDRESS 0x68 // I2C address of the MPU-6050
#define SSF_GYRO    65.5 // Sensitivity Scale Factor of the gyro

#define X  0     // X axis
#define Y  1     // Y axis
#define Z  2     // Z axis
#define FREQ 250 // Sampling frequency

int acc_raw[3]  = {0,0,0};
int gyro_raw[3] = {0,0,0};
int period;
int temperature = 0;
unsigned long int loop_timer;
unsigned long int now; // Exists just to reduce the calls to micros()
float gyro_angle[3]  = {0,0,0};
float gyro_offset[3] = {0,0,0};

void setup() {
  // Setup MPU registers
  setupMpu6050Registers();

  // Calibrate MPU6050 calculating average offset of each axis
  calibrateMpu6050();

  // Calculate period in µs
  period = (1000000/FREQ);

  // Init loop timer
  loop_timer = micros();
}

void loop() {
  readSensor();
  
  calculateGyroAngles();  

  // Do nothing until sampling period is reached
  while((now = micros()) - loop_timer <  period);
  loop_timer = now;
}

/**
 * Calculate angles with gyro data
 */
void calculateGyroAngles()
{
  // Substract offsets
  gyro_raw[X] -= gyro_offset[X];
  gyro_raw[Y] -= gyro_offset[Y];
  gyro_raw[Z] -= gyro_offset[Z];
  
  // Angle calculation
  gyro_angle[X] += gyro_raw[X]   / (FREQ * SSF_GYRO);
  gyro_angle[Y] += (gyro_raw[Y]) / (FREQ * SSF_GYRO);

  // Transfer roll to pitch if IMU has yawed
  gyro_angle[Y] -= gyro_angle[X] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
  gyro_angle[X] += gyro_angle[Y] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
}

Super ça marche ! Mais pour un court moment seulement...

gryo drift
Dérive du gyroscope

Après quelques instants et bien que le capteur ne bouge pas, on constate que les valeurs mesurées dérivent et s'éloignent de plus en plus de leur valeur réelle. C'est un phénomène connu des gyroscopes appelé la dérive.
Les vibrations des moteurs vont accélérer ce phénomène et on va vite se retrouver avec des mesures complètement foireuses.

Autre point problématique, si on démarre le capteur sur un plan incliné, il va prendre cette inclinaison comme référence du 0°. Dans certains cas ça peut être utile mais dans le nôtre c'est carrément problématique !

Pas de panique, nous allons utiliser l'accéléromètre pour corriger tout ça cool

6. Mesures angulaires avec l'accéléromètre

Un accéléromètre mesure l'accélération subite et l'exprime en $g$. $1g$ représente l'accélération de pesanteur sur Terre, soit environ $9,8m.s^{-2}$ et des bananes.

L'accéléromètre au repos mesure donc une accélération de $1g$.

Le schéma suivant est une analogie du fonctionnement d'un accéléromètre. On peut voir une masse (en jaune) attachée par des fils aux axes X et Y, orthogonaux entre eux.

imu1

L'accéléromètre mesure l'accélération subite sur chaque axe.

Le vecteur gravité (en vert) de la masse jaune n'est autre que la somme des deux vecteurs X et Y :

imu2

On se se retrouve alors avec un triangle rectangle donc l'angle adjacent n'est autre que l'angle $\alpha$ que l'on cherche.

On peut donc utiliser Pythagore pour calculer la norme du vecteur gravité : $G = \sqrt{X^2 + Y²}$

A partir de là, on peut calculer l'angle adjacent:
$\alpha = asin(\dfrac{X}{G})$

On a donc un calcul en deux temps :

  1. Calul du vecteur gravitation
  2. Calcul des angles pitch/roll

En langage Arduino, ça se traduit de la sorte:


#define X  0        // X axis
#define Y  1        // Y axis
#define Z  2        // Z axis

int acc_raw[3]     = {0,0,0};
float acc_angle[3] = {0,0,0};
float acc_total_vector;

/**
 * Calculate angles using the 3 axis accelerometer.
 */
void measureAccelerometerAngles()
{
  // Calculate total 3D acceleration vector : √(X² + Y² + Z²)
  acc_total_vector = sqrt(pow(acc_raw[X], 2) + pow(acc_raw[Y], 2) + pow(acc_raw[Z], 2));

  // To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
  if (abs(acc_raw[X]) < acc_total_vector) {
    acc_angle[X] = asin((float)acc_raw[Y] / acc_total_vector) * (180 / PI); // asin gives angle in radian. Convert to degree multiplying by 180/pi
  }

  if (abs(acc_raw[Y]) < acc_total_vector) {
    acc_angle[Y] = asin((float)acc_raw[X] / acc_total_vector) * (180 / PI);
  }
}

Et on peut voir que le relevé de mesures est très précis !

Acc angles
Mesures angulaires à l'accéléromètre

Alors pourquoi on s'emmerde avec le gyroscope, autant utiliser directement l'accéléromètre, non ?

Les vibrations ! Encore et toujours. Voyez plutôt à quoi ressemblent les mesures lorsque le capteur vibre :

acc vibrate
Relevés de l'accéléromètre avec vibrations

La mesure angulaire à l'aide d'un accéléromètre est très sensible aux vibrations et les mesures deviennent alors inutilisables.

7. Renforcement des mesures

Il est temps de faire un récapitulatif de ce qu'on vient de voir :

  • L'IMU gyroscope est moins sensible aux vibrations que l'IMU accéléromètre
  • IMU gyroscope dérive dans le temps
  • IMU gyroscope prend l'inclinaison de départ comme référence du 0°
  • Les vibrations rendent l'IMU accéléromètre inutilisable

Il va falloir de tirer parti du meilleur des deux instruments pour avoir des mesures exploitables.

7.1 Compenser la dérive du gyroscope

Pour compenser la dérive du gyroscope, on va simplement utiliser un peu des mesures des deux instruments :


pitch_angle = pitch_angle_gyro * 0.9996 + pitch_angle_acc * 0.0004;
roll_angle = roll_angle_gyro * 0.9996 + roll_angle_acc * 0.0004;

On prend 99.96% de la mesure gyroscope et 0.04% de la mesure de l'accéléromètre et c'est tout !

7.2 Démarrage sur plan incliné

Il nous reste un dernier problème à résoudre : le démarrage sur plan incliné (ou simplement pas parfaitement horizontal).

On va simplement initialiser les mesures du gyroscope avec les mesures de l'accéléromètre. Ça nécessite par contre que le drone ne bouge pas pendant cette phase.
Une fois les valeurs initialisées, on peut utiliser le mix des deux instruments :


if (initialized) {
	pitch_angle = pitch_angle_gyro * 0.9996 + pitch_angle_acc * 0.0004;
	roll_angle  = roll_angle_gyro  * 0.9996 + roll_angle_acc  * 0.0004;
} else {
	pitch_angle_gyro = pitch_angle_acc;
	roll_angle_gyro  = roll_angle_acc;

	initialized = true;
}

8. Implémentation finale

Pour la suite, nous allons nous baser sur cette figure pour définir les sens positifs de rotation :

YPR
Sens de rotation

C'est totalement arbitraire, mais il faut bien poser une référence.

En clair, ça signifie :

  • aile gauche vers le haut : roulis (roll) positif
  • nez vers le haut : tangage (pitch) positif
  • nez vers la droite : lacet (yaw) positif

Au final, on obtient le code suivant, disponible sur mon repo GitHub : https://github.com/lobodol/IMU

Le code final est un peu volumineux, vous vous en doutez. C'est pourquoi je ne l'affiche directement pas ici.

9. Conclusion

Nous voici arrivés à la fin de ce chapitre un peu complexe, je vous l'accorde, mais néanmoins indispensable pour la suite.

Nous sommes maintenant capables de recevoir des instructions de vol en radiofréquences, de mesurer l'inclinaison du drone avec une fréquence de rafraichissement de 250Hz, il ne nous reste plus qu'à attaquer l'asservissement du drone !

Mais nous verrons ça dans le prochain chapitre smiley

À bientôt sur Fire-DIY !

Sources: [1] [2]

Vos réactions (12) :

MilesHigh

Merci pour ce super tuto, encore une fois.

Concernant l'accéléromètre, il n'est utilisable que quand le vol est stable et non "accéléré". Si le drone accélère en ligne droite, la verticale ressentie par les accéléromètres est faussée par l'accélération. Il en va de meme lors d'un virage coordonné. C'est comme sur un vélo, quand on tourne, on s'incline... Mais la "verticale" ressentie se trouve dans le plan du cadre du vélo.

J'attend avec impatience l'asservissement :)

20/08/2018 à 12:58

TK

Salut lobodol.
Très interessant.
Ceci dit, les vibrations on l'avantage d'être "sinusoidales" théoriquement non ? (ou à peut prêt). Tu dois donc retomber à 0 lors de l’intégration non ? (pic+ + pic- = pic null)

@MilesHigh : l'accéléromètre projette dans 3 axes relatif. Accélérer en XY ne change pas le niveau du Z (gravité si on reste dans le plan). Corriger avec l'angle rXrYrZ tu passes dans un repère "absolue" et tout roule, que tu accélères avec les moteurs et un angle ou si le vent te chasse à "plat". Idéalement, avec une intégration de la poussé des moteurs, le drone serait capable de connaitre le chemin précis qu'il effectue dans toutes conditions depuis son départ avec uniquement un accéléromètre et un gyroscope.

20/08/2018 à 17:08

lobodol

Yo TK, les vibrations sont anarchiques donc elles sont à prendre en compte. A la limite on pourrait filtrer les perturbations en connaissant leur fréquence de résonance

20/08/2018 à 17:18

AbderrazakErrahhali

Merci pour ce tuto, j'attend avec impatience l'asservissement du drone.

20/08/2018 à 22:19

toclim

Merci pour ce tuto très bien fait. Je me demande le MPU6050 gagne il en précision quand il est en mode 500°/s plutôt que 2000?

21/08/2018 à 22:47

lobodol

C'est pas qu'il gagne en précision, c'est que c'est pas utile d'avoir une précision plus poussée. Ça utilise de l'espace mémoire inutilement

21/08/2018 à 23:47

toclim

En fait j'ai re réfléchit a la question. C'est du détail mais il y a une petite confusion entre précision et plage de mesure. Quand on parle de +/-2000°/s par seconde il s'agit de la plage. Le gyro nous donne une valeur 16bits signés,
Donc si je comprend bien
La valeur minimum mesurable est de 2000/32767 °/s avec la plage 2000°/s (valeur correspondant à 1 en mesure sur le int16)
La valeur minimum mesurable est de 500/32767 °/s avec la plage 500
Donc a priori oui le fait de baisser la plage augmente la précision.

22/08/2018 à 21:16

Tenshi

Bonjour, t
Tout d'abord un grand merci pour le partage de toutes vos connaissances, c'est très enrichissant.
Cependant, étant un grand débutant dans ce domaine, je cherche vraiment à comprendre tout ce que je fait, et j'aimerais comprendre comment se fait le choix de la fréquence de coupure du filtre passe bas du capteur, pourriez vous l'expliquer ?
Merci beaucoup et bravo pour tout ce qui a été fait jusque là !

24/10/2018 à 11:33

Chuck182

Bonjour,
Déjà, merci beaucoup pour cet article, très interessant. J'ai quand même une grosse réserve sur un point : l'intégration.
Si on calcule l'angle en sommant tous les échantillons depuis le lancement du programme (pour simplifier), l'angle calculé est-il toujours cohérent après plusieurs minutes? J'imagine qu'on intègre un certain taux d'erreur dans les mesures, et j'ai peur de me retrouver avec des valeurs abbérantes au bout de plusieurs minutes.
Est-ce que mon analyse est bonne ?

24/10/2018 à 16:59

Chuck182

Bon, finalement après quelques tests, je viens de comprendre. C'est justement cette dérive qui en est la conséquence.

25/10/2018 à 12:03

caways

Bonjour,

Je me permets de demander des nouvelles quant à l'asservissement :-)

Bonne continuation et à bientôt

12/11/2018 à 16:42

lobodol

Salut @caways, le prochain article est toujours en cours d'écriture. C'est un sujet assez complexe et j'essaye de le vulgariser du mieux que je peux donc ça prend un peu de temps.
Pas de date officielle sur sa sortie mais ça viendra, c'est certain.
Promis, ça ne sera pas aussi long que la sortie de Kaamelott au cinéma :P

12/11/2018 à 17:57

Vous avez besoin d'aide ? Utilisez le Forum plutôt que les commentaires.

Un commentaire ?

* Champs obligatoires