Merci pour les solutions données mais mon problème principal est que l’état bloquant dure 25 ms et que durant cette période mon loop est bloqué et les temps de résultats sont trop lents pour avoir une commande fluide du drone .
Voici mon programme:
#include "Servo.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "MsTimer2.h"
//partie lecture_capteur
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t mx, my, mz;
# define LED_PIN 13
bool blinkState = false;
const float CoefAcc=1.197509766E-6; // 9.81* 10^-3 / 8192 pour m.s-2
//const float CoefW=1.332312406E-4; // pi / (180 * 131) pour rad.s-1
const float CoefW=1.332312406E-4*1.57/1.3; // pi / (180 * 131) pour rad.s-1
int n=0;
float Wy; //axe W donné par le gyroscope
float Ot; //axe w du drone
float AOt; //axe w du drone du precedent calcul
float Dt; //temps entre chaques calculs
float axt; //Accélération du drone par rapport au terrestre x
float azt; //Accélération du drone par rapport au terrestre z
float Vxt; //Vitesse angulaire du drone par rapport au terrestre x
float AVxt; //Vitesse angulaire du drone par rapport au terrestre x
float Vzt; //Vitesse angulaire du drone par rapport au terrestre z
float AVzt; //Vitesse angulaire du drone par rapport au terrestre z
float Xdt; //Position du drone par rapport au terrestre z
float AXdt; //Position angulaire du drone par rapport au terrestre z
float Zdt; //Position angulaire du drone par rapport au terrestre z
float AZdt; //Position angulaire du drone par rapport au terrestre z
float Max; //moyenne 100 ax
float Maz; //moyenne 100 az
float Mwy; //moyenne 100 wy
float gzt; //calcul gravitée
float axsi; //axe x donné par le gyro en m.s-2
float azsi; //axe z donné par le gyro en m.s-2
float gysi; //axe y donné par le gyro en rad.s-1
float ogy;
//partie différentiel
int ch1; // Nos deux cannaux de RC
int ch2;
int g; //information entre 0 et 254 du moteur gauche
int d; //information entre 0 et 254 du moteur gauche
Servo monservodroit;
Servo monservogauche;
void calculgyro() {
// partie lecture_capteur
// accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); // lit les valeurs du gyroscope g et de l'accéléromètre a
// autre moyen pour recevoir les données:
//accelgyro.getAcceleration(&ax, &ay, &az);
//accelgyro.getRotation(&gx, &gy, &gz);
axsi= (float) CoefAcc*ax; //calcul de ax avec le déclalage oax
azsi= (float) CoefAcc*az; //calcul de az avec le déclalage oaz
gysi= (float) CoefW*(gy-ogy); //calcul de gy avec le déclalage ogy
axt = (float) cos(Ot)*axsi+sin(Ot)*azsi ; //Accélération du drone par rapport au terrestre x
azt = (float) -sin(Ot)*axsi+cos(Ot)*azsi -gzt ; //Accélération du drone par rapport au terrestre z
Vxt = (float) Vxt+Dt*axt ; //Vitesse angulaire du drone par rapport au terrestre x
Vzt = (float) Vzt+Dt*azt ; //Vitesse angulaire du drone par rapport au terrestre z
Xdt = (float) Xdt+Dt*Vxt ; //Position du drone par rapport au terrestre x
Zdt = (float) Zdt+Dt*Vzt ; //Position du drone par rapport au terrestre z
Ot = (float) Ot+gysi*Dt; //axe w du drone
//partie différentiel
g =(ch1+ch2*.2)*0.18+128;
d =(ch1-ch2*.2)*0.18+128;
if (g < 20)
{g = 21;}
if (g > 235)
{g = 234;}
if (d < 20)
{d = 21;}
if (d > 235)
{d = 234;}
//Serial.print("Ch1 = ");
//Serial.print(ch1);
//Serial.print(" Ch2 = ");
//Serial.print(ch2);
// monservodroit.write(g);
// monservogauche.write(d);
}
void setup() {
pinMode(test, OUTPUT); // D9 en sortie
//partie lecture_capteur setup
Dt = (float) 0.001 ; //temps entre chaques calculs
// join I2C bus (I2Cdev library doesn't do this automatically)
Serial.begin(115200); //fréquence de la carte (en Bauds)
Serial.println("Initialisation...");
accelgyro.initialize();
Serial.println("Test de connection...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection reussie" : "MPU6050 connection echouee");
// configure Arduino LED for
for (int i=0; i < 100; i++){ //boucle de calcul de 100 valeurs de ax, az et gy
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); // lit les valeurs du gyroscope g et de l'accéléromètre a
Max = (float) (Max+ax); //moyenne 100 ax
Maz = (float) (Maz+az); //moyenne 100 az
Mwy = (float) (Mwy+gy); //moyenne 100 wy
delay(2);
}
ogy=Mwy/100;
Max = (float) (CoefAcc*Max/100); //moyenne 100 ax
Maz = (float) (CoefAcc*Maz/100); //moyenne 100 az
Mwy = (float) (CoefW*Mwy/100); //moyenne 100 wy
Ot = (float) atan (Max/Maz) ; //Calcul inclinaison initiale
gzt = (float) sqrt (Max*Max + Maz*Maz) ; //Calcul module de la gravitée
n=0;
Serial.print (Ot); Serial.print("\t");
Serial.println (gzt);
Serial.println ("Xdt ; Zdt ; Ot ; ax ; az ; wy:\t");
//partie différentiel setup
pinMode(2, INPUT); // ch1 sur Arduino pin5 STerring
pinMode(3, INPUT); // ch2 sur Arduino pin6 THrottle
monservodroit.attach(7); // moteur en entrées 7
monservogauche.attach(8); // moteur en entrées 8
//partie interuption
MsTimer2::set(200,calculgyro); // delai de 1 ms de calcul de gyro
MsTimer2::start(); //delai de 10 ms
Serial.println("test post-interruption");
}
void loop() {
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); // lit les valeurs du gyroscope g et de l'accéléromètre a etat bloquant
ch1 = pulseIn(2, HIGH, 25000); // Lire ch1 lecture a etat bloquant (25 ms)
ch1 = ch1 - 1500;
ch2 = pulseIn(3, HIGH, 25000); // Lire ch2 lecture a etat bloquant (25 ms)
ch2 = ch2 - 1500;
n++;
if (n>=100) { //Boucle d'affichage toutes les s
n=0;
Serial.print(" Vxt:"); Serial.print(Vxt*100);
Serial.print(" Vzt: "); Serial.print(Vzt*100);
Serial.print(" Ot: "); Serial.print(Ot);
Serial.print(" axt: "); Serial.print(axt*100);
Serial.print(" azt: "); Serial.print(azt*100);
Serial.print(" gy: "); Serial.print(gy);
Serial.print(" ax: "); Serial.print(ax);
Serial.print(" az: "); Serial.print(az);
Serial.println();
Serial.print("g= "); // Afficher les lectures Arduino
Serial.print(g);
Serial.print(" d = ");
Serial.println(d);
}
}