Accueil > Forum > Electronique > Arduino > Correction inclinaison MPU6050 + LIS3MDL avec Arduino

Correction inclinaison MPU6050 + LIS3MDL avec Arduino

RadioG RadioG 7 Messages
Bonjour,
Je réalise un chariot autonome asservi par Gps Ublox-Neo-M8t
https://drotek.com/shop/en/u-blox/884-ublox-neo-m8t-gps-lis3mdl-compass-xxl.html
qui devra circuler entre quelques WayPoints prédéfinis et enregistrés dans la base GPS. Les coordonnées Long/Latitude à atteindre seront envoyées au mobile par liaison radio VHF. Ce projet comporte de nombreux calculs à réaliser par prog de l'arduino embarqué.
Pour ne pas encombrer ce fil, je me limite donc au problème spécifique qui est de corriger la valeur de heading envoyée par la boussole LIS3MDL intégrée au Gps NeoM8t. La valeur de cap doit être corrigée par le MPU6050 en fonction de l'erreur due à la pente du terrain.
J'ai deux sketches Arduino, un pour la boussole et l'autre pour le gyroscope qui fonctionnent. L'idée est maintenant de réaliser la fusion pour appliquer les offsets de chaque axe et la compensation d'inclinaison.
Merci lobodol, je vois que tu as beaucoup bossé sur le 6050 et ton aide me sera précieuse.
lobodol lobodol 506 Messages BIG BOSS
Salut, je ne suis pas sûr de bien comprendre ta problématique.
Comment sont définis tes points de cheminement ?

Qu'est-ce que tu appelles "valeur de heading" ?

Tu dis que tu arrives bien à obtenir des angles exploitables avec le MPU, c'est bien ça ?
Sur les trois axes ?
As-tu besoin d'une lecture sur les trois axes ?
A chaque faute d'orthographe que vous faites, votre sexe rétrécit inexorablement ...
RadioG RadioG 7 Messages
Salut, je te remercie de bien vouloir suivre mon projet, probablement un peu complexe, mais réalisable si j'arrive à l'exprimer clairement.
J'ai construit ce projet sur Arduino en pensant que je peux exécuter un certain nombre de fonctions en cascade afin de piloter les actionneurs en final.
Un des premiers void est celui qui traite le GPS afin de donner la longitude et la latitude à un moment donné entre 2 WP. Les voids suivants traitent le calcul du Cap pour aboutir au WayPoint de destination...et ainsi de suite.
Comment sont définis tes points de cheminement ?
Ces points que j'appelle WPoints, sont fixes et préalablement définis par rapport à la configuration du terrain.
Qu'est-ce que tu appelles "valeur de heading" ?
La boussole nous envoie un Heading qui est en fait la position angulaire du mobile par rapport au Nord géographique, comme sur un bateau.
Tu dis que tu arrives bien à obtenir des angles exploitables avec le MPU, c'est bien ça ?
Oui, et à partir de ton fichier calibrer-le-capteur-mpu6050 auquel j'ai ajouté quelques lignes.
Xvomnfh
As-tu besoin d'une lecture sur les trois axes ?
Oui j'ai besoin des 3 axes pour corriger le cap indiqué par la boussole LIS3MDL. Ce cap est erroné sur un terrain en pente.
lobodol lobodol 506 Messages BIG BOSS
Un des premiers void est celui qui traite le GPS

Tu parles de fonction ?

"void" c'est un mot-clé pour dire qu'une fonction ne retourne rien s'il est placé avant le nom de la fonction :


/**
 * Une fonction qui ne retourne rien
 */
void myFunction()
{
  // Your code here
}

/**
 * Une fonction qui retourne un integer
 */
int anotherFunction(int foo)
{
  return foo * 2;
}


Pour compenser l'erreur de cap donné par la boussole en cas de terrain de en pente, je pense que tu vas devoir connaitre la valeur de la pente en °.
D'instinct, je dirais qu'on peut s'en sortir avec Pythagore.
A chaque faute d'orthographe que vous faites, votre sexe rétrécit inexorablement ...
RadioG RadioG 7 Messages
Effectivement il s'agit de fonctions.
Concernant la correction de cap boussole, elle doit être faite à 3 Hz environ pour compenser le balancement permanent (positif ou négatif) sur les axes X et Y.
J'ai fait un test et obtenu de bons résultats avec un MPU6050 et le HMC5883L qui est l'ancienne génération .
En remplaçant le HMC par le LIS3MDL, les choses semblent plus compliquées.
lobodol lobodol 506 Messages BIG BOSS
Tu aurais un exemple de ce que tu as fait en code Arduino ?
Car j'ai un peu de mal à comprendre ce que tu veux faire et comment t'aider.
A chaque faute d'orthographe que vous faites, votre sexe rétrécit inexorablement ...
RadioG RadioG 7 Messages
Je t'envoie la dernière mouture telle que je l'ai téléversée sur mon Nano.
J'ai prévu un afficheur dans le fonctionnement en extérieur ( ce qui est tout de même l'objectif...
Les deux premières fonctions ainsi que l'afficheur fonctionnent correctement. Seule la compensation du tilt boussole perturbe les résultats.
Une photo du montage et le code à 92% de l'espace de stockage de programme.
[code]
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
// --------- MPU650 variables ---|
#define YAW      0 //            |
#define PITCH    1 //            |
#define ROLL     2 //            |
// -------- MPU650 variables ----|
                                      #include <LIS3MDL.h>
                                      LIS3MDL mag;
                                      float heading,headingOffset;
                                      float magOffsetX,magOffsetY,magOffsetZ;
                                      float angleOrient,headingTiltCompensed;
                                      char report[80];
// AD0 low = 0x68 // AD0 high = 0x69
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// Orientation/motion vars
Quaternion q;        // [w, x, y, z]         quaternion container
VectorFloat gravity; // [x, y, z]            gravity vector
float ypr[3];        // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

volatile bool mpuInterrupt = false;     // Indicates whether MPU interrupt pin has gone high
// ---------------------------------------------------------------------------

/** * Interrup détection routine MPU6050*/
void dmpDataReady() {
  mpuInterrupt = true;
}
                                  #include "U8glib.h"  // Afficheur Oled
                                  U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE|U8G_I2C_OPT_DEV_0);

void setup() {
    Wire.begin();Serial.begin(57600);
        //====== init LIS3MDL ============
          if (!mag.init()){ Serial.println(F("Failed to detect and initialize magnetometer!"));while (1);}
          mag.enableDefault();
        //====== init LIS3MDL ============
    TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) MPU6050
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // Verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // Load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // MPU calibration: set YOUR offsets here.
    mpu.setXAccelOffset(-5060);
    mpu.setYAccelOffset(-3543);
    mpu.setZAccelOffset(5819);
    mpu.setXGyroOffset(-1136);
    mpu.setYGyroOffset(-8);
    mpu.setZGyroOffset(-32);

    // Returns 0 if it worked
    if (devStatus == 0) {
        // Turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));mpu.setDMPEnabled(true);

        // Enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0 : #pin2)..."));
        attachInterrupt(0, dmpDataReady, RISING);  //int0 ==> D2 pour nano
        mpuIntStatus = mpu.getIntStatus();

        // Set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));dmpReady = true;
        // Get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();Serial.println(F("L85")); } else { Serial.println(F("L86"));
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));Serial.print(devStatus);Serial.println(F(")"));
    }    
}

void loop() {
  
                  calculsMPU6050(); // Mesures inclinaison 3 axes et définition des valeurs correcteurs
                  calculsLIS3MDL(); // Cap boussole sans compensation d'inclinaison 
                  tiltCompensation(); // Correction du heading boussole en fonction des correcteurs d'inclinaison
                  u8g.firstPage();  
                    do {
                      draw();
                    } while( u8g.nextPage() );
                    delay(300);
                        }
void calculsMPU6050(){
   // If programming failed, don't try to do anything
    if (!dmpReady) { return; }

    // Wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) { // Do nothing... 
      }
    // Reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;mpuIntStatus = mpu.getIntStatus();
    // Get current FIFO count
    fifoCount = mpu.getFIFOCount();
    // Check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) { //fifoCount == 1024) reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.print(fifoCount);Serial.println(F(" FIFO overflow!"));
        // Otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // Wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) { fifoCount = mpu.getFIFOCount();}
        // Read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        // Track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;
        // Convert Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        // Print angle values in degrees.
        Serial.print("YAW ");Serial.print(ypr[YAW] * (180 / M_PI));Serial.print("\t");
        Serial.print("PITCH ");Serial.print(ypr[PITCH] * (180 / M_PI));Serial.print("\t");
        Serial.print("ROLL ");Serial.println(ypr[ROLL] * (180 / M_PI));       
         angleOrient=(ypr[YAW] * (180 / M_PI))* -1;/*angleOrient ne représente pas le cap 
        par rapport au Nord mais l'angle du mobile par rapport à sa position avant mise sous tension */
        if ((ypr[YAW] * (180 / M_PI))* -1 <=0) {  // multiplication par -1 pour sens horaire
          angleOrient=360-(ypr[YAW] * (180 / M_PI));}  // valeurs d'angle sur 360 degrés
          Serial.print("angleOrient Degrés: ");Serial.println(angleOrient);
         //delay(300); 
        }
}     
void calculsLIS3MDL() {  
                float Pi = 3.14159265;
                mag.read();
                Serial.print("*      mag.m.x brut : ");Serial.println(mag.m.x);//AJOUTé
                Serial.print("*      mag.m.y brut : ");Serial.println(mag.m.y);//AJOUTé
                Serial.print("*      mag.m.z brut : ");Serial.println(mag.m.z);//AJOUTé
                 // Calculate the angle of the vector y,x AVANT OFFSET
                 heading = atan2(mag.m.y,mag.m.x) * 180 / Pi;
                 Serial.print("*      Compass Heading: ");Serial.println(heading);
                int offsetX=-906; int offsetY=2753;int offsetZ=-152; //valeurs définies par le scetch de calibrage
 magOffsetX=(mag.m.x)-offsetX;
 magOffsetY=(mag.m.y)-offsetY;
 magOffsetZ=(mag.m.z)-offsetZ;
                magOffsetZ=-magOffsetZ; //INVERSION SENS ROTATION HORAIRE POUR VALEURS CROISSANTES DU HEADING              
                snprintf(report, sizeof(report), "M: %6d %6d %6d",magOffsetX, magOffsetY, magOffsetZ);
                Serial.print("*     ");Serial.println(report);                
                // Calculate the angle of the vector y,x (//Compute heading)
                magOffsetY= -magOffsetY; //INVERSION SENS ROTATION HORAIRE POUR VALEURS CROISSANTES DU HEADING
                 headingOffset = atan2(magOffsetY,magOffsetX )* 180 / PI; //               
                 //Normalize to 0-360
               //if (heading < 0){heading = 360 + heading;}
               if (headingOffset < 0){headingOffset= 360 + headingOffset;}
                Serial.print("Compass HeadingOffset: ");Serial.println(headingOffset); }
void tiltCompensation(){
 // Some of these are used twice, so rather than computing them twice in the algorithem we precompute them before hand.
  float cosRoll = cos(ypr[ROLL] *(180 / M_PI));//float cosRoll = cos(ypr[ROLL]);
Serial.print(F("ypr[ROLL] = "));Serial.print(ypr[ROLL],6);Serial.print(F("    cosRoll = "));Serial.println(cosRoll);
float sinRoll = sin(ypr[ROLL] *(180 / M_PI)); //float sinRoll = sin(ypr[ROLL]);  
Serial.print(F("ypr[ROLL] = "));Serial.print(ypr[ROLL],6);Serial.print(F("    sinRoll = "));Serial.println(sinRoll); 
float cosPitch = cos(ypr[PITCH] *(180 / M_PI));//float cosPitch = cos(ypr[PITCH]);  
Serial.print(F("ypr[PITCH] = "));Serial.print(ypr[PITCH],6);Serial.print(F("    cosPitch = "));Serial.println(cosPitch);
float sinPitch = sin(ypr[PITCH] *(180 / M_PI));//float sinPitch = sin(ypr[PITCH]);  
Serial.print(F("ypr[PITCH] = "));Serial.print(ypr[PITCH],6);Serial.print(F("    sinPitch = "));Serial.println(sinPitch);
  
  // Tilt compensation
  float Xh = mag.m.x * cosPitch + mag.m.z * sinPitch; 
  float Yh = mag.m.x * sinRoll * sinPitch + mag.m.y * cosRoll - mag.m.z * sinRoll * cosPitch;
  Serial.print("Xh= ");Serial.println(Xh);Serial.print("Yh= ");Serial.println(Yh);
  headingTiltCompensed = atan2(Xh, Yh);//  headingTiltCompensed = atan2(Yh, Xh);
  Serial.print("*     headingTiltCompensed: ");Serial.println(headingTiltCompensed);
  //return headingTiltCompensed;
} //fin void tiltCompensation
 void draw(void) {
                          u8g.setFont(u8g_font_unifont);
                        u8g.setPrintPos(0, 13);u8g.print("Fifo");u8g.setPrintPos(70, 13);u8g.print(mpuIntStatus & 0x10);
                        u8g.setPrintPos(0, 30);u8g.print("Orientat");u8g.setPrintPos(80, 30);u8g.print(angleOrient);
                        u8g.setPrintPos(0, 45);u8g.print("headOff");u8g.setPrintPos(80, 45);u8g.print(headingOffset);
                        u8g.setPrintPos(0, 60);u8g.print("hTiltComp");u8g.setPrintPos(80, 60);u8g.print(headingTiltCompensed);
}

[/code]
XxI3KFP
RadioG RadioG 7 Messages
Pour info je te transmets ces deux liens qui représentent ma démarche avec une boussole différente.
Boussole seule
Boussole avec compensation d'inclinaison
Vous devez être connecté pour pouvoir répondre à ce sujet.