QT C++ 草包版八轴飞控系统控制算法

假设每个电机的推力可以独立控制，并且系统能够获取每个电机的当前状态。依次实现姿态估计、PID控制器和八轴控制逻辑。

### 八轴飞控系统控制算法

#### 1. 传感器融合（姿态估计）

使用互补滤波器进行姿态估计：

```cpp
class AttitudeEstimator {
public:
    double roll, pitch, yaw;
    double alpha;

    AttitudeEstimator(double alpha) : roll(0), pitch(0), yaw(0), alpha(alpha) {}

    void update(double accel_x, double accel_y, double accel_z, 
                double gyro_x, double gyro_y, double gyro_z, double dt) {
        double pitch_accel = atan2(accel_y, accel_z);
        double roll_accel = atan2(-accel_x, sqrt(accel_y * accel_y + accel_z * accel_z));

        roll += gyro_x * dt;
        pitch += gyro_y * dt;
        yaw += gyro_z * dt;

        roll = alpha * roll + (1 - alpha) * roll_accel;
        pitch = alpha * pitch + (1 - alpha) * pitch_accel;
        // 对于yaw的估计，通常需要引入磁力计，但这里简化处理
    }
};
```

#### 2. PID控制器

定义PID控制器：

```cpp
class PIDController {
public:
    double Kp, Ki, Kd;
    double error, integral, derivative;
    double prev_error;

    PIDController(double Kp, double Ki, double Kd) 
        : Kp(Kp), Ki(Ki), Kd(Kd), error(0), integral(0), derivative(0), prev_error(0) {}

    double update(double setpoint, double current, double dt) {
        error = setpoint - current;
        integral += error * dt;
        derivative = (error - prev_error) / dt;
        prev_error = error;

        return Kp * error + Ki * integral + Kd * derivative;
    }
};
```

#### 3. 八轴控制逻辑

定义电机控制和主控制循环：

```cpp
class MotorController {
public:
    double motor_speeds[8];

    MotorController() {
        for (int i = 0; i < 8; ++i) {
            motor_speeds[i] = 0;
        }
    }

    void setMotorSpeed(int index, double speed) {
        if (index >= 0 && index < 8) {
            motor_speeds[index] = speed;
        }
    }

    void updateMotors(double roll_output, double pitch_output, double yaw_output) {
        // 简单分配电机速度，根据不同的飞行器架构进行分配
        // 这里假设一个简单的+配置
        motor_speeds[0] = roll_output + pitch_output + yaw_output;
        motor_speeds[1] = -roll_output + pitch_output - yaw_output;
        motor_speeds[2] = roll_output - pitch_output - yaw_output;
        motor_speeds[3] = -roll_output - pitch_output + yaw_output;
        motor_speeds[4] = roll_output + pitch_output - yaw_output;
        motor_speeds[5] = -roll_output + pitch_output + yaw_output;
        motor_speeds[6] = roll_output - pitch_output + yaw_output;
        motor_speeds[7] = -roll_output - pitch_output - yaw_output;

        // 输出电机速度
        for (int i = 0; i < 8; ++i) {
            std::cout << "Motor " << i + 1 << " speed: " << motor_speeds[i] << std::endl;
        }
    }
};

int main() {
    AttitudeEstimator estimator(0.98);
    PIDController rollController(1.0, 0.1, 0.05);
    PIDController pitchController(1.0, 0.1, 0.05);
    PIDController yawController(1.0, 0.1, 0.05);
    MotorController motorController;

    double dt = 0.01;
    double accel_x = 0.0, accel_y = 0.0, accel_z = 1.0;
    double gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0;
    double setpoint_roll = 0.0;
    double setpoint_pitch = 0.0;
    double setpoint_yaw = 0.0;

    for (int i = 0; i < 1000; ++i) {
        estimator.update(accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, dt);

        double current_roll = estimator.roll;
        double current_pitch = estimator.pitch;
        double current_yaw = estimator.yaw;

        double roll_output = rollController.update(setpoint_roll, current_roll, dt);
        double pitch_output = pitchController.update(setpoint_pitch, current_pitch, dt);
        double yaw_output = yawController.update(setpoint_yaw, current_yaw, dt);

        motorController.updateMotors(roll_output, pitch_output, yaw_output);

        gyro_x += 0.01;
        gyro_y += 0.01;
        gyro_z += 0.01;
    }

    return 0;
}
```

### 解释

- **姿态估计**：使用互补滤波器融合加速度计和陀螺仪数据，估计飞行器的姿态。
- **PID控制器**：每个方向（横滚、俯仰、偏航）都有一个PID控制器，计算控制输出。
- **电机控制**：根据PID控制器的输出调整八个电机的速度，假设一个简单的+配置。

### 调整和优化

- **传感器数据**：需要从实际传感器中读取数据，并处理噪声。
- **滤波器和PID参数**：需要通过实验调整滤波器系数和PID参数，以获得最佳控制效果。
- **电机分配逻辑**：实际中需要根据飞行器的具体架构调整电机分配逻辑，以实现稳定飞行。