Gyroscope integration echantillonnage

Bonjour,

Après avoir programmer un gyroscope et l’avoir tester je trouve étrange une de mes formules…
Je m’explique, je mesure les données du gyroscope toutes les 20 millisecondes. Et donc j’intègre ma vitesse pour avoir ma position.

Voilà mon code :

(Dans le code j’utilise aussi les données accéléromètre mais je n’ai pas encore tester)
(Enfin dans la librairie LSM9DS0 la donnée récuperé du capteur est multiplié par 0.00875 dps pour convertir la tension reçu en dégrée par seconde)

#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LSM9DS0.h>
#include <Adafruit_Sensor.h>

// Timer
unsigned long timer=0;
unsigned int Te=20000;

// Capteur
Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0(10, 9); //SPI pin CS

// Delta angle (raw input from gyro)
float dpsz = 0;
// Acceleration
float accx = 0;
float accy = 0;
// Actual angle
float angz = 0;
// Actual speed
float vitX = 0;
float vitY = 0;
// Previous vitesse for calculation
float p_vitX = 0;
float p_vitY = 0;
// Actual position
float x = 0;
float y = 0;

// Calibration values
float gyroLowz = 0;
float gyroHighz = 0;
float accLowX = 0 ;
float accLowY = 0;
float accHighX = 0;
float accHighY = 0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  // Try to initialise and warn if we couldn't detect the chip
  if (!lsm.begin())
  {
    Serial.println("Oops ... unable to initialize the LSM9DS0. Check your wiring!");
    while (1);
  }
  Serial.println("Found LSM9DS0 9DOF");
  setupSensor();
  calibrate();

}

void loop() {
  // put your main code here, to run repeatedly:
  if(millis()-timer>=20);
  {
    // Mise a jour timer
    timer=millis();
    
    // Mise a jour donnees
    sensors_event_t accel, mag, gyro, temp; 
    lsm.getEvent(&accel, &mag, &gyro, &temp);
    dpsz = gyro.gyro.z;
    accx = accel.acceleration.x;
    accy = accel.acceleration.y;
    
    // Calcul des angles
    
    // En z
    if(dpsz >= gyroHighz || dpsz <= gyroLowz) {
      angz += dpsz * 0.002;
    }
    
    // Calcul des vitesses et positions 
    
    // En x
    if(accx >= accHighX || accx <= accLowX) {
      p_vitX = vitX;
      vitX += (accx) * 0.02;
      x += Te*Te*accx+p_vitX * Te;
    }
    
    // En y
    if(accy >= accHighY || accy <= accLowY) {
      p_vitY = vitY;
      vitY += (accy) * 0.02;
      y += Te*Te*accy+p_vitY*Te;
    }
    
    Serial.println(angz);
    
   }

  delay(2);

}

void setupSensor()
{
  // 1.) Set the accelerometer range
  lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_8G);
  
  // 2.) Set the magnetometer sensitivity
  lsm.setupMag(lsm.LSM9DS0_MAGGAIN_2GAUSS);

  // 3.) Setup the gyroscope
  lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_245DPS);
}

void calibrate()
{
  Serial.println("Calibrating gyro, don't move!");
  for(int i = 0; i < 250; i++) {
    sensors_event_t accel, mag, gyro, temp; 
    lsm.getEvent(&accel, &mag, &gyro, &temp);
    dpsz = gyro.gyro.z;
    accx = accel.acceleration.x;
    accy = accel.acceleration.y;

    if(dpsz > gyroHighz)
      gyroHighz = dpsz;
    else if(dpsz < gyroLowz)
      gyroLowz = dpsz;

    if(accx > accHighX)
      accHighX = accx;
    else if(accx < accLowX)
      accLowX = accx;

    if(accy > accHighY)
      accHighY = accy;
    else if(accy < accLowY)
      accLowY = accy;
    
    delay(10);
  }
  Serial.println("Calibration complete.");
}

Donc on a : Valeur Gyro*0.00875=gyro.gyro.z;
Angle[n]=gyro.gyro.z[n]*Te + Angle[n-1]
Avec Te = 0.002

Ce que je comprend pas c’est que le Te j’aurais du mettre 0.02 puisque je mesure toutes les 20 millisecondes et non pas toutes les 2 millisecondes. Mais quand j’affiche millis() dans mon if j’ai les valeurs suivantes :

6258
6260
6262
6264
6266
6268
6270
6272
6274
6276
6278

Donc on constate que je calcule toutes les 2 ms donc c’est mon if qui ne fonctionne pas, je ne comprend pas pourquoi…

Bonne soirée !

Affiche déjà les valeurs brutes avant de les combiner dans des formules.

Dans Ton if (millis()-timer >= 20); ... Il y a le ; en trop ...

Écrit comme ça, quand le test est vérifié ... Tu ne fais rien ... Et ton ton test n'est pas vérifié ... Tu ne fais rien aussi

bonjour, +1 pour le ; derrière le if donc le if s'arrête au ; au lieu de rentrer dans les {}

Bonjour,

désolé du retard, très occupé en ce moment.

Juste oh my god pour le ";" je l'ai juste pas vu après toutes ces relectures, honte a moi.

Ensuite ce qui est étonnant, c'est qu'il rentre quand même dans le if toutes les 2 ms avec le ; alors qu'il ne devrait pas... Je test comme ça demain, et je vous tiens au courant.

Merci ;)