Mpu6050 instable

Bonjour, je suis en train de concevoir un drone sur une base de rover 5 + atmega 2560 + carte contrôleur 4 moteur DAGU + bluetooth HC06 pour un controle depuis un smartphone sur à partir du projet de Kas .
Tout pareil sauf que le mien a quatre moteurs et je que souhaite lui ajouter une fonction de pilote automatique avec correction d’erreurs de trajectoire par gyroscope. C’est à dire que j’enregistre un parcours et qu’il le répète ensuite en corrigeant les erreurs induites par le patinage des chenilles.

Tout cela fonctionne bien sauf que le MPU6050 est rapidement prit de folie et perd totalement l’orientation. Cela se produit bien sûr dans la partie “pilote automatique” du programme. Durant le mode manuel et l’enregistrement du parcours, aucun problème.

Je joins mon long programme en piece jointe avec le “MPU6050_6Axis_MotionApps20.h” modifié pour éviter les problèmes de “FIFO overflow” du MPU6050. (j’ai essayé de coller mon code mais cela dépasse les 9000 caractères autorisés).

Bref si quelqu’un peut m’expliquer pourquoi le MPU perd la boussole ce serait sympa.

DroneChenilleHC06beta3.ino (35.9 KB)

MPU6050_6Axis_MotionApps20.h (39.7 KB)