56、C++项目实战：无人机飞控模拟

无人机飞控，说白了就是让飞机自己稳住、自己飞。我最早接触这玩意儿是在一个业余航模项目里，当时代码写得乱七八糟，飞机一上天就翻跟头。后来我才明白，飞控的核心不是控制本身，而是对传感器数据的实时处理。

今天这个项目，咱们用C++模拟一个简化版的无人机飞控系统。你想想看，真飞机上天前，代码得先在仿真环境里跑通。这就是我们做这个模拟的目的。

飞控系统的核心模块

一个完整的飞控系统，至少包含这几个部分：

• 传感器模块：读取加速度计、陀螺仪、磁力计数据
• 姿态解算：把原始数据转换成俯仰、横滚、偏航角
• 控制算法：PID控制器，输出电机指令
• 执行器模块：模拟电机响应

我个人习惯把每个模块封装成独立的类，这样后期调试和替换算法都很方便。嗯，这里要注意，传感器数据一定要加噪声模拟，否则你的控制算法在真实环境下根本扛不住。

姿态解算：从原始数据到欧拉角

姿态解算有很多方法，比如互补滤波、Mahony滤波、EKF。咱们这个项目用最简单的互补滤波，因为它计算量小，适合嵌入式环境。

核心思路是这样的：

• 陀螺仪积分得到角度，但会漂移
• 加速度计算出的角度不漂移，但噪声大
• 两者互补，取长补短

我曾经在一个四轴项目里只用陀螺仪积分，结果飞了30秒后飞机自己开始旋转……从那以后我再也不敢只用单一传感器了。

class AttitudeEstimator {
public:
    AttitudeEstimator(float dt) : dt_(dt) {
        roll_ = 0.0f;
        pitch_ = 0.0f;
        yaw_ = 0.0f;
    }

    void update(float gx, float gy, float gz, float ax, float ay, float az) {
        // 陀螺仪积分
        float roll_gyro = roll_ + gx * dt_;
        float pitch_gyro = pitch_ + gy * dt_;
        float yaw_gyro = yaw_ + gz * dt_;

        // 加速度计计算角度
        float roll_acc = atan2f(ay, az);
        float pitch_acc = atan2f(-ax, sqrtf(ay*ay + az*az));

        // 互补滤波
        const float alpha = 0.98f;
        roll_ = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
        pitch_ = alpha * pitch_gyro + (1.0f - alpha) * pitch_acc;
        yaw_ = yaw_gyro;  // 偏航角依赖磁力计，这里简化
    }

    float getRoll() const { return roll_; }
    float getPitch() const { return pitch_; }
    float getYaw() const { return yaw_; }

private:
    float dt_;
    float roll_, pitch_, yaw_;
};

PID控制器：让飞机稳住

PID控制器是飞控的灵魂。说白了就是三个字：比、积、微。

• 比例项：当前偏差有多大，就拉多大力
• 积分项：消除稳态误差，但容易积分饱和
• 微分项：预测偏差趋势，抑制震荡

我刚开始调PID时，总是把P调得很大，结果飞机像抽风一样抖。后来才明白，P和D要配合着调，先P后D再I，这个顺序不能乱。

class PIDController {
public:
    PIDController(float kp, float ki, float kd, float dt)
        : kp_(kp), ki_(ki), kd_(kd), dt_(dt) {
        integral_ = 0.0f;
        prev_error_ = 0.0f;
    }

    float compute(float setpoint, float measurement) {
        float error = setpoint - measurement;

        // 比例项
        float p_term = kp_ * error;

        // 积分项（带限幅）
        integral_ += error * dt_;
        integral_ = clamp(integral_, -100.0f, 100.0f);
        float i_term = ki_ * integral_;

        // 微分项
        float derivative = (error - prev_error_) / dt_;
        float d_term = kd_ * derivative;

        prev_error_ = error;

        return p_term + i_term + d_term;
    }

private:
    float kp_, ki_, kd_, dt_;
    float integral_;
    float prev_error_;

    float clamp(float value, float min, float max) {
        return (value < min) ? min : (value > max) ? max : value;
    }
};

系统架构图

下面这张图展示了整个飞控模拟的数据流。我习惯先画图再写代码，这样结构清晰，不容易漏掉模块。

主循环：把一切串起来

飞控的主循环其实很简单，就是不断重复：读传感器 -> 算姿态 -> 算控制 -> 输出电机。我见过有人把主循环写得特别复杂，各种状态机嵌套，其实没必要。

class FlightController {
public:
    FlightController(float dt)
        : estimator_(dt), pid_roll_(1.5f, 0.02f, 0.1f, dt),
          pid_pitch_(1.5f, 0.02f, 0.1f, dt), dt_(dt) {}

    void update(float gx, float gy, float gz, float ax, float ay, float az,
                float target_roll, float target_pitch) {
        // 1. 姿态解算
        estimator_.update(gx, gy, gz, ax, ay, az);

        // 2. PID控制
        float roll_output = pid_roll_.compute(target_roll, estimator_.getRoll());
        float pitch_output = pid_pitch_.compute(target_pitch, estimator_.getPitch());

        // 3. 混合输出到电机（简化版）
        motor_fl_ = 1000 + roll_output + pitch_output;
        motor_fr_ = 1000 - roll_output + pitch_output;
        motor_bl_ = 1000 + roll_output - pitch_output;
        motor_br_ = 1000 - roll_output - pitch_output;

        // 限幅
        clampMotors(1000, 2000);
    }

private:
    AttitudeEstimator estimator_;
    PIDController pid_roll_, pid_pitch_;
    float dt_;
    float motor_fl_, motor_fr_, motor_bl_, motor_br_;

    void clampMotors(float min, float max) {
        motor_fl_ = std::max(min, std::min(max, motor_fl_));
        motor_fr_ = std::max(min, std::min(max, motor_fr_));
        motor_bl_ = std::max(min, std::min(max, motor_bl_));
        motor_br_ = std::max(min, std::min(max, motor_br_));
    }
};

调试与避坑

调试飞控模拟，我一般分三步走：

1. 开环测试：不给控制，只读传感器，看姿态数据是否合理
2. 单轴调试：只控制横滚，看响应是否平滑
3. 联合调试：所有轴一起上，看耦合情况

我曾经在单轴调试时发现横滚响应总是滞后，查了半天发现是dt设置错了——我用的毫秒，但代码里当成了秒。这种低级错误，嗯，谁还没犯过呢？

调试阶段	常见问题	解决方法
开环测试	角度跳变、数据异常	检查传感器噪声模型，确认单位一致
单轴调试	震荡、超调	降低P增益，增加D增益
联合调试	轴间耦合、偏航漂移	检查电机混控矩阵，确认安装方向

好了，这个无人机飞控模拟项目就讲到这里。代码量不大，但涉及的知识点很密集——传感器融合、控制理论、实时系统，每一个都值得深挖。你把这个项目跑通了，再去玩真飞机，心里就有底了。

---