Увеличение частоты Гц при работе с 3 последовательными портами (датчики IMU)

я занимаюсь университетским проектом, в котором мы используем 2 датчика IMU и устройство чтения SD-карт для сохранения данных с датчиков.

Как вы, вероятно, увидите из кода, мы указываем порт датчика, который мы прослушиваем, сохраняем данные в виде строк, а затем открываем порт для SD Reader и сохраняем данные. Все находится в цикле.

Проблема в том, что мы получаем только около 5 Гц (10 Гц только с одним датчиком), видимые по таймеру, чего, к сожалению, недостаточно для нашего проекта.

Я слышал об использовании буфера fifo, но я не очень хорош в низкоуровневом программировании. В любом случае, вы можете помочь? Кроме того, считаете ли вы, что удаление всех Serial.print может помочь с небольшим повышением производительности?

датчики - https://wiki.dfrobot.com/Serial_6_Axis_Accelerometer_SKU_SEN0386#target_0

uCon - Arduino Uno Rev3

#include <DFRobot_WT61PC.h>
#include <SoftwareSerial.h>
#include <SPI.h>
#include <SD.h>

#define FILE_BASE_NAME "Pomiar"
#define VCC2 7

const uint8_t CS_PIN = 4;

SoftwareSerial mySerial(2, 3);
SoftwareSerial mySerial2(5, 6);
DFRobot_WT61PC sensor(&mySerial);
DFRobot_WT61PC sensor2(&mySerial2);


File myFile;
File file;

const uint8_t BASE_NAME_SIZE = sizeof(FILE_BASE_NAME) - 1;
char fileName[] = FILE_BASE_NAME "00.txt";

void setup()
{

  pinMode(VCC2,OUTPUT);
  digitalWrite(VCC2,HIGH);

  //Использовать Serial в качестве последовательного порта для отладки 
  Serial.begin(9600);
  Serial.print("Initializing SD card...");

  if (!SD.begin(4)) {
    Serial.println("initialization failed!");
    while (1);
  }
  Serial.println("initialization done.");

  while (SD.exists(fileName)) {
    if (fileName[BASE_NAME_SIZE + 1] != '9') {
      fileName[BASE_NAME_SIZE + 1]++;
    } else if (fileName[BASE_NAME_SIZE] != '9') {
      fileName[BASE_NAME_SIZE + 1] = '0';
      fileName[BASE_NAME_SIZE]++;
    } else {
      Serial.println(F("Can't create file name"));
      return;
    }
  }

  
  //Использовать программный последовательный порт mySerial в качестве порта связи seiral 
  mySerial.begin(9600);
  mySerial2.begin(9600);
  // Измените частоту вывода данных датчика FREQUENCY_0_1HZ на 0,1 Гц, FREQUENCY_0_5HZ на 0,5 Гц, FREQUENCY_1HZ для 1 Гц, FREQUENCY_2HZ для 2 Гц,
  // FREQUENCY_5HZ для 5 Гц, FREQUENCY_10HZ для 10 Гц, FREQUENCY_20HZ для 20 Гц, FREQUENCY_50HZ для 50 Гц,
  // FREQUENCY_100HZ для 100 Гц, FREQUENCY_125HZ для 125 Гц, FREQUENCY_200HZ для 200 Гц.
  sensor.modifyFrequency(FREQUENCY_100HZ);
  sensor2.modifyFrequency(FREQUENCY_100HZ);
}


void loop()
{

  long timestamp = millis();

  String acc1 = "";
  String acc2 = "";
  String acc3 = "";
  String gyro1 = "";
  String gyro2 = "";
  String gyro3 = "";
  String angle1 = "";
  String angle2 = "";
  String angle3 = "";

  String acc12 = "";
  String acc22 = "";
  String acc32 = "";
  String gyro12 = "";
  String gyro22 = "";
  String gyro32 = "";
  String angle12 = "";
  String angle22 = "";
  String angle32 = "";
  
  mySerial.listen();
  if (sensor.available()) {
    
    acc1 = sensor.Acc.X;
    acc2 = sensor.Acc.Y;
    acc3 = sensor.Acc.Z;
    gyro1 = sensor.Gyro.X;
    gyro2 = sensor.Gyro.Y;
    gyro3 = sensor.Gyro.Z;
    angle1 = sensor.Angle.X;
    angle2 = sensor.Angle.Y;
    angle3 = sensor.Angle.Z;
    
    Serial.print("Acc\t"); Serial.print(sensor.Acc.X); Serial.print("\t"); Serial.print(sensor.Acc.Y); Serial.print("\t"); Serial.println(sensor.Acc.Z); //информация об ускорении X, Y,Z
    Serial.print("Gyro\t"); Serial.print(sensor.Gyro.X); Serial.print("\t"); Serial.print(sensor.Gyro.Y); Serial.print("\t"); Serial.println(sensor.Gyro.Z); //информация об угловой скорости X, Y,Z
    Serial.print("Angle\t"); Serial.print(sensor.Angle.X); Serial.print("\t"); Serial.print(sensor.Angle.Y); Serial.print("\t"); Serial.println(sensor.Angle.Z); // информация об углах X, Y, Z 
    Serial.println(" ");
  }

  mySerial2.listen();
  if (sensor2.available()) {
    
    acc12 = sensor2.Acc.X;
    acc22 = sensor2.Acc.Y;
    acc32 = sensor2.Acc.Z;
    gyro12 = sensor2.Gyro.X;
    gyro22 = sensor2.Gyro.Y;
    gyro32 = sensor2.Gyro.Z;
    angle12 = sensor2.Angle.X;
    angle22 = sensor2.Angle.Y;
    angle32 = sensor2.Angle.Z;
    
    Serial.print("Acc2\t"); Serial.print(sensor2.Acc.X); Serial.print("\t"); Serial.print(sensor2.Acc.Y); Serial.print("\t"); Serial.println(sensor2.Acc.Z); //информация об ускорении X, Y,Z
    Serial.print("Gyro2\t"); Serial.print(sensor2.Gyro.X); Serial.print("\t"); Serial.print(sensor2.Gyro.Y); Serial.print("\t"); Serial.println(sensor2.Gyro.Z); // информация об угловой скорости X, Y,Z
    Serial.print("Angle2\t"); Serial.print(sensor2.Angle.X); Serial.print("\t"); Serial.print(sensor2.Angle.Y); Serial.print("\t"); Serial.println(sensor2.Angle.Z); // информация об углах X, Y, Z 
    Serial.println(" ");
  }

  myFile = SD.open(fileName, FILE_WRITE);

  if (myFile) {
    myFile.print(timestamp);
    myFile.print("|");
    myFile.print(acc1);
    myFile.print("|");
    myFile.print(acc2);
    myFile.print("|");
    myFile.print(acc3);
    myFile.print("|");
    myFile.print(gyro1);
    myFile.print("|");
    myFile.print(gyro2);
    myFile.print("|");
    myFile.print(gyro3);
    myFile.print("|");
    myFile.print(angle1);
    myFile.print("|");
    myFile.print(angle2);
    myFile.print("|");
    myFile.print(angle3);
    myFile.println("");

    myFile.print(timestamp);
    myFile.print("|");
    myFile.print(acc12);
    myFile.print("|");
    myFile.print(acc22);
    myFile.print("|");
    myFile.print(acc32);
    myFile.print("|");
    myFile.print(gyro12);
    myFile.print("|");
    myFile.print(gyro22);
    myFile.print("|");
    myFile.print(gyro32);
    myFile.print("|");
    myFile.print(angle12);
    myFile.print("|");
    myFile.print(angle22);
    myFile.print("|");
    myFile.print(angle32);
    myFile.println("");
    
    myFile.close();
  } else {
    Serial.println("error opening test.txt");
  }
}