Как очистить буфер FIFO на MPU6050?

мой MPU6050 работает очень нестабильно, проблема не в том, чтобы очистить лишний буфер FIFO на MPU6050, я использую библиотеку MPU Джеффа Роуберга и согласно его примеру DMP, но без использования прерываний, вот мой модифицированный код, но я попробовал несколько методов указан в библиотеке для очистки буфера, но не работает.

Большое спасибо!

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu(0x69); //AD0=Высокий


//Управление MPU/переменные состояния
bool dmpReady = false; 
uint8_t mpuIntStatus;  //фактический байт состояния прерывания
uint8_t deviceStatus; //статус устройства, 0 = успех,
uint16_t packetSize; //ожидаемый размер пакета DMP (по умолчанию 42) -- ?
uint16_t fifoCount; //подсчет всех байтов, находящихся в данный момент в FIFO
uint8_t fifoBuffer[64]; // буферное хранилище FIFO

// переменные ориентации/движения
Quaternion q;           // [w, x, y, z] контейнер кватернионов
VectorInt16 aa;         // [x, y, z] измерения датчика ускорения
VectorInt16 aaReal;     // [x, y, z] измерения датчика ускорения без гравитации
VectorInt16 aaWorld;    // [x, y, z] измерения датчика ускорения мирового кадра
VectorFloat gravity;    // [x, y, z] вектор гравитации
float euler[3];         // [psi, theta, phi] Контейнер углов Эйлера
float ypr[3];           // [рыскание, тангаж, крен] контейнер рыскания/тангажа/крена и вектор силы тяжести





volatile bool mpuInterrupt = false;
//void dmpDataReady() {
// mpuInterrupt = true;
//}


// поместите сюда свой код установки для однократного запуска:
void setup() {
  //подключаемся к шине I2C
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 48; // Тактовая частота I2C 400 кГц (200 кГц, если процессор 8 МГц)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
  //инициализируем последовательную связь
  Serial.begin(115200);
  while(!Serial); //обычно для задач Леонардо

  //инициализируем MPU
  Serial.println("Call MPU6050 Lib to initialize devices...");
  mpu.initialize(); //инициализируем устройство I2C с помощью библиотеки MPU6050

  //проверяем соединение
  Serial.println("Tesing device connections");
  Serial.println(mpu.testConnection() ? F("MPU6050 connection test successed ") : F("MPU6050 connection test failed"));

// //ждём начала, раскомментируем, если нам нужно дождаться прерывания пользователя
// Serial.println("Нажмите любую кнопку, чтобы начать");
// while (Serial.available() && Serial.read()); // пустой буфер
// пока (!Serial.available()); // ждем данных
// while (Serial.available() && Serial.read()); // снова пустой буфер


  //загружаем и настраиваем DMP
  Serial.println("initializing DMP"); 
  deviceStatus = mpu.dmpInitialize(); //используем библиотеку MPU6050 для инициализации dmp

 // смещения подачи
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 заводских настроек для моего тестового чипа

  //убедимся, что это работает
  if (deviceStatus == 0) {
    Serial.println("DMP initialization success, now enable DMP for use");
    //включаем DMP
    mpu.setDMPEnabled(true); //используем библиотеку MPU6050 для включения DMP)

    //ждём первого прерывания. в настоящее время просто оставьте значение false автоматически
    mpuInterrupt == false; 

    // сообщаем основному циклу Llop, что можно использовать DMP, устанавливаем флаг dmpRead в значение ture
    dmpReady = true;
    Serial.println("DMP is ready to use.");

    // получаем ожидаемый размер пакета DMP для последующего сравнения
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    //ОШИБКА! , статус устройства !=0 при инициализации DMP
    Serial.print("DMP initialization failed when using MPU6050 library:");
    if (deviceStatus == 1) {
      Serial.println(" intial memory load failed");
    } else if (deviceStatus == 2) {
      Serial.println(" failed to update DMP configuration");
    } else {
      Serial.print(" unknow error with code: ");
      Serial.println(deviceStatus);
    }
}
}


int ticket = 1;
void printOnlyOnce (String message){
  if (ticket == 1){
   Serial.println(message);
   ticket = 0 ;
  } else {
    return;
  }
}

void loop() {
  //если DMP не готов ничего не делать
  if (!dmpReady) {
    printOnlyOnce("MAIN LOOP: DMP disabled");
    return;
  } else {

    //проверка переполнения
    if (fifoCount == 1024) {
      mpu.resetFIFO();
      Serial.println("FIFO overflow");
    } else {


    //ждём достаточной доступной длины данных
    while (fifoCount < packetSize) {
      //ждём, пока наберёмся достаточно
      fifoCount = mpu.getFIFOCount();
    }


    //читаем этот пакет из буфера FIFO
    mpu.getFIFOBytes(fifoBuffer,packetSize);

    //отслеживание количества FIFO, здесь доступно более одного пакета

    //сбрасываем счетчик Fifo
    fifoCount -= packetSize ;
    Serial.println(fifoCount);

    if (fifoCount > 2) {
        ////// очистка буфера FiFO
    }
    //этап отображения
                // отображаем углы Эйлера в градусах
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
  }
  }
}