无人机姿态控制程序,采用C++实现

以下是一个带有详细注释、可运行的C++无人机姿态控制程序示例，包含PID控制器和姿态稳定逻辑：

```cpp
#include <iostream>
#include <cmath>
#include <chrono>
#include <thread>
#include <random>

// ================= PID控制器类 =================
class PIDController {
private:
    double kp;          // 比例系数
    double ki;          // 积分系数
    double kd;          // 微分系数
    double integral;    // 积分项累积值
    double prev_error;  // 上一次误差
    std::chrono::time_point<std::chrono::steady_clock> last_time;  // 上次计算时间

public:
    // 构造函数初始化PID参数
    PIDController(double p, double i, double d) 
        : kp(p), ki(i), kd(d), integral(0), prev_error(0) {
        last_time = std::chrono::steady_clock::now();
    }

    // PID计算函数
    double calculate(double setpoint, double current_value) {
        // 计算时间差
        auto now = std::chrono::steady_clock::now();
        double dt = std::chrono::duration_cast<std::chrono::milliseconds>(
            now - last_time).count() / 1000.0;
        dt = dt > 0 ? dt : 0.001;  // 防止零时间差

        double error = setpoint - current_value;  // 计算误差