Обновить starterkit.ino

starterkit.ino

@@ -1,10 +1,8 @@
-#include <I2Cdev.h>
 #include <MPU6050_6Axis_MotionApps20.h>
 #include <Wire.h>
 #include <Servo.h>
 #include <GyverPID.h>
 
-
 // Пины сервоприводов
 #define AILERON_LEFT_PIN 9
 #define AILERON_RIGHT_PIN 10
@@ -14,33 +12,26 @@
 // Объекты сервоприводов
 Servo aileron_left, aileron_right, elevator, rudder;
 
-// ПИД-регуляторы (Kp, Ki, Kd, dt)
-GyverPID pid_roll(6.0, 0.2, 20.0, 10);   // Крен
-GyverPID pid_pitch(5.0, 0.1, 15.0, 10); // Тангаж
-GyverPID pid_yaw(3.0, 0.05, 8.0, 10);   // Рыскание
+// ПИД-регуляторы (Kp, Ki, Kd)
+GyverPID pid_roll(6.0, 0.2, 20.0);   // Крен
+GyverPID pid_pitch(5.0, 0.1, 15.0); // Тангаж
+GyverPID pid_yaw(3.0, 0.05, 8.0);    // Рыскание
 
 // MPU6050 и DMP
 MPU6050 mpu;
 bool dmpReady = false;
-uint8_t mpuIntStatus;
-uint8_t devStatus;
 uint16_t packetSize;
-uint16_t fifoCount;
 uint8_t fifoBuffer[64];
 Quaternion q;
 VectorFloat gravity;
 float ypr[3]; // [yaw, pitch, roll]
 
+// Тайминг
+uint32_t prevUpdate = 0;
+const uint16_t UPDATE_INTERVAL = 10; // 10 мс (100 Гц)
+
 // Эмулированные параметры
 float target_altitude = 100.0;
-volatile bool mpuInterrupt = false;
-
-// ================================================================
-// Прерывание для DMP
-// ================================================================
-void dmpDataReady() {
-  mpuInterrupt = true;
-}
 
 // ================================================================
 // Настройка
@@ -52,9 +43,9 @@ void setup() {
 
   // Инициализация MPU6050
   mpu.initialize();
-  devStatus = mpu.dmpInitialize();
+  uint8_t devStatus = mpu.dmpInitialize();
   
-  // Калибровка гироскопа (значения для вашего модуля!)
+  // Калибровка (подставьте свои значения!)
   mpu.setXGyroOffset(220);
   mpu.setYGyroOffset(76);
   mpu.setZGyroOffset(-85);
@@ -62,10 +53,10 @@ void setup() {
 
   if (devStatus == 0) {
     mpu.setDMPEnabled(true);
-    attachInterrupt(0, dmpDataReady, RISING);
-    mpuIntStatus = mpu.getIntStatus();
     dmpReady = true;
     packetSize = mpu.dmpGetFIFOPacketSize();
+  } else {
+    while(1); // Остановка при ошибке инициализации
   }
 
   // Инициализация сервоприводов
@@ -88,28 +79,17 @@ void setup() {
 // Основной цикл
 // ================================================================
 void loop() {
-  if (!dmpReady) return;
+  // Таймер на 10 мс
+  if (millis() - prevUpdate >= UPDATE_INTERVAL) {
+    prevUpdate = millis();
     
-  // Получение данных с DMP
-  if (mpuInterrupt && fifoCount < packetSize) {
-    mpuInterrupt = false;
-    mpuIntStatus = mpu.getIntStatus();
-    fifoCount = mpu.getFIFOCount();
-    
-    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
-      mpu.resetFIFO();
-    } else if (mpuIntStatus & 0x02) {
-      while (fifoCount >= packetSize) {
-        mpu.getFIFOBytes(fifoBuffer, packetSize);
-        fifoCount -= packetSize;
-      }
-
-      // Получение углов Эйлера
+    // Чтение данных с DMP
+    if (dmpReady && mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
       mpu.dmpGetQuaternion(&q, fifoBuffer);
       mpu.dmpGetGravity(&gravity, &q);
       mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
       
-      // Преобразование радианы -> градусы
+      // Конвертация в градусы
       float roll = ypr[2] * 180/M_PI;
       float pitch = ypr[1] * 180/M_PI;
       float yaw = ypr[0] * 180/M_PI;
@@ -117,14 +97,14 @@ void loop() {
       // Управление высотой через тангаж
       pid_pitch.setpoint = calculate_pitch_setpoint(target_altitude);
       
-      // Обновление PID
+      // Обновление PID с авторасчетом dt
       pid_roll.input = roll;
       pid_pitch.input = pitch;
       pid_yaw.input = yaw;
       
-      pid_roll.getResult();
-      pid_pitch.getResult();
-      pid_yaw.getResult();
+      pid_roll.getResultNow(); // Автоматически считает dt
+      pid_pitch.getResultNow();
+      pid_yaw.getResultNow();
 
       // Управление сервами
       control_ailerons(pid_roll.output);