Проблема в последовательности чтения данных датчика

Я считываю данные с двух датчиков MPU6050 с помощью Arduino Uno, но это вызывает проблемы. Код идеален, а также выполняется считывание датчиков. См. изображение последовательного монитора.

Вы можете видеть изображение выше, на котором выходные данные последовательного монитора не совпадают. Он разрывает строки после некоторого интервала времени. Я хочу ожидать серийные данные в подкладке. Вот код:

// I2Cdev и MPU6050 должны быть установлены как библиотеки, иначе файлы .cpp/.h
// для обоих классов должен быть указан путь включения вашего проекта
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // не требуется, если используется включаемый файл MotionApps

// Библиотека Arduino Wire требуется, если реализация I2Cdev I2CDEV_ARDUINO_WIRE
// используется в I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

// адрес класса I2C по умолчанию 0x68
// в качестве параметра здесь могут быть переданы конкретные адреса I2C
// Низкий уровень AD0 = 0x68 (по умолчанию для SparkFun breakout и оценочной платы InvenSense)
// Высокий уровень AD0 = 0x69
//MPU6050 мпу;
MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69); // <-- используем для высокого уровня AD0


// раскомментируйте "OUTPUT_READABLE_YAWPITCHROLL", если хотите увидеть рыскание/
// углы тангажа/крена (в градусах), рассчитанные по приходу q1uaternion
// из ФИФО. Обратите внимание, что это также требует вычислений вектора силы тяжести1.
// Также обратите внимание, что углы рыскания/тангажа/крена страдают от блокировки карданного подвеса (для
// подробнее см.: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

int pre_count = 0;

#define LED_PIN 13 // (Arduino — 13, Teensy — 11, Teensy++ — 6)
bool blinkState = false;

// переменные управления/состояния MPU
bool dmpReady1 = false;  // устанавливаем true, если инициализация DMP прошла успешно
uint8_t mpuIntStatus1;   // содержит фактический байт состояния прерывания от MPU
uint8_t devStatus1;      // возвращаем статус после каждой операции с устройством (0 = успех, !0 = ошибка)
uint16_t packetSize1;    // ожидаемый размер пакета DMP (по умолчанию 42 байта)
uint16_t fifoCount1;     // подсчет всех байтов в настоящее время в FIFO
uint8_t fifoBuffer1[64]; // Буфер хранения FIFO

// переменные управления/состояния MPU
bool dmpReady2 = false;  // устанавливаем true, если инициализация DMP прошла успешно
uint8_t mpuIntStatus2;   // содержит фактический байт состояния прерывания от MPU
uint8_t devStatus2;      // возвращаем статус после каждой операции с устройством (0 = успех, !0 = ошибка)
uint16_t packetSize2;    // ожидаемый размер пакета DMP (по умолчанию 42 байта)
uint16_t fifoCount2;     // подсчет всех байтов в настоящее время в FIFO
uint8_t fifoBuffer2[64]; // Буфер хранения FIFO

// переменные ориентации/движения
Quaternion q1;           // [w, x, y, z] контейнер q1uaternion
VectorInt16 aa1;         // [x, y, z] измерения датчика ускорения
VectorInt16 aa1Real1;     // [x, y, z] измерения датчика ускорения без гравитации1
VectorInt16 aa1World1;    // [x, y, z] измерения датчика ускорения мирового кадра
VectorFloat gravity1;    // [x, y, z] вектор гравитации1
float euler1[3];         // [пси, тета, фи] контейнер угла Эйлера1
float ypr1[3];           // [рыскание, тангаж, крен] контейнер рыскания/тангажа/крена и вектор гравитации1

// структура пакета для демонстрации чайника InvenSense
uint8_t teapotPacket1[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };

Quaternion q2;           // [w, x, y, z] контейнер q1uaternion
VectorInt16 aa2;         // [x, y, z] измерения датчика ускорения
VectorInt16 aa1Real2;     // [x, y, z] измерения датчика ускорения без гравитации1
VectorInt16 aa1World2;    // [x, y, z] измерения датчика ускорения мирового кадра
VectorFloat gravity2;    // [x, y, z] вектор гравитации1
float euler2[3];         // [пси, тета, фи] контейнер угла Эйлера1
float ypr2[3];           // [рыскание, тангаж, крен] контейнер рыскания/тангажа/крена и вектор гравитации1

// структура пакета для демонстрации чайника InvenSense
uint8_t teapotPacket2[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };



// =============================================== ================
// === ПРОЦЕДУРА ОБНАРУЖЕНИЯ ПРЕРЫВАНИЯ ===
// =============================================== ================

volatile bool mpuInterrupt1 = false;     // указывает, перешел ли вывод прерывания MPU в высокий уровень
void dmpDataReady1() {
  mpuInterrupt1 = true;
// Serial.println("Прерывание МПУ истинно");
}

volatile bool mpuInterrupt2 = false;     // указывает, перешел ли вывод прерывания MPU в высокий уровень
void dmpDataReady2() {
  mpuInterrupt2 = true;
}



// =============================================== ================
// === НАЧАЛЬНАЯ НАСТРОЙКА ===
// =============================================== ================

void setup() {
  // подключаемся к шине I2C (библиотека I2Cdev не делает этого автоматически)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // Тактовая частота I2C 400 кГц (200 кГц, если процессор 8 МГц)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  // инициализируем последовательную связь
  // (выбрано 115200, потому что оно требуется для вывода Teapot Demo, но это
  // на ваше усмотрение в зависимости от вашего проекта)
  Serial.begin(115200);
  while (!Serial); // ожидание перечисления Leonardo, остальные продолжают работу немедленно

  // ПРИМЕЧАНИЕ. Хост-процессоры с тактовой частотой 8 МГц или более медленные, такие как Teensy @ 3.3v или Ardunio.
  // Pro Mini, работающий от 3,3 В, не может надежно обрабатывать эту скорость передачи данных из-за
  // время передачи слишком не совпадает с тактами процессора. Вы должны использовать
  // 38400 или медленнее в этих случаях, или использовать какой-то внешний отдельный
  // кристальное решение для таймера UART.

  // инициализируем устройство
  Serial.println(F("Initializing I2C devices 1..."));
  mpu1.initialize();

   // инициализируем устройство
  Serial.println(F("Initializing I2C devices 2..."));
  mpu2.initialize();

  // проверяем соединение
  Serial.println(F("Testing device 1 connections..."));
  Serial.println(mpu1.testConnection() ? F("MPU6050 1 connection successful") : F("MPU6050 1 connection failed"));

  // проверяем соединение
  Serial.println(F("Testing device 2 connections..."));
  Serial.println(mpu2.testConnection() ? F("MPU6050 2 connection successful") : F("MPU6050 2 connection failed"));


  // ждем готовности
  Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  while (Serial.available() && Serial.read()); // пустой буфер
  while (!Serial.available());                 // ждем данных
  while (Serial.available() && Serial.read()); // снова пустой буфер

  // загрузить и настроить DMP
  Serial.println(F("Initializing DMP 1..."));
  devStatus1 = mpu1.dmpInitialize();

  // загрузить и настроить DMP
  Serial.println(F("Initializing DMP 2..."));
  devStatus2 = mpu2.dmpInitialize();

  // укажите здесь собственные смещения гироскопа, масштабированные для минимальной чувствительности
  mpu1.setXGyroOffset(220);
  mpu1.setYGyroOffset(76);
  mpu1.setZGyroOffset(-85);
  mpu1.setZAccelOffset(1788); // 1688 по умолчанию для моего тестового чипа

  // укажите здесь собственные смещения гироскопа, масштабированные для минимальной чувствительности
  mpu2.setXGyroOffset(74);
  mpu2.setYGyroOffset(14);
  mpu2.setZGyroOffset(-19);
  mpu2.setZAccelOffset(253); // 1688 по умолчанию для моего тестового чипа

  // убедиться, что это сработало (возвращает 0, если это так)
  if (devStatus1 == 0) {
    // включаем DMP, теперь, когда он готов
    Serial.println(F("Enabling DMP 1..."));
    mpu1.setDMPEnabled(true);


    // включить обнаружение прерываний Arduino
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady1, RISING);
    mpuIntStatus1 = mpu1.getIntStatus();


    // устанавливаем наш флаг готовности DMP, чтобы основная функция loop() знала, что ее можно использовать
    Serial.println(F("DMP 1 ready! Waiting for first interrupt..."));
    dmpReady1 = true;


    // получаем ожидаемый размер пакета DMP для последующего сравнения
    packetSize1 = mpu1.dmpGetFIFOPacketSize();
  } else {
    // ОШИБКА!
    // 1 = первоначальная загрузка памяти не удалась
    // 2 = не удалось обновить конфигурацию DMP
    // (если он сломается, обычно код будет 1)
    Serial.print(F("DMP 1 Initialization failed (code "));
    Serial.print(devStatus1);
    Serial.println(F(")"));
  }

  // убедиться, что это сработало (возвращает 0, если это так)
  if (devStatus2 == 0) {
    // включаем DMP, теперь, когда он готов
    Serial.println(F("Enabling DMP 2..."));
    mpu2.setDMPEnabled(true);


    // включить обнаружение прерываний Arduino
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady2, RISING);
    mpuIntStatus2 = mpu2.getIntStatus();


    // устанавливаем наш флаг готовности DMP, чтобы основная функция loop() знала, что ее можно использовать
    Serial.println(F("DMP 2 ready! Waiting for first interrupt..."));
    dmpReady2 = true;


    // получаем ожидаемый размер пакета DMP для последующего сравнения
    packetSize2 = mpu2.dmpGetFIFOPacketSize();
  } else {
    // ОШИБКА!
    // 1 = первоначальная загрузка памяти не удалась
    // 2 = не удалось обновить конфигурацию DMP
    // (если он сломается, обычно код будет 1)
    Serial.print(F("DMP 2 Initialization failed (code "));
    Serial.print(devStatus2);
    Serial.println(F(")"));
  }




  // настроить светодиод для вывода
  pinMode(LED_PIN, OUTPUT);
}



// =============================================== ================
// === ГЛАВНЫЙ ПРОГРАММНЫЙ ЦИКЛ ===
// =============================================== ================

void loop() {

  // если программирование не удалось, не пытайтесь ничего сделать
  if (!dmpReady1) return;

  // если программирование не удалось, не пытайтесь ничего сделать
  if (!dmpReady2) return;

// Serial.println("код доступа здесь 1");
  // ждем прерывания MPU или доступных дополнительных пакетов
  while (!mpuInterrupt1 && fifoCount1 < packetSize1) {
    // здесь другие сведения о поведении программы
    // .
    // .
    // .
    // если вы действительно параноик, вы можете часто тестировать между другими
    // что-то, чтобы проверить, истинно ли значение mpuInterrupt1, и если да, то "break;" из
    // цикл while() для немедленной обработки данных MPU
    // .
    // .
    // .
     break;

// Serial.println("код доступа здесь 2");
  }

  // ждем прерывания MPU или доступных дополнительных пакетов
  while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
    // здесь другие сведения о поведении программы
    // .
    // .
    // .
    // если вы действительно параноик, вы можете часто тестировать между другими
    // что-то, чтобы проверить, истинно ли значение mpuInterrupt1, и если да, то "break;" из
    // цикл while() для немедленной обработки данных MPU
    // .
    // .
    // .
    break;
  }



  // сброс флага прерывания и получение байта INT_STATUS
  mpuInterrupt1 = false;
  mpuIntStatus1 = mpu1.getIntStatus();

  // сброс флага прерывания и получение байта INT_STATUS
  mpuInterrupt2 = false;
  mpuIntStatus2 = mpu2.getIntStatus();

  // получаем текущий счетчик FIFO
  fifoCount1 = mpu1.getFIFOCount();

  // получаем текущий счетчик FIFO
  fifoCount2 = mpu2.getFIFOCount();

  // проверка на переполнение (это никогда не должно происходить, если только наш код не слишком неэффективен)
  if ((mpuIntStatus1 & 0x10) || fifoCount1 == 1024) {
    // сбрасываем, чтобы мы могли продолжить работу без ошибок
    mpu1.resetFIFO();
    Serial.println(F("FIFO overflow!"));

    // в противном случае проверьте прерывание готовности данных DMP (это должно происходить часто)
  } else if (mpuIntStatus1 & 0x02) {
    // ожидание правильной доступной длины данных, должно быть ОЧЕНЬ короткое ожидание
    while (fifoCount1 < packetSize1) fifoCount1 = mpu1.getFIFOCount();

    // читаем пакет из FIFO
    mpu1.getFIFOBytes(fifoBuffer1, packetSize1);

    // отслеживаем количество FIFO здесь, если есть > 1 пакет в наличии
    // (это позволяет нам сразу читать больше, не дожидаясь прерывания)
    fifoCount1 -= packetSize1;


#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // отображаем углы euler1 в градусах
    mpu1.dmpGetQuaternion(&q1, fifoBuffer1);
    mpu1.dmpGetGravity(&gravity1, &q1);
    mpu1.dmpGetYawPitchRoll(ypr1, &q1, &gravity1);
    int count = 0;
    count++;
    pre_count += count;
    Serial.print(pre_count);
    Serial.print("\t");
    Serial.print(ypr1[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr1[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr1[2] * 180 / M_PI);
    Serial.print("\t");
#endif


// // мигать светодиодом для индикации активности
// blinkState = !blinkState;
// digitalWrite(LED_PIN, blinkState);
  }

   // проверка на переполнение (это никогда не должно происходить, если только наш код не слишком неэффективен)
  if ((mpuIntStatus2 & 0x10) || fifoCount2 == 1024) {
    // сбрасываем, чтобы мы могли продолжить работу без ошибок
    mpu2.resetFIFO();
    Serial.println(F("FIFO 2 overflow!"));

    // в противном случае проверьте прерывание готовности данных DMP (это должно происходить часто)
  } else if (mpuIntStatus2 & 0x02) {
    // ожидание правильной доступной длины данных, должно быть ОЧЕНЬ короткое ожидание
    while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();

    // читаем пакет из FIFO
    mpu2.getFIFOBytes(fifoBuffer2, packetSize2);

    // отслеживаем количество FIFO здесь, если есть > 1 пакет в наличии
    // (это позволяет нам сразу читать дальше, не дожидаясь прерывания)
    fifoCount2 -= packetSize2;


#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // отображаем углы euler1 в градусах
    mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
    mpu2.dmpGetGravity(&gravity2, &q2);
    mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
    Serial.print("\t");
    Serial.print(ypr2[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr2[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.println(ypr2[2] * 180 / M_PI);
#endif


// // мигать светодиодом для индикации активности
// blinkState = !blinkState;
// digitalWrite(LED_PIN, blinkState);
  }

   // мигать светодиодом для индикации активности
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}

В чем проблема? Заранее спасибо!