MPU6050 Проблема с NodeMCU

Я использую MPU6050 как часть IMU моего дрона, и я написал эту программу для Arduino Nano, чтобы получить ориентацию модуля MPU6050 в 3D-пространстве. И это прекрасно работает, но когда я загрузил этот точный код в NodeMCU, вывод начал только увеличиваться, т.е. всякий раз, когда происходит какое-либо изменение ориентации модуля, вывод вместо того, чтобы показывать конкретную ориентацию, увеличивается и на. Не могли бы вы помочь мне с интерфейсом MPU6050 с моим NodeMCU. Я предоставляю программу внизу.

#include <Wire.h>
const int MPU = 0x68; // I2C-адрес MPU6050

char status;                // Объявление всех необходимых переменных
double T, P;
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll_sensor, pitch_sensor, yaw_sensor;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();                      // Инициализация связи
  Wire.beginTransmission(MPU);       // Начать связь с MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Обращение к регистру 6B
  Wire.write(0x00);                  // Сделать сброс - поместить 0 в регистр 6B
  Wire.endTransmission(true);        // конец передачи
  calculate_IMU_error();  // Вызовите эту функцию, если вам нужно получить значения ошибок IMU для вашего модуля
  delay(20);
}
void loop() {
  // === Чтение данных ускорителя === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Начать с регистра 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true); //Чтение всего 6 регистров, значение каждой оси хранится в 2 регистрах

  //Для диапазона +-2g нам нужно разделить необработанные значения на 16384, согласно таблице данных
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси X
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Y
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // значение по оси Z

  // Расчет roll_sensor и pitch_sensor по данным акселерометра
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * (180 / PI)) - AccErrorX;
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * (180 / PI)) - AccErrorY;

  // === Чтение данных гироскопа === //
  previousTime = currentTime;        // Предыдущее время сохраняется перед чтением фактического времени
  currentTime = millis();            // Текущее время фактическое время чтения
  elapsedTime = (currentTime - previousTime) / 1000; // Делим на 1000, чтобы получить секунды
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Адрес первого регистра данных гироскопа 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 14, true); //Чтение всего 4 регистра, значение каждой оси хранится в 2 регистрах

  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Для диапазона 250 град/с мы должны сначала разделить необработанное значение на 131,0, согласно техническому описанию
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

  // Исправьте выходные данные с вычисленными значениями ошибки
  GyroX = GyroX - GyroErrorX;
  GyroY = GyroY - GyroErrorY;
  GyroZ = GyroZ - GyroErrorZ;

  // В настоящее время необработанные значения выражены в градусах в секунду, град/с, поэтому нам нужно умножить на сендоны (с), чтобы получить угол в градусах
  gyroAngleX = gyroAngleX + (GyroX * elapsedTime); // град/с * с = град
  gyroAngleY = gyroAngleY + (GyroY * elapsedTime);
  yaw_sensor =  yaw_sensor + (GyroZ * elapsedTime);

  // Дополнительный фильтр - объединить значения акселерометра и угла гироскопа
  roll_sensor = (0.94 * gyroAngleX) + (0.06 * accAngleX);
  pitch_sensor = (0.94 * gyroAngleY) + (0.06 * accAngleY);
  

  // Печатаем значения на последовательном мониторе
  Serial.print("                   roll_GY: "); Serial.print(roll_sensor);
  Serial.print("         pitch_GY: "); Serial.print(pitch_sensor);
  Serial.print("         yaw_GY: "); Serial.print(yaw_sensor);
  Serial.println();
  
  
}
void calculate_IMU_error() {
  // Мы можем вызвать эту функцию в разделе настройки, чтобы рассчитать погрешность данных акселерометра и гироскопа. Отсюда мы получим значения ошибок, используемые в приведенных выше уравнениях, напечатанных на последовательном мониторе.
  // Обратите внимание, что мы должны разместить IMU горизонтально, чтобы получить правильные значения, чтобы мы могли затем получить правильные значения
  // Чтение значений акселерометра 300 раз
  while (c < 300) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Суммируем все показания
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * (180 / PI)));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * (180 / PI)));
    c++ ;
  }
  // Делим сумму на 200, чтобы получить значение ошибки
  AccErrorX = AccErrorX / 300;
  AccErrorY = AccErrorY / 300;
  c = 0;
  // Чтение значений гироскопа 300 раз
  while (c < 300) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);

    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();

    // Суммируем все показания
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    c++;
  }

  // Делим сумму на 300, чтобы получить значение ошибки
  GyroErrorX = GyroErrorX / 300;
  GyroErrorY = GyroErrorY / 300;
  GyroErrorZ = GyroErrorZ / 300;
}