151 lines
4.8 KiB
C++
151 lines
4.8 KiB
C++
#include <MPU6050_6Axis_MotionApps20.h>
|
||
#include <Wire.h>
|
||
#include <Servo.h>
|
||
#include <GyverPID.h>
|
||
|
||
// Пины сервоприводов
|
||
#define AILERON_LEFT_PIN 9
|
||
#define AILERON_RIGHT_PIN 10
|
||
#define ELEVATOR_PIN 11
|
||
#define RUDDER_PIN 12
|
||
|
||
// Объекты сервоприводов
|
||
Servo aileron_left, aileron_right, elevator, rudder;
|
||
|
||
// ПИД-регуляторы (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;
|
||
uint16_t packetSize;
|
||
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;
|
||
|
||
// ================================================================
|
||
// Настройка
|
||
// ================================================================
|
||
void setup() {
|
||
Wire.begin();
|
||
Wire.setClock(400000);
|
||
Serial.begin(115200);
|
||
|
||
// Инициализация MPU6050
|
||
mpu.initialize();
|
||
uint8_t devStatus = mpu.dmpInitialize();
|
||
|
||
// Калибровка (подставьте свои значения!)
|
||
mpu.setXGyroOffset(220);
|
||
mpu.setYGyroOffset(76);
|
||
mpu.setZGyroOffset(-85);
|
||
mpu.setZAccelOffset(1788);
|
||
|
||
if (devStatus == 0) {
|
||
mpu.setDMPEnabled(true);
|
||
dmpReady = true;
|
||
packetSize = mpu.dmpGetFIFOPacketSize();
|
||
} else {
|
||
while(1); // Остановка при ошибке инициализации
|
||
}
|
||
|
||
// Инициализация сервоприводов
|
||
aileron_left.attach(AILERON_LEFT_PIN);
|
||
aileron_right.attach(AILERON_RIGHT_PIN);
|
||
elevator.attach(ELEVATOR_PIN);
|
||
rudder.attach(RUDDER_PIN);
|
||
|
||
// Настройка PID
|
||
pid_roll.setDirection(NORMAL);
|
||
pid_pitch.setDirection(NORMAL);
|
||
pid_yaw.setDirection(NORMAL);
|
||
|
||
pid_roll.setLimits(-50, 50);
|
||
pid_pitch.setLimits(-30, 30);
|
||
pid_yaw.setLimits(-25, 25);
|
||
}
|
||
|
||
// ================================================================
|
||
// Основной цикл
|
||
// ================================================================
|
||
void loop() {
|
||
// Таймер на 10 мс
|
||
if (millis() - prevUpdate >= UPDATE_INTERVAL) {
|
||
prevUpdate = millis();
|
||
|
||
// Чтение данных с 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;
|
||
|
||
// Управление высотой через тангаж
|
||
pid_pitch.setpoint = calculate_pitch_setpoint(target_altitude);
|
||
|
||
// Обновление PID с авторасчетом dt
|
||
pid_roll.input = roll;
|
||
pid_pitch.input = pitch;
|
||
pid_yaw.input = yaw;
|
||
|
||
pid_roll.getResultNow(); // Автоматически считает dt
|
||
pid_pitch.getResultNow();
|
||
pid_yaw.getResultNow();
|
||
|
||
// Управление сервами
|
||
control_ailerons(pid_roll.output);
|
||
control_elevator(pid_pitch.output);
|
||
control_rudder(pid_yaw.output);
|
||
}
|
||
}
|
||
}
|
||
|
||
// ================================================================
|
||
// Управление элеронами (противофазное)
|
||
// ================================================================
|
||
void control_ailerons(float pid_output) {
|
||
int left_angle = 90 + pid_output; // Нейтрал 90°
|
||
int right_angle = 90 - pid_output;
|
||
|
||
aileron_left.write(constrain(left_angle, 0, 180));
|
||
aileron_right.write(constrain(right_angle, 0, 180));
|
||
}
|
||
|
||
// ================================================================
|
||
// Управление рулём высоты
|
||
// ================================================================
|
||
void control_elevator(float pid_output) {
|
||
int angle = 90 + pid_output;
|
||
elevator.write(constrain(angle, 0, 180));
|
||
}
|
||
|
||
// ================================================================
|
||
// Управление рулём направления
|
||
// ================================================================
|
||
void control_rudder(float pid_output) {
|
||
int angle = 90 + pid_output;
|
||
rudder.write(constrain(angle, 0, 180));
|
||
}
|
||
|
||
// ================================================================
|
||
// Эмуляция высоты и расчёт тангажа
|
||
// ================================================================
|
||
float calculate_pitch_setpoint(float target_alt) {
|
||
static float current_alt = 100.0; // Эмуляция текущей высоты
|
||
float alt_error = target_alt - current_alt;
|
||
return constrain(alt_error * 0.5, -15, 15);
|
||
} |