diff --git a/starterkit.ino b/starterkit.ino index b5d4b93..688d4ba 100644 --- a/starterkit.ino +++ b/starterkit.ino @@ -1,10 +1,8 @@ -#include #include #include #include #include - // Пины сервоприводов #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; - - // Получение данных с DMP - if (mpuInterrupt && fifoCount < packetSize) { - mpuInterrupt = false; - mpuIntStatus = mpu.getIntStatus(); - fifoCount = mpu.getFIFOCount(); + // Таймер на 10 мс + if (millis() - prevUpdate >= UPDATE_INTERVAL) { + prevUpdate = millis(); - 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);