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.
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 ?
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 ?
"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.
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.
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);
}
RadioG
7 Messages
lobodol
