上一章介绍了PID控制器的基础理论和实现方法。本章将在此基础上，讲解如何构建串级PID控制结构来实现四旋翼飞行器的姿态控制，包括角度环和角速度环的设计、电机混控算法、输入映射以及完整的飞控代码集成。

15.1 串级PID控制结构

15.1.1 为什么需要串级控制

四旋翼飞行器的姿态动力学是二阶系统：电机力矩产生角加速度，角加速度积分得到角速度，角速度积分得到角度。如果只用单级PID直接控制角度，控制器需要同时处理二阶动态特性，调参困难且抗干扰能力差。

串级控制（Cascade Control）将控制任务分解为两个层次：

(1) 外环（角度环）：将角度误差转化为期望角速度——这是"策略层"。

(2) 内环（角速度环）：将角速度误差转化为电机修正量——这是"执行层"。

这种分层设计使得每一层只需处理较简单的动态特性，大幅简化了调参过程。

15.1.2 串级控制框图

完整的串级PID控制系统框图如下：

角度环(外环) 角速度环(内环)

目标角度 目标角速度 电机修正量

─────────> [P] ──────────> [PID] ──────────> [电机混控] ──> [电机+气动]

↑ │ ↑ │

│ │ │ │

│ ↓ ↓ ↓

遥控器/APP 实际角度 ← 互补滤波 实际角速度 ← 陀螺仪 IMU传感器

(加速度+陀螺仪) (直接读取)

15.1.3 内外环频率设计

串级控制的一个关键设计决策是内外环的执行频率：

控制环执行频率周期输入信号信号特点
外环（角度环）200Hz5ms互补滤波后的角度较平滑，有延迟
内环（角速度环）1kHz1ms陀螺仪原始角速度实时性好，有噪声

内环频率高于外环的原因：

(1) 陀螺仪角速度信号直接可用，无需额外计算，可以高频执行。

(2) 角速度控制需要快速响应以抵抗突发扰动（如阵风）。

(3) 角度估计（互补滤波或卡尔曼滤波）需要融合加速度计数据，计算量较大，且角度信号本身变化较慢，200Hz已经足够。

15.1.4 串级控制的优势

优势说明
更好的抗扰能力内环快速响应角速度扰动（阵风、电机不对称），外环只需处理慢变的角度偏差
更易调参先调内环（角速度PID），再调外环（角度P），各环独立
解耦角度和角速度角度控制精度由外环保证，角速度控制精度由内环保证
安全限幅外环输出（目标角速度）可以方便地限幅，防止过大倾斜角速度

:::info

  • 外环：对比【目标角度】与【当前角度】，算出期望角速度（内环设定值）
  • 内环：对比【期望角速度】与陀螺仪【实际角速度】，运行 PID 输出修正力矩
  • 力矩送入电机混控，驱动飞行器
  • 传感器实时采集角速度、角度，再次反馈进闭环，循环运算

:::

15.2 外环——角度环设计

15.2.1 角度环输入与输出

角度环（Angle Loop / Attitude Loop）的输入输出定义如下：

  • 输入：目标角度（来自遥控器/APP）和当前估计角度（来自互补滤波器）

  • 输出：期望角速度（°/s），作为内环角速度控制器的设定值

  • 执行频率：200Hz（每5ms执行一次）

15.2.2 为什么用纯P控制器

角度环通常只需要一个比例（P）控制器，不需要积分和微分项：

target_rate = Kp_angle * (target_angle - current_angle)

原因分析：

(1) 不需要积分：角度误差通过内环的角速度控制间接消除。内环PID中的积分项已经能够补偿持续的角度偏差，外环再加积分会导致双重积分和积分饱和。

(2) 不需要微分：角度信号变化较慢，微分项收益不大。而且角速度信息已经在内环中被充分利用。

(3) 纯P控制简单可靠：只有一个参数Kp_angle，调参非常容易。

:::info
外环任务：角度偏差越大，输出更大的期望角速度，指挥内环快速把角度拉回来。 仅靠 P 完全够用。

:::

15.2.3 角度限幅与速率限制

安全设计中，需要对角度环的输入和输出进行限幅：

(1) 最大倾斜角度限制：通常限制在±30度以内，防止飞行器过度倾斜失控。

(2) 目标角速度限幅：外环输出的目标角速度应限制在合理范围（如±200°/s），防止过大的角速度指令。

(3) 输入速率限制（Rate Limiter）：对目标角度的变化速率进行限制，避免遥控器输入阶跃变化导致的超调。

15.2.4 代码示例15-1：角度环P控制器

以下代码实现了带输入整形（速率限制）的角度环P控制器。

代码15-1 角度环P控制器（带输入整形）

:::info
**遥控器原始角度指令 → 角度外环P控制器 → 目标角速度 → 简易角速度内环 → 刚体飞行器模型 → 机身实际角度 **

:::

**仿真设定：
****程序运行总时长 4000 × 1ms = 4秒
****t = 500ms（i=500）时，遥控器原始目标角度从 0° 阶跃跳变到 15°
****1、你能观察到的现象（运行后绘图直观看到）
****raw_target_angle（原始遥控指令）
****在 500ms 瞬间直接从 0 → 15°，阶跃突变。
****smooth_target_angle（经过速率限制后的平滑目标角度）
****不会瞬间跳到 15°！会平缓斜坡上升。
****核心功能：ANGLE_RATE_LIMIT 500 °/s 速率限制，对遥控指令做输入整形；
****作用：模拟人猛打摇杆，不让目标角度突变，抑制飞行器冲击震荡。
****real_angle（飞行器真实倾斜角度）
****跟随平滑后的目标角度慢慢上升，试图跟踪到 15°；
****受外环 Kp、内环响应速度共同决定：
****Kp 偏小：上升慢，跟踪滞后；
****Kp 偏大：上升快，容易出现超调、小幅振荡。
****target_rate（外环输出：期望角速度）
**角度误差越大，期望角速度越大；角度接近目标时，目标角速度逐步收敛到 0。

#include <stdio.h>
#include <math.h>

#define MAX_TILT_ANGLE     30.0f   // 最大倾斜角度 (度)
#define MAX_RATE_CMD      200.0f   // 最大目标角速度 (度/秒)
#define ANGLE_RATE_LIMIT  500.0f   // 目标角度变化速率限制 (度/秒)

typedef struct {
    float Kp;                  // 角度环比例增益
    float target_angle;        // 当前目标角度（经过速率限制）
    float prev_target_angle;   // 上次的目标角度
} AngleController;

void AngleCtrl_Init(AngleController *ac, float kp) {
    ac->Kp = kp;
    ac->target_angle = 0.0f;
    ac->prev_target_angle = 0.0f;
}

float AngleCtrl_Update(AngleController *ac, float raw_target,
                       float current_angle, float dt) {
    // 步骤1: 原始目标角度硬限幅
    float target = raw_target;
    if (target >  MAX_TILT_ANGLE) target =  MAX_TILT_ANGLE;
    if (target < -MAX_TILT_ANGLE) target = -MAX_TILT_ANGLE;

    // 步骤2: 目标角度速率限制（输入整形平滑）
    float max_change = ANGLE_RATE_LIMIT * dt;
    float delta = target - ac->prev_target_angle;
    if (delta >  max_change) delta =  max_change;
    if (delta < -max_change) delta = -max_change;
    ac->target_angle = ac->prev_target_angle + delta;
    ac->prev_target_angle = ac->target_angle;

    // 步骤3: P控制器计算期望角速度
    float angle_error = ac->target_angle - current_angle;
    float target_rate = ac->Kp * angle_error;

    // 步骤4: 目标角速度限幅
    if (target_rate >  MAX_RATE_CMD) target_rate =  MAX_RATE_CMD;
    if (target_rate < -MAX_RATE_CMD) target_rate = -MAX_RATE_CMD;

    return target_rate;
}

void AngleCtrl_Reset(AngleController *ac) {
    ac->target_angle = 0.0f;
    ac->prev_target_angle = 0.0f;
}

// ==================== 仿真模型：内环简化 + 刚体飞行器 ====================
typedef struct
{
    float angle;
    float ang_vel;
    float inertia;
} Plant;

void Plant_Init(Plant *p)
{
    p->angle = 0.0f;
    p->ang_vel = 0.0f;
    p->inertia = 2.5e-5f;
}

// rate_cmd：外环输出的目标角速度（内环指令）
void Plant_Step(Plant *p, float rate_cmd, float dt)
{
    // 简化角速度内环P
    const float RATE_KP = 0.8f;
    float torque = RATE_KP * (rate_cmd - p->ang_vel);
    float ang_acc = torque / p->inertia;

    p->ang_vel += ang_acc * dt;
    p->ang_vel *= 0.999f;    // 空气阻尼
    p->angle += p->ang_vel * dt;
}

// ==================== 仿真主函数 ====================
int main(void)
{
    AngleController angle_ctrl;
    Plant plant;

    // 初始化角度外环，Kp=2.0
    AngleCtrl_Init(&angle_ctrl, 2.0f);
    Plant_Init(&plant);

    const float dt = 0.001f;
    float raw_target_angle = 0.0f;

    // 打开文件保存数据
    FILE* fp = fopen("angle_loop_data.txt", "w");
    if(fp == NULL)
    {
        printf("文件打开失败！\n");
        return -1;
    }
    fprintf(fp, "t_ms, raw_target, smooth_target, real_angle, target_rate\n");
    printf("t_ms, raw_target, smooth_target, real_angle, target_rate\n");

    // 仿真总时长 4000ms
    for(int i = 0; i < 4000; i++)
    {
        // 500ms 时遥控器阶跃给到15度
        if(i == 500)
        {
            raw_target_angle = 15.0f;
        }

        // 执行角度外环
        float target_rate = AngleCtrl_Update(&angle_ctrl, raw_target_angle, plant.angle, dt);
        // 更新飞行器模型
        Plant_Step(&plant, target_rate, dt);

        // 每10步输出一次数据
        if(i % 10 == 0)
        {
            printf("%d, %.2f, %.2f, %.3f, %.2f\n",
                   i, raw_target_angle, angle_ctrl.target_angle, plant.angle, target_rate);
            fprintf(fp,"%d, %.2f, %.2f, %.3f, %.2f\n",
                   i, raw_target_angle, angle_ctrl.target_angle, plant.angle, target_rate);
        }
    }

    fclose(fp);
    printf("\n仿真运行完毕！生成 angle_loop_data.txt\n");
    return 0;
}

ℹ️**** 注意 角度环Kp的典型值为3.0~8.0。Kp=5.0意味着10度的角度误差会产生50°/s的目标角速度。Kp过大时飞行器会对遥控器输入过于敏感，Kp过小时响应迟钝。

15.3 内环——角速度环设计

15.3.1 角速度环输入与输出

角速度环（Rate Loop / Angular Velocity Loop）的输入输出定义如下：

  • 输入：期望角速度（来自角度环输出）和当前角速度（来自陀螺仪直接读取）

  • 输出：电机修正量（无量纲值，后续与油门混合后送入PWM）

  • 执行频率：1kHz（每1ms执行一次）

15.3.2 完整PID控制器

角速度环使用完整的PID控制器：

P项（比例）：提供"刚度"，使角速度快速跟踪目标值。

I项（积分）：补偿持续力矩扰动（如重心偏移、风、电机不对称）。

D项（微分）：提供"阻尼"，抑制角速度的快速变化和振荡。

角速度环PID参数的典型范围（小型四旋翼）：

参数典型范围作用调参提示
Kp0.01 ~ 0.1提供刚度，快速响应角速度误差从小到大增加，直到出现振荡
Ki0.001 ~ 0.01消除稳态角速度误差从Kp/10开始，逐渐增加
Kd0.0001 ~ 0.001提供阻尼，抑制振荡从Kp/100开始，逐渐增加

15.3.3 D项滤波的重要性

角速度环的D项对陀螺仪噪声极其敏感。MPU-6050陀螺仪在±2000°/s量程下的噪声密度约为0.0028°/s/√Hz，带宽250Hz时噪声RMS约0.044°/s。

经过微分运算后，高频噪声被进一步放大。如果不加滤波，D项输出可能包含大量噪声，导致电机产生可听到的高频啸叫和不必要的功率消耗。

解决方案：对D项使用一阶IIR低通滤波器，截止频率通常设为50~100Hz。这样既保留了D项在控制带宽内的阻尼作用，又抑制了高频噪声。

15.3.4 代码示例15-2：完整角速度PID控制器

以下代码实现了带滤波D项的角速度PID控制器。

:::info
**遥控器角度指令 → 角度外环P(AngleController) → 目标角速度 → 角速度内环PID(RateController) → 输出力矩/PWM修正量 → 电机 **

程序启动 500ms 时，遥控器突然给到 15° 目标角度：

  1. 角度外环会平滑目标角度，不会瞬间跳变；
  2. 外环输出期望角速度；
  3. 内环 PID 驱动机身转动，让角度逐步跟踪至 15°；
  4. 稳态时角速度趋近于 0。

:::

#include <stdio.h>
#include <math.h>

// ====================== 【外环：角度P控制器】代码 ======================
#define MAX_TILT_ANGLE     30.0f   // 最大倾斜角度 (度)
#define MAX_RATE_CMD      200.0f   // 最大目标角速度 (度/秒)
#define ANGLE_RATE_LIMIT  500.0f   // 目标角度变化速率限制 (度/秒)

typedef struct {
    float Kp;                  // 角度环比例增益
    float target_angle;        // 当前目标角度（经过速率限制）
    float prev_target_angle;   // 上次的目标角度
} AngleController;

void AngleCtrl_Init(AngleController *ac, float kp) {
    ac->Kp = kp;
    ac->target_angle = 0.0f;
    ac->prev_target_angle = 0.0f;
}

float AngleCtrl_Update(AngleController *ac, float raw_target,
                       float current_angle, float dt) {
    // 步骤1: 原始目标角度硬限幅
    float target = raw_target;
    if (target >  MAX_TILT_ANGLE) target =  MAX_TILT_ANGLE;
    if (target < -MAX_TILT_ANGLE) target = -MAX_TILT_ANGLE;

    // 步骤2: 目标角度速率限制（输入整形平滑）
    float max_change = ANGLE_RATE_LIMIT * dt;
    float delta = target - ac->prev_target_angle;
    if (delta >  max_change) delta =  max_change;
    if (delta < -max_change) delta = -max_change;
    ac->target_angle = ac->prev_target_angle + delta;
    ac->prev_target_angle = ac->target_angle;

    // 步骤3: P控制器计算期望角速度
    float angle_error = ac->target_angle - current_angle;
    float target_rate = ac->Kp * angle_error;

    // 步骤4: 目标角速度限幅
    if (target_rate >  MAX_RATE_CMD) target_rate =  MAX_RATE_CMD;
    if (target_rate < -MAX_RATE_CMD) target_rate = -MAX_RATE_CMD;

    return target_rate;
}

void AngleCtrl_Reset(AngleController *ac) {
    ac->target_angle = 0.0f;
    ac->prev_target_angle = 0.0f;
}

// ====================== 【内环：角速度PID控制器】代码 ======================
typedef struct {
    float Kp, Ki, Kd;        // PID增益
    float integral;            // 积分累积
    float prev_gyro;           // 上次角速度（用于D项）
    float d_filtered;          // 滤波后的D值
    float d_filter_alpha;      // D项滤波系数
    float output_limit;        // 输出限幅
    float integral_limit;      // 积分限幅
} RateController;

void RateCtrl_Init(RateController *rc, float kp, float ki, float kd,
                   float out_limit, float dt) {
    rc->Kp = kp;
    rc->Ki = ki;
    rc->Kd = kd;
    rc->integral = 0.0f;
    rc->prev_gyro = 0.0f;
    rc->d_filtered = 0.0f;
    rc->output_limit = out_limit;
    rc->integral_limit = out_limit * 0.3f; // 积分最多贡献30%输出
    // D项滤波: 截止频率80Hz
    float tau = 1.0f / (2.0f * 3.14159f * 80.0f);
    rc->d_filter_alpha = dt / (tau + dt);
}

float RateCtrl_Update(RateController *rc, float target_rate,
                      float gyro_rate, float dt) {
    float error = target_rate - gyro_rate;
    
    // --- P项 ---
    float p_out = rc->Kp * error;
    
    // --- I项（含积分限幅抗饱和）---
    rc->integral += error * dt;
    if (rc->integral > rc->integral_limit)
        rc->integral = rc->integral_limit;
    if (rc->integral < -rc->integral_limit)
        rc->integral = -rc->integral_limit;
    float i_out = rc->Ki * rc->integral;
    
    // --- D项（测量值微分 + 低通滤波）---
    float raw_d = -(gyro_rate - rc->prev_gyro) / dt;
    rc->prev_gyro = gyro_rate;
    // 一阶IIR低通滤波
    rc->d_filtered = rc->d_filter_alpha * raw_d
                   + (1.0f - rc->d_filter_alpha) * rc->d_filtered;
    float d_out = rc->Kd * rc->d_filtered;
    
    // --- 总输出 + 限幅 ---
    float output = p_out + i_out + d_out;
    if (output > rc->output_limit) output = rc->output_limit;
    if (output < -rc->output_limit) output = -rc->output_limit;
    
    return output;
}

void RateCtrl_Reset(RateController *rc) {
    rc->integral = 0.0f;
    rc->prev_gyro = 0.0f;
    rc->d_filtered = 0.0f;
}

// ====================== 飞行器刚体仿真模型 ======================
typedef struct
{
    float angle;       // 倾斜角度 °
    float ang_vel;     // 角速度 °/s (等效陀螺仪读数)
    float inertia;     // 转动惯量
} RigidPlant;

void RigidPlant_Init(RigidPlant *p)
{
    p->angle = 0.0f;
    p->ang_vel = 0.0f;
    p->inertia = 2.5e-5f;
}

// torque：内环PID输出控制力矩
void RigidPlant_Step(RigidPlant *p, float torque, float dt)
{
    float ang_acc = torque / p->inertia;
    p->ang_vel += ang_acc * dt;
    p->ang_vel *= 0.999f;   // 气动阻尼
    p->angle += p->ang_vel * dt;
}

// ====================== 仿真主函数 ======================
int main(void)
{
    AngleController angle_ctrl;
    RateController rate_ctrl;
    RigidPlant plant;

    const float dt = 0.001f;
    // 初始化外环角度P
    AngleCtrl_Init(&angle_ctrl, 2.2f);
    // 初始化内环角速度PID，输出限幅±500
    RateCtrl_Init(&rate_ctrl, 0.08f, 0.02f, 0.0015f, 500.0f, dt);
    RigidPlant_Init(&plant);

    float raw_target_angle = 0.0f;

    FILE* fp = fopen("cascade_data.txt", "w");
    if(fp == NULL)
    {
        printf("无法创建数据文件\n");
        return -1;
    }
    fprintf(fp,"t_ms, raw_target_angle, real_angle, target_rate, gyro_rate, torque_out\n");
    printf("t_ms, raw_target_angle, real_angle, target_rate, gyro_rate, torque_out\n");

    // 仿真时长4秒
    for(int i=0; i<4000; i++)
    {
        // 500ms 遥控器阶跃指令：目标角度15°
        if(i == 500)
        {
            raw_target_angle = 15.0f;
        }

        // 1.外环角度P计算：输出期望角速度
        float target_rate = AngleCtrl_Update(&angle_ctrl, raw_target_angle, plant.angle, dt);
        // 2.内环角速度PID计算：输出控制力矩
        float torque = RateCtrl_Update(&rate_ctrl, target_rate, plant.ang_vel, dt);
        // 3.更新飞行器模型
        RigidPlant_Step(&plant, torque, dt);

        if(i % 10 == 0)
        {
            printf("%d, %.2f, %.3f, %.2f, %.3f, %.2f\n",
                   i, raw_target_angle, plant.angle, target_rate, plant.ang_vel, torque);
            fprintf(fp,"%d, %.2f, %.3f, %.2f, %.2f, %.2f\n",
                   i, raw_target_angle, plant.angle, target_rate, plant.ang_vel, torque);
        }
    }
    fclose(fp);
    printf("\n仿真结束，生成 cascade_data.txt\n");
    return 0;
}

🧪**** 实验 在代码15-2中，将D项滤波的截止频率分别设为20Hz、80Hz和200Hz，对比电机在悬停时的振动水平和角速度响应波形。频率越低电机越安静，但D项的阻尼效果也越弱。

15.4 四旋翼电机混控算法

15.4.1 X型四旋翼电机布局

本项目采用X型（X-configuration）四旋翼布局，四个电机分布在以飞行器中心为对称点的四个方向。从上方俯视：

:::info
混控算法作用： 将三维姿态控制指令（滚转、俯仰、偏航）解算分配给 4 个电机，依靠电机推力差产生力矩，实现飞行器姿态转动； 搭配 AirMode 缩放保护，防止电机饱和丢失控制能力。

:::

前方 (机头)

M2(CW) M1(CCW)

\ /

\ /

\ /

[X] 中心

/ </font>

/ </font>

/ </font>

M3(CCW) M4(CW)

后方 (机尾)

CW = 顺时针旋转

CCW = 逆时针旋转

轴距约65mm（对角线电机中心距离）

15.4.2 旋转方向与反扭矩平衡

四个电机的旋转方向安排遵循反扭矩（Yaw Torque）平衡原则：

  • M1（前右）：逆时针（CCW）→ 产生顺时针反扭矩

  • M2（前左）：顺时针（CW）→ 产生逆时针反扭矩

  • M3（后左）：逆时针（CCW）→ 产生顺时针反扭矩

  • M4（后右）：顺时针（CW）→ 产生逆时针反扭矩

当四个电机转速相同时，顺时针反扭矩和逆时针反扭矩相互抵消，飞行器不发生偏航旋转。通过差速调节可以实现偏航控制。

15.4.3 混控矩阵推导

混控算法的核心是将PID控制器的三个输出（Pitch、Roll、Yaw修正量）和油门信号分配到四个电机上。推导过程基于力和力矩平衡：

俯仰控制（Pitch）：

前倾需要后方电机加速、前方电机减速：

M1(前右) += pitch_out, M2(前左) += pitch_out

M3(后左) -= pitch_out, M4(后右) -= pitch_out

滚转控制（Roll）：

右倾需要左侧电机加速、右侧电机减速：

M1(前右) -= roll_out, M3(后右) -= roll_out

M2(前左) += roll_out, M4(后左) += roll_out

偏航控制（Yaw）：

顺时针偏航需要逆时针电机加速、顺时针电机减速：

M1(CCW) -= yaw_out, M3(CCW) -= yaw_out

M2(CW) += yaw_out, M4(CW) += yaw_out

综合以上分析，完整的混控矩阵如下：

电机位置/方向油门PitchRollYaw
M1前右/CCW++--
M2前左/CW++++
M3后左/CCW+--+
M4后右/CW+-+-

对应的电机输出计算公式：

M1 = throttle + pitch_out - roll_out - yaw_out

M2 = throttle + pitch_out + roll_out + yaw_out

M3 = throttle - pitch_out - roll_out + yaw_out

M4 = throttle - pitch_out + roll_out - yaw_out

15.4.4 归一化与饱和处理

当多个控制量叠加时，某些电机的输出可能超过PWM最大值（1000）或低于最小值（0）。处理方法有两种：

方法原理优点缺点
简单截断超过范围的电机值直接截断到0或MAX实现简单破坏了控制比例关系，可能导致姿态异常
等比缩放(Airmode)所有电机输出等比缩放，使最大值=MAX保持控制比例关系悬停点偏移，油门感觉不线性
偏移缩放将超出量从油门中扣除，保持控制量不变最接近理想行为油门被压缩

本项目推荐使用等比缩放（Airmode）方法，因为它在大机动时仍能保持姿态控制的有效性。

15.4.5 最小油门与悬停油门

电机有一个最小工作转速（约10-15% PWM），低于此值电机无法启动或运行不稳定。同时，飞行器悬停时需要约50-60%的油门来抵消重力。

在代码中，通常将PID修正量叠加在基础油门之上，基础油门由遥控器油门杆位决定（0%100%对应01000）。

15.4.6 代码示例17-3：完整电机混控函数

代码17-3 四旋翼电机混控（含归一化和饱和处理）

遥控器油门 → 外环角度P → 内环角速度PID → pitch/roll/yaw三轴控制量 →【混控MotorMix】→ 4路电机PWM

500ms 下达 15° 俯仰指令：

  1. 前侧电机 (M1,M2) 油门上升、后侧电机 (M3,M4) 下降；
  2. 机身向前倾斜，角度逐步收敛到 15°；
  3. 稳态时俯仰误差趋近于 0，四路电机恢复接近均等的悬停油门。
#include <stdio.h>
#include <math.h>
#include <stdint.h>

// ====================== 【外环：角度P控制器】 ======================
#define MAX_TILT_ANGLE     30.0f
#define MAX_RATE_CMD      200.0f
#define ANGLE_RATE_LIMIT  500.0f

typedef struct {
    float Kp;
    float target_angle;
    float prev_target_angle;
} AngleController;

void AngleCtrl_Init(AngleController *ac, float kp) {
    ac->Kp = kp;
    ac->target_angle = 0.0f;
    ac->prev_target_angle = 0.0f;
}

float AngleCtrl_Update(AngleController *ac, float raw_target,
                       float current_angle, float dt) {
    float target = raw_target;
    if (target >  MAX_TILT_ANGLE) target =  MAX_TILT_ANGLE;
    if (target < -MAX_TILT_ANGLE) target = -MAX_TILT_ANGLE;

    float max_change = ANGLE_RATE_LIMIT * dt;
    float delta = target - ac->prev_target_angle;
    if (delta >  max_change) delta =  max_change;
    if (delta < -max_change) delta = -max_change;
    ac->target_angle = ac->prev_target_angle + delta;
    ac->prev_target_angle = ac->target_angle;

    float angle_error = ac->target_angle - current_angle;
    float target_rate = ac->Kp * angle_error;

    if (target_rate >  MAX_RATE_CMD) target_rate =  MAX_RATE_CMD;
    if (target_rate < -MAX_RATE_CMD) target_rate = -MAX_RATE_CMD;
    return target_rate;
}

void AngleCtrl_Reset(AngleController *ac) {
    ac->target_angle = 0.0f;
    ac->prev_target_angle = 0.0f;
}

// ====================== 【内环：角速度PID控制器】 ======================
typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_gyro;
    float d_filtered;
    float d_filter_alpha;
    float output_limit;
    float integral_limit;
} RateController;

void RateCtrl_Init(RateController *rc, float kp, float ki, float kd,
                   float out_limit, float dt) {
    rc->Kp = kp;
    rc->Ki = ki;
    rc->Kd = kd;
    rc->integral = 0.0f;
    rc->prev_gyro = 0.0f;
    rc->d_filtered = 0.0f;
    rc->output_limit = out_limit;
    rc->integral_limit = out_limit * 0.3f;
    float tau = 1.0f / (2.0f * 3.14159f * 80.0f);
    rc->d_filter_alpha = dt / (tau + dt);
}

float RateCtrl_Update(RateController *rc, float target_rate,
                      float gyro_rate, float dt) {
    float error = target_rate - gyro_rate;
    float p_out = rc->Kp * error;

    rc->integral += error * dt;
    if (rc->integral > rc->integral_limit) rc->integral = rc->integral_limit;
    if (rc->integral < -rc->integral_limit) rc->integral = -rc->integral_limit;
    float i_out = rc->Ki * rc->integral;

    float raw_d = -(gyro_rate - rc->prev_gyro) / dt;
    rc->prev_gyro = gyro_rate;
    rc->d_filtered = rc->d_filter_alpha * raw_d + (1.0f - rc->d_filter_alpha) * rc->d_filtered;
    float d_out = rc->Kd * rc->d_filtered;

    float output = p_out + i_out + d_out;
    if (output > rc->output_limit) output = rc->output_limit;
    if (output < -rc->output_limit) output = -rc->output_limit;
    return output;
}

void RateCtrl_Reset(RateController *rc) {
    rc->integral = 0.0f;
    rc->prev_gyro = 0.0f;
    rc->d_filtered = 0.0f;
}

// ====================== 【X型四旋翼电机混控】 ======================
#define MOTOR_MAX     1000
#define MOTOR_MIN     0
#define MOTOR_IDLE    80

typedef struct {
    uint16_t motor[4];  // M1前右, M2前左, M3后左, M4后右
} MotorOutput;

MotorOutput motor_output;

void MotorMix_Update(float throttle, float pitch_out,
                     float roll_out, float yaw_out,
                     uint8_t armed) {
    if (!armed) {
        motor_output.motor[0] = MOTOR_IDLE;
        motor_output.motor[1] = MOTOR_IDLE;
        motor_output.motor[2] = MOTOR_IDLE;
        motor_output.motor[3] = MOTOR_IDLE;
        return;
    }

    float m[4];
    m[0] = throttle + pitch_out - roll_out - yaw_out; // M1
    m[1] = throttle + pitch_out + roll_out + yaw_out; // M2
    m[2] = throttle - pitch_out - roll_out + yaw_out; // M3
    m[3] = throttle - pitch_out + roll_out - yaw_out; // M4

    float max_val = m[0];
    float min_val = m[0];
    for (int i = 1; i < 4; i++) {
        if (m[i] > max_val) max_val = m[i];
        if (m[i] < min_val) min_val = m[i];
    }

    if (max_val > MOTOR_MAX) {
        float scale = (float)MOTOR_MAX / max_val;
        for (int i = 0; i < 4; i++) m[i] *= scale;
    }
    if (min_val < MOTOR_MIN) {
        float offset = (float)MOTOR_MIN - min_val;
        for (int i = 0; i < 4; i++) m[i] += offset;
        for (int i = 0; i < 4; i++) if (m[i] > MOTOR_MAX) m[i] = MOTOR_MAX;
    }

    for (int i = 0; i < 4; i++) {
        if (m[i] > MOTOR_MAX) m[i] = MOTOR_MAX;
        if (m[i] < MOTOR_MIN) m[i] = MOTOR_MIN;
        motor_output.motor[i] = (uint16_t)m[i];
    }
}

// PC仿真不需要硬件PWM，空实现
void set_motor_pwm(int ch, uint16_t val) {
    // 仿真环境无硬件，仅占位
}
void MotorMix_ApplyOutput(void) {
    set_motor_pwm(0, motor_output.motor[0]);
    set_motor_pwm(1, motor_output.motor[1]);
    set_motor_pwm(2, motor_output.motor[2]);
    set_motor_pwm(3, motor_output.motor[3]);
}

// ====================== 单轴刚体仿真模型（俯仰通道演示） ======================
typedef struct
{
    float angle;
    float ang_vel;
    float inertia;
} RigidPlant;

void RigidPlant_Init(RigidPlant *p)
{
    p->angle = 0.0f;
    p->ang_vel = 0.0f;
    p->inertia = 2.5e-5f;
}

void RigidPlant_Step(RigidPlant *p, float torque, float dt)
{
    float ang_acc = torque / p->inertia;
    p->ang_vel += ang_acc * dt;
    p->ang_vel *= 0.999f;
    p->angle += p->ang_vel * dt;
}

// ====================== 仿真主函数 ======================
int main(void)
{
    AngleController angle_ctrl;
    RateController rate_ctrl;
    RigidPlant plant;

    const float dt = 0.001f;
    AngleCtrl_Init(&angle_ctrl, 2.2f);
    RateCtrl_Init(&rate_ctrl, 0.08f, 0.02f, 0.0015f, 250.0f, dt);
    RigidPlant_Init(&plant);

    float raw_target_angle = 0.0f;
    const float hover_throttle = 500.0f; // 悬停油门
    uint8_t armed = 1;

    FILE* fp = fopen("quad_full_data.txt", "w");
    fprintf(fp,"t_ms, target_angle, real_angle, target_rate, gyro, pitch_out, m1,m2,m3,m4\n");
    printf("t_ms, target_angle, real_angle, target_rate, gyro, pitch_out, m1,m2,m3,m4\n");

    for(int i=0; i<4000; i++)
    {
        // 500ms给阶跃目标角度15度（俯仰）
        if(i == 500) raw_target_angle = 15.0f;

        // 外环角度P
        float target_rate = AngleCtrl_Update(&angle_ctrl, raw_target_angle, plant.angle, dt);
        // 内环角速度PID（只用俯仰通道演示）
        float pitch_out = RateCtrl_Update(&rate_ctrl, target_rate, plant.ang_vel, dt);
        float roll_out = 0.0f;
        float yaw_out = 0.0f;

        // 混控计算四路电机
        MotorMix_Update(hover_throttle, pitch_out, roll_out, yaw_out, armed);
        MotorMix_ApplyOutput();

        // 简化：用pitch输出等效力矩驱动刚体
        RigidPlant_Step(&plant, pitch_out, dt);

        if(i % 10 == 0)
        {
            printf("%d, %.2f, %.3f, %.2f, %.3f, %.2f, %d,%d,%d,%d\n",
                   i, raw_target_angle, plant.angle, target_rate, plant.ang_vel, pitch_out,
                   motor_output.motor[0],motor_output.motor[1],motor_output.motor[2],motor_output.motor[3]);
            fprintf(fp,"%d, %.2f, %.3f, %.2f, %.3f, %.2f, %d,%d,%d,%d\n",
                   i, raw_target_angle, plant.angle, target_rate, plant.ang_vel, pitch_out,
                   motor_output.motor[0],motor_output.motor[1],motor_output.motor[2],motor_output.motor[3]);
        }
    }
    fclose(fp);
    printf("\n仿真完成，输出 quad_full_data.txt\n");
    return 0;
}


提示 在首次上电测试时，务必拆掉螺旋桨！验证每个电机的转向和混控方向是否正确。可以用手轻轻倾斜飞行器，观察对应电机是否正确加速来补偿倾斜。

15.5 油门处理与输入映射

15.5.1 遥控器输入范围

标准遥控器接收器（如NRF24L01或PPM接收器）输出的通道值范围为1000~2000微秒（μs），中心值为1500μs。各通道的物理含义：

通道功能范围中心值映射目标
CH1 (Aileron)滚转1000~20001500目标滚转角 ±30°
CH2 (Elevator)俯仰1000~20001500目标俯仰角 ±30°
CH3 (Throttle)油门1000~20001000(底)PWM占空比 0~100%
CH4 (Rudder)偏航1000~20001500目标偏航速率 ±200°/s

15.5.2 油门映射

油门通道与其他通道不同：它没有中心回弹，范围从1000（最低）到2000（最高），直接映射为0~1000的PWM值：

throttle_pwm = (rc_throttle - 1000) * 1.0

即 RC值1000→PWM 0，RC值1500→PWM 500，RC值2000→PWM 1000。

15.5.3 角度和角速度输入映射

对于角度控制模式（自稳模式），遥控器杆位映射为目标角度：

target_angle = (rc_value - 1500) / 500.0 * MAX_ANGLE

其中 MAX_ANGLE 通常设为30度。即杆位从最左到最右对应-30度到+30度。

对于角速度控制模式（手动模式），杆位映射为目标角速度：

target_rate = (rc_value - 1500) / 500.0 * MAX_RATE

其中 MAX_RATE 通常设为200°/s。

15.5.4 死区处理

由于遥控器摇杆的机械精度限制，即使在中心位置，输出值也可能在1490~1510之间抖动。因此需要设置死区（Dead Band），在中心附近的一定范围内将输入视为零：

if |rc_value - 1500| < DEADBAND: input = 0

死区通常设为2550μs（对应中心值±2550）。

15.5.5 指数曲线（Expo）

线性映射在中心附近过于灵敏，不利于精确操控。指数曲线（Exponential Curve）可以降低中心附近的灵敏度，同时保持两端的灵敏度不变：

output = input^3 * expo + input * (1 - expo)

其中 input 为归一化后的输入（-1到+1），expo为指数系数（0~1）。expo=0为线性，expo=1为纯三次曲线。

15.5.6 代码示例15-4：输入处理与Expo曲线

代码15-4 遥控器输入处理（含死区和Expo）

:::info
通俗一句话： 把接收机统一的 1000~2000 原始脉冲，转换成程序算法认识的标准数值。

附带两个基础处理

  1. 摇杆死区 摇杆中立位置存在机械抖动。中间一小段区间强制输出 0，防止飞机自己乱抖。
  2. 限幅 防止用户遥控器故障输出超限数值，保护控制器不异常。

:::

#include <stdio.h>
#include <math.h>
#include <stdint.h>

#define RC_CENTER      1500
#define RC_MIN         1000
#define RC_MAX         2000
#define RC_DEADBAND    30      // 死区宽度
#define MAX_ANGLE      30.0f   // 最大倾斜角度
#define MAX_YAW_RATE  200.0f   // 最大偏航速率
#define THROTTLE_DEADBAND 50   // 油门死区

typedef struct {
    float roll_target;    // 目标滚转角 (度)
    float pitch_target;   // 目标俯仰角 (度)
    float throttle;       // 油门值 (0~1000)
    float yaw_target;     // 目标偏航速率 (度/秒)
} RCInput;

// 指数曲线函数
float apply_expo(float input, float expo) {
    return input * input * input * expo
         + input * (1.0f - expo);
}

// 处理角度通道（滚转/俯仰）
float process_angle_channel(uint16_t rc_value, float expo) {
    int16_t raw = (int16_t)rc_value - RC_CENTER;
    
    // 死区处理
    if (abs(raw) < RC_DEADBAND) {
        return 0.0f;
    }
    
    // 去死区后归一化到 -1.0 ~ +1.0
    float normalized;
    if (raw > 0) {
        normalized = (float)(raw - RC_DEADBAND)
                   / (500.0f - RC_DEADBAND);
    } else {
        normalized = (float)(raw + RC_DEADBAND)
                   / (500.0f - RC_DEADBAND);
    }
    
    // 应用指数曲线
    float shaped = apply_expo(normalized, expo);
    
    // 映射到目标角度
    return shaped * MAX_ANGLE;
}

// 处理油门通道
float process_throttle(uint16_t rc_value) {
    if (rc_value < (RC_MIN + THROTTLE_DEADBAND)) {
        return 0.0f;  // 油门死区内视为0
    }
    // 线性映射: 1050~2000 -> 0~1000
    float t = (float)(rc_value - RC_MIN - THROTTLE_DEADBAND)
            / (RC_MAX - RC_MIN - THROTTLE_DEADBAND) * 1000.0f;
    if (t > 1000.0f) t = 1000.0f;
    if (t < 0.0f) t = 0.0f;
    return t;
}

// 处理偏航通道
float process_yaw_channel(uint16_t rc_value) {
    int16_t raw = (int16_t)rc_value - RC_CENTER;
    if (abs(raw) < RC_DEADBAND) return 0.0f;
    float normalized;
    if (raw > 0) {
        normalized = (float)(raw - RC_DEADBAND) / (500.0f - RC_DEADBAND);
    } else {
        normalized = (float)(raw + RC_DEADBAND) / (500.0f - RC_DEADBAND);
    }
    return normalized * MAX_YAW_RATE;
}

void RC_Process(uint16_t rc_raw[4], RCInput *input, float expo) {
    input->roll_target  = process_angle_channel(rc_raw[0], expo);
    input->pitch_target = process_angle_channel(rc_raw[1], expo);
    input->throttle     = process_throttle(rc_raw[2]);
    input->yaw_target   = process_yaw_channel(rc_raw[3]);
}

// ====================== 仿真测试主函数 ======================
int main(void)
{
    RCInput rc_out;
    uint16_t rc_raw[4];
    float expo = 0.4f;

    FILE* fp = fopen("rc_data.txt", "w");
    fprintf(fp,"rc_raw_pitch, pitch_target_angle\n");
    printf("rc_raw_pitch, pitch_target_angle\n");

    // 模拟摇杆：从中立1500逐步推到2000
    for(uint16_t val = 1500; val <= 2000; val += 10)
    {
        rc_raw[0] = 1500;    // roll中立
        rc_raw[1] = val;     // pitch变化
        rc_raw[2] = 1600;    // 油门固定
        rc_raw[3] = 1500;    // yaw中立
        RC_Process(rc_raw, &rc_out, expo);

        printf("%d, %.3f\n", val, rc_out.pitch_target);
        fprintf(fp,"%d, %.3f\n", val, rc_out.pitch_target);
    }
    fclose(fp);
    printf("\n仿真结束，输出 rc_data.txt\n");
    return 0;
}

🧪**** 实验 测试不同的expo值（0.0, 0.3, 0.6, 0.9）对操控手感的影响。在遥控器杆位中心附近小幅度移动，观察目标角度的变化灵敏度。Expo=0.5~0.7通常能提供较好的精细操控体验。

15.6 完整串级PID飞控代码

15.6.1 系统架构概览

将前面各节实现的模块组合在一起，构成完整的串级PID飞控系统。系统的核心任务按频率分为三层：

任务频率周期内容
传感器读取+姿态解算200Hz5ms读取MPU-6050，互补滤波更新姿态角
外环（角度环）+RC处理200Hz5ms处理遥控器输入，角度P控制器
内环（角速度环）+电机输出1kHz1ms角速度PID，电机混控，PWM输出

15.6.2 代码示例15-5：完整姿态控制器

以下代码将所有控制模块整合为一个完整的姿态控制器，是飞控软件的核心部分。

代码15-5 完整串级PID姿态控制器

:::info
外部传入：目标角度、当前姿态角度、陀螺仪角速度、基础油门 → <font style="color:rgb(0, 0, 0);background-color:rgba(0, 0, 0, 0);">AttitudeCtrl_OuterLoop()</font> 角度外环 P → <font style="color:rgb(0, 0, 0);background-color:rgba(0, 0, 0, 0);">AttitudeCtrl_InnerLoop()</font> 角速度 PID + 电机混控 → 输出四路电机 PWM 数值

:::

接收遥控器目标角度和飞行器姿态传感器数据，通过串级 PID 自动计算姿态修正力矩，最终输出 4 路电机 PWM 值，控制四旋翼保持期望倾斜角度稳定飞行。 

#include <stdio.h>
#include <math.h>
#include <stdint.h>
#include <string.h>

/* ========== PID控制器结构体 ========== */
typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_meas;
    float d_filtered;
    float d_alpha;       // 微分滤波系数
    float out_limit;
    float int_limit;
} PID;

void PID_Init(PID *pid, float kp, float ki, float kd,
              float limit, float dt, float d_freq) {
    pid->Kp = kp; pid->Ki = ki; pid->Kd = kd;
    pid->integral = 0.0f;
    pid->prev_meas = 0.0f;
    pid->d_filtered = 0.0f;
    pid->out_limit = limit;
    pid->int_limit = limit * 0.3f / (fabsf(ki) + 1e-6f);
    float tau = 1.0f / (6.2832f * d_freq);
    pid->d_alpha = dt / (tau + dt);
}

float PID_Update(PID *pid, float setpoint, float meas, float dt) {
    float error = setpoint - meas;
    // P项
    float p = pid->Kp * error;
    // I项（含限幅）
    pid->integral += error * dt;
    if (pid->integral > pid->int_limit)
        pid->integral = pid->int_limit;
    if (pid->integral < -pid->int_limit)
        pid->integral = -pid->int_limit;
    float i = pid->Ki * pid->integral;
    // D项（测量值微分 + 滤波）
    float raw_d = -(meas - pid->prev_meas) / dt;
    pid->prev_meas = meas;
    pid->d_filtered = pid->d_alpha * raw_d
                    + (1.0f - pid->d_alpha) * pid->d_filtered;
    float d = pid->Kd * pid->d_filtered;
    // 输出
    float out = p + i + d;
    if (out > pid->out_limit) out = pid->out_limit;
    if (out < -pid->out_limit) out = -pid->out_limit;
    return out;
}

void PID_Reset(PID *pid) {
    pid->integral = 0.0f;
    pid->prev_meas = 0.0f;
    pid->d_filtered = 0.0f;
}

/* ========== 姿态控制器 ========== */
#define NUM_AXES  3  // Roll, Pitch, Yaw
#define AXIS_ROLL  0
#define AXIS_PITCH 1
#define AXIS_YAW   2

typedef struct {
    PID rate_pid[3];      // 角速度PID (Roll, Pitch, Yaw)
    float angle_kp[3];    // 角度环P增益
    float angle_target[3]; // 目标角度
    float rate_target[3];  // 目标角速度
    uint8_t armed;         // 解锁状态
} AttitudeController;

static AttitudeController ac;

/* --- 初始化 --- */
void AttitudeCtrl_Init(void) {
    memset(&ac, 0, sizeof(ac));

    // Roll和Pitch角速度PID
    PID_Init(&ac.rate_pid[AXIS_ROLL],
             0.05f, 0.005f, 0.0005f,
             500.0f, 0.001f, 80.0f);
    PID_Init(&ac.rate_pid[AXIS_PITCH],
             0.05f, 0.005f, 0.0005f,
             500.0f, 0.001f, 80.0f);
    // Yaw角速度PID（增益较小）
    PID_Init(&ac.rate_pid[AXIS_YAW],
             0.03f, 0.002f, 0.0f,
             300.0f, 0.001f, 50.0f);

    // 角度环P增益
    ac.angle_kp[AXIS_ROLL]  = 5.0f;
    ac.angle_kp[AXIS_PITCH] = 5.0f;
    ac.angle_kp[AXIS_YAW]   = 3.0f;
}

/* --- 外环更新（200Hz） --- */
void AttitudeCtrl_OuterLoop(float angle_est[3],
                            float target_angle[3]) {
    for (int i = 0; i < 3; i++) {
        ac.angle_target[i] = target_angle[i];
        // 角度环P控制
        float error = ac.angle_target[i] - angle_est[i];
        ac.rate_target[i] = ac.angle_kp[i] * error;
        // 目标角速度限幅
        float max_rate = (i == AXIS_YAW) ? 200.0f : 300.0f;
        if (ac.rate_target[i] >  max_rate)
            ac.rate_target[i] =  max_rate;
        if (ac.rate_target[i] < -max_rate)
            ac.rate_target[i] = -max_rate;
    }
}

/* --- 内环更新（1kHz） --- */
void AttitudeCtrl_InnerLoop(float gyro_rate[3],
                            float throttle,
                            uint16_t motor_out[4]) {
    float pid_out[3];

    for (int i = 0; i < 3; i++) {
        pid_out[i] = PID_Update(&ac.rate_pid[i],
                                 ac.rate_target[i],
                                 gyro_rate[i], 0.001f);
    }

    // X型电机混控
    float m[4];
    m[0] = throttle + pid_out[1] - pid_out[0] - pid_out[2];
    m[1] = throttle + pid_out[1] + pid_out[0] + pid_out[2];
    m[2] = throttle - pid_out[1] - pid_out[0] + pid_out[2];
    m[3] = throttle - pid_out[1] + pid_out[0] - pid_out[2];

    // 归一化
    float max_v = m[0], min_v = m[0];
    for (int i = 1; i < 4; i++) {
        if (m[i] > max_v) max_v = m[i];
        if (m[i] < min_v) min_v = m[i];
    }
    if (max_v > 1000.0f) {
        float s = 1000.0f / max_v;
        for (int i = 0; i < 4; i++) m[i] *= s;
    }
    if (min_v < 0.0f) {
        float off = -min_v;
        for (int i = 0; i < 4; i++) {
            m[i] += off;
            if (m[i] > 1000.0f) m[i] = 1000.0f;
        }
    }
    for (int i = 0; i < 4; i++) {
        if (m[i] < 0.0f) m[i] = 0.0f;
        motor_out[i] = (uint16_t)m[i];
    }
}

void AttitudeCtrl_ResetAll(void) {
    for (int i = 0; i < 3; i++) {
        PID_Reset(&ac.rate_pid[i]);
        ac.rate_target[i] = 0.0f;
        ac.angle_target[i] = 0.0f;
    }
}

// ==================== 飞行器仿真模型（单轴俯仰演示） ====================
typedef struct
{
    float angle;
    float gyro;
    float inertia;
} Plant;

void Plant_Init(Plant *p)
{
    p->angle = 0;
    p->gyro = 0;
    p->inertia = 2.5e-5f;
}

void Plant_Step(Plant *p, float torque, float dt)
{
    float acc = torque / p->inertia;
    p->gyro += acc * dt;
    p->gyro *= 0.999f;
    p->angle += p->gyro * dt;
}

// ==================== 仿真主函数 ====================
int main(void)
{
    AttitudeCtrl_Init();
    Plant plant;
    Plant_Init(&plant);

    const float dt = 0.001f;
    uint16_t motor_out[4];
    float target_angle[3] = {0,0,0};
    float angle_est[3] = {0,0,0};
    float gyro[3] = {0,0,0};
    const float hover_throttle = 500.0f;

    FILE* fp = fopen("attitude_sim.txt", "w");
    fprintf(fp,"t_ms, target_pitch, real_pitch, gyro_pitch, m1,m2,m3,m4\n");
    printf("t_ms, target_pitch, real_pitch, gyro_pitch, m1,m2,m3,m4\n");

    int outer_cnt = 0;
    for(int i=0; i<4000; i++)
    {
        // 500ms 俯仰阶跃目标角度15度
        if(i == 500)
            target_angle[AXIS_PITCH] = 15.0f;

        // 外环200Hz：每5ms运行一次 (dt=1ms，5次触发一次)
        if(outer_cnt >=5)
        {
            angle_est[AXIS_PITCH] = plant.angle;
            AttitudeCtrl_OuterLoop(angle_est, target_angle);
            outer_cnt = 0;
        }
        outer_cnt++;

        // 内环1kHz，每1ms运行
        gyro[AXIS_PITCH] = plant.gyro;
        AttitudeCtrl_InnerLoop(gyro, hover_throttle, motor_out);

        // 使用pitch控制量作为力矩驱动仿真模型
        float torque = ac.rate_pid[AXIS_PITCH].integral*ac.rate_pid[AXIS_PITCH].Ki
                     + ac.rate_pid[AXIS_PITCH].Kp*(ac.rate_target[AXIS_PITCH]-plant.gyro)
                     + ac.rate_pid[AXIS_PITCH].Kd * ac.rate_pid[AXIS_PITCH].d_filtered;
        Plant_Step(&plant, torque, dt);

        if(i%10 ==0)
        {
            printf("%d, %.2f, %.3f, %.3f, %d,%d,%d,%d\n",
                   i, target_angle[1], plant.angle, plant.gyro,
                   motor_out[0],motor_out[1],motor_out[2],motor_out[3]);
            fprintf(fp,"%d, %.2f, %.3f, %.3f, %d,%d,%d,%d\n",
                   i, target_angle[1], plant.angle, plant.gyro,
                   motor_out[0],motor_out[1],motor_out[2],motor_out[3]);
        }
    }
    fclose(fp);
    printf("\n仿真结束，输出 attitude_sim.txt\n");
    return 0;
}

15.6.3 代码示例15-6：主控制循环集成

这是**整套小四轴飞控顶层调度框架（主控制循环）**，运行在 CH58 单片机 1ms 定时器中断内； 作用：按照不同频率，有序调度你前面所有算法模块，定义整个飞控实时运行时序，把所有独立算法串联成完整可飞行的飞控程序。

  • RC 输入处理
  • 角度外环 P 控制器
  • 角速度内环 PID
  • X 型电机混控
  • 本段代码：顶层调度器，规定上面所有模块什么时候执行

代码15-6 主控制循环（定时器ISR调度）

/* 代码17-6: 主控制循环集成 */

#include “CH58x_common.h”

// 全局变量

static volatile uint32_t loop_counter = 0;

static float estimated_angle[3]; // Roll, Pitch, Yaw (度)

static float gyro_rate[3]; // 陀螺仪角速度 (度/秒)

static uint16_t rc_channels[4]; // 遥控器原始输入

static uint16_t motor_pwm[4]; // 电机PWM输出

static RCInput rc_input;

static uint8_t system_armed = 0;

/* ===== 1ms定时器中断服务程序 ===== */

void attribute((interrupt(“WCH-Interrupt-fast”)))

TMR0_IRQHandler(void) {

uint32_t count = loop_counter++;

// ---- 每1ms：角速度内环 + 电机输出 ----

read_gyro(gyro_rate); // 读取陀螺仪 (快速I2C)

if (system_armed) {

AttitudeCtrl_InnerLoop(gyro_rate,

rc_input.throttle,

motor_pwm);

MotorMix_ApplyOutput();

} else {

// 未解锁：电机怠速

for (int i = 0; i < 4; i++) motor_pwm[i] = 0;

}

// ---- 每5ms (200Hz)：传感器+姿态解算+角度外环 ----

if ((count % 5) == 0) {

// 读取加速度计

float accel[3];

read_accel(accel);

// 互补滤波更新姿态估计

ComplementaryFilter_Update(gyro_rate, accel,

0.005f, estimated_angle);

// 处理遥控器输入

RC_Read(rc_channels);

RC_Process(rc_channels, &rc_input, 0.5f);

// 解锁/上锁逻辑

if (rc_input.throttle < 50.0f &&

rc_input.yaw_target < -180.0f) {

if (!system_armed) {

system_armed = 1;

AttitudeCtrl_ResetAll();

}

}

if (rc_input.throttle < 50.0f &&

rc_input.yaw_target > 180.0f) {

system_armed = 0;

AttitudeCtrl_ResetAll();

}

// 外环角度控制

float target_angles[3] = {

rc_input.roll_target,

rc_input.pitch_target,

0.0f // Yaw无角度环

};

AttitudeCtrl_OuterLoop(estimated_angle, target_angles);

}

// ---- 每20ms (50Hz)：遥测数据发送 ----

if ((count % 20) == 0) {

// 通过UART/BLE发送飞行数据

Telemetry_Send(estimated_angle, gyro_rate,

motor_pwm, rc_input.throttle);

}

// 清除中断标志

R8_TMR0_INT_FLAG = 0xFF;

}

/* ===== 定时器初始化（1ms中断）===== */

void ControlLoop_Init(void) {

// 60MHz系统时钟, 1ms定时

R32_TMR0_CNT_END = 60000 - 1;

R8_TMR0_CTRL_MOD = 0; // 计数模式

R8_TMR0_INTER_EN = RB_TMR_IE_CYC_END;

PFIC_EnableIRQ(TMR0_IRQn);

// 初始化姿态控制器

AttitudeCtrl_Init();

}

ℹ️**** 注意 代码15-6中的解锁/上锁逻辑使用"油门最低+偏航最左"解锁、"油门最低+偏航最右"上锁的约定。这是多旋翼飞控的常见安全设计，防止意外解锁。

提示 内环角速度PID的参数对飞行品质影响最大。建议在首次试飞时采用保守的参数（Kp偏小、Ki和Kd偏小），确保安全后再逐步优化。

15.7 偏航控制特殊处理

15.7.1 偏航控制的特殊性

偏航（Yaw）控制与滚转（Roll）和俯仰（Pitch）有本质的不同：

(1) 无绝对参考：滚转和俯仰可以通过加速度计测量重力方向来确定绝对零点。但偏航没有磁力计的情况下，无法确定绝对航向。

(2) 控制力矩较弱：偏航力矩来自对角电机的反扭矩差，而非升力差。反扭矩远小于升力产生的力矩，因此偏航控制力度较弱。

(3) 陀螺仪漂移：由于没有磁力计修正，偏航角度只能由陀螺仪积分得到，存在累积漂移。

15.7.2 偏航控制策略

本项目采用的偏航控制策略：

  • 角度环：可以省略（因为没有绝对航向参考），或使用陀螺仪积分得到的"相对航向"。

  • 角速度环：必须使用，直接控制偏航角速度。

  • 偏航增益：比Roll/Pitch小30~50%，因为偏航控制力矩较弱，过大的增益容易引起振荡。

  • 偏航D项：通常设为0或很小值，因为偏航响应本身就较慢。

参数Roll/PitchYaw原因
Kp0.03~0.10.02~0.05Yaw力矩较弱
Ki0.003~0.010.001~0.005Yaw积分不宜过大
Kd0.0002~0.0010~0.0002Yaw响应慢，D项收益小
角度环Kp4~82~4（可选）Yaw角度环可省略
输出限幅500300Yaw控制量应受限

15.7.3 代码示例15-7：偏航角速度控制器

代码15-7 偏航角速度控制器（简化版）

:::info

  • 摇杆不打偏航时，飞机自动锁定机头朝向（航向保持）；
  • 摇杆打偏航时，跟随摇杆指令自旋，同时重置航向记忆。
  • 专门处理飞行器偏航通道，摇杆回中时自动锁住机头朝向，打杆时跟随摇杆自旋；输出偏航修正力矩，送给电机混控控制飞机原地旋转。

:::

/* 代码15-7: 偏航角速度控制器 */

typedef struct {

float Kp, Ki; // 只用PI控制（无D项）

float integral;

float int_limit;

float out_limit;

float heading; // 相对航向（陀螺仪积分）

float heading_kp; // 航向保持P增益

uint8_t heading_hold; // 是否启用航向保持

} YawController;

static YawController yaw_ctrl;

void YawCtrl_Init(void) {

yaw_ctrl.Kp = 0.03f;

yaw_ctrl.Ki = 0.003f;

yaw_ctrl.integral = 0.0f;

yaw_ctrl.out_limit = 300.0f;

yaw_ctrl.int_limit = 100.0f;

yaw_ctrl.heading = 0.0f;

yaw_ctrl.heading_kp = 3.0f;

yaw_ctrl.heading_hold = 1; // 默认启用航向保持

}

float YawCtrl_Update(float target_yaw_rate, float gyro_yaw,

float dt) {

// 更新相对航向（陀螺仪积分）

yaw_ctrl.heading += gyro_yaw * dt;

// 航向归一化到 -180 ~ +180

while (yaw_ctrl.heading > 180.0f) yaw_ctrl.heading -= 360.0f;

while (yaw_ctrl.heading < -180.0f) yaw_ctrl.heading += 360.0f;

// 航向保持模式：无输入时保持当前航向

float rate_setpoint = target_yaw_rate;

if (yaw_ctrl.heading_hold && fabsf(target_yaw_rate) < 5.0f) {

// 用小P控制器维持航向

rate_setpoint = -yaw_ctrl.heading_kp * yaw_ctrl.heading;

} else {

// 有偏航输入时重置航向记录

yaw_ctrl.heading = 0.0f;

}

// PI控制器

float error = rate_setpoint - gyro_yaw;

float p_out = yaw_ctrl.Kp * error;

yaw_ctrl.integral += error * dt;

if (yaw_ctrl.integral > yaw_ctrl.int_limit)

yaw_ctrl.integral = yaw_ctrl.int_limit;

if (yaw_ctrl.integral < -yaw_ctrl.int_limit)

yaw_ctrl.integral = -yaw_ctrl.int_limit;

float i_out = yaw_ctrl.Ki * yaw_ctrl.integral;

float output = p_out + i_out;

if (output > yaw_ctrl.out_limit) output = yaw_ctrl.out_limit;

if (output < -yaw_ctrl.out_limit) output = -yaw_ctrl.out_limit;

return output;

}

void YawCtrl_Reset(void) {

yaw_ctrl.integral = 0.0f;

yaw_ctrl.heading = 0.0f;

}

提示 航向保持功能（Heading Hold）是陀螺仪积分的典型应用。当遥控器偏航杆位回中时，飞行器会维持解锁时的航向角度。由于没有磁力计修正，长时间飞行后航向可能漂移10~20度，这对于室内短航时飞行是可以接受的。

15.8 串级PID控制仿真

15.8.1 四旋翼动力学简化模型

为了在PC上验证串级PID控制器的性能，我们需要一个简单的四旋翼动力学模型。模型的核心参数：

参数符号典型值说明
转动惯量Ixx, Iyy2.5e-5 kg*m^265mm轴距微型四旋翼
偏航转动惯量Izz4.0e-5 kg*m^2约1.5倍Ixx
电机时间常数tau_m0.02 s空心杯电机响应快
推力系数kT2.5e-6 N/(RPM^2)65mm螺旋桨
力矩系数kM5e-8 N*m/(RPM^2)反扭矩系数
力臂L0.0325 m轴距65mm的一半
最大电机转速RPM_max300003.7V供电

动力学方程：

角加速度 = 力矩 / 转动惯量

力矩 = (电机1推力 - 电机3推力) * L  （以Roll为例）

推力 = kT * RPM^2

d(RPM)/dt = (目标RPM - 当前RPM) / tau_m

角速度 += 角加速度 * dt

角度 += 角速度 * dt

15.8.2 代码示例15-8：完整仿真程序

以下代码实现了一个完整的串级PID控制仿真环境，包含四旋翼动力学模型和控制算法。可以在PC上编译运行，验证控制参数的合理性。

代码15-8 串级PID仿真程序

/* 代码15-8: 串级PID控制仿真 (PC端运行) */

:::info
这是一套完整四旋翼飞行器数字仿真平台（PC 端纯仿真，不用于单片机烧录） 用来在 VS Code 电脑上模拟真实小四轴闭环飞行，验证你前面整套串级姿态控制算法（角度外环 P + 角速度内环 PID + X 型混控）。

:::

#include <stdio.h>
#include <math.h>
#include <string.h>

/* === 简化的PID结构 === */
typedef struct { float Kp, Ki, Kd, integral, prev, d_filt, d_alpha; } SimPID;

void SimPID_Init(SimPID *p, float kp, float ki, float kd, float dt, float fc) {
    p->Kp=kp; p->Ki=ki; p->Kd=kd;
    p->integral=0; p->prev=0; p->d_filt=0;
    float tau=1.0f/(6.2832f*fc); p->d_alpha=dt/(tau+dt);
}

float SimPID_Update(SimPID *p, float sp, float meas, float dt) {
    float e = sp - meas;
    p->integral += e * dt;
    if (p->integral > 500) p->integral = 500;
    if (p->integral < -500) p->integral = -500;
    float raw_d = -(meas - p->prev)/dt; p->prev = meas;
    p->d_filt = p->d_alpha*raw_d + (1-p->d_alpha)*p->d_filt;
    return p->Kp*e + p->Ki*p->integral + p->Kd*p->d_filt;
}

/* === 四旋翼动力学模型 === */
typedef struct {
    float angle[3];      // Roll, Pitch, Yaw (度)
    float rate[3];       // 角速度 (度/秒)
    float motor_rpm[4];  // 各电机转速
    float inertia[3];    // 转动惯量 (Roll, Pitch, Yaw)
    float arm_length;    // 力臂
    float motor_tau;     // 电机时间常数
    float thrust_coeff;  // 推力系数
    float max_rpm;
    float air_damping;   // 空气阻尼
} QuadModel;

void Quad_Init(QuadModel *q) {
    memset(q, 0, sizeof(*q));
    q->inertia[0] = 2.5e-5f;  // Roll
    q->inertia[1] = 2.5e-5f;  // Pitch
    q->inertia[2] = 4.0e-5f;  // Yaw
    q->arm_length = 0.0325f;
    q->motor_tau = 0.02f;
    q->thrust_coeff = 2.5e-6f;
    q->max_rpm = 30000.0f;
    q->air_damping = 0.01f;
}

void Quad_Update(QuadModel *q, float motor_cmd[4], float dt) {
    // 电机响应（一阶滞后）
    for (int i=0; i<4; i++) {
        float target = motor_cmd[i]/1000.0f * q->max_rpm;
        q->motor_rpm[i] += (target - q->motor_rpm[i])
                         / q->motor_tau * dt;
    }
    // 计算各电机推力
    float thrust[4];
    for (int i=0; i<4; i++) {
        thrust[i] = q->thrust_coeff * q->motor_rpm[i]*q->motor_rpm[i];
    }
    // Roll力矩 = (M2+M4 - M1-M3) * L （简化）
    float torque_roll = (thrust[1]+thrust[3]-thrust[0]-thrust[2])
                      * q->arm_length;
    // Pitch力矩 = (M1+M2 - M3-M4) * L
    float torque_pitch = (thrust[0]+thrust[1]-thrust[2]-thrust[3])
                       * q->arm_length;
    // Yaw力矩（反扭矩差，简化处理）
    float torque_yaw = (thrust[1]+thrust[3]-thrust[0]-thrust[2])
                     * 0.01f; // 反扭矩系数远小于升力
    
    float torques[3] = {torque_roll, torque_pitch, torque_yaw};
    // 角动力学积分
    for (int i=0; i<3; i++) {
        float accel = torques[i] / q->inertia[i];
        q->rate[i] += accel * dt;
        q->rate[i] *= (1.0f - q->air_damping * dt);
        q->angle[i] += q->rate[i] * dt;
    }
}

/* === 主仿真循环 === */
int main(void) {
    QuadModel quad;
    SimPID rate_roll, rate_pitch, rate_yaw;
    float angle_kp = 5.0f;
    float dt = 0.001f;  // 1ms
    
    Quad_Init(&quad);
    SimPID_Init(&rate_roll,  0.05f, 0.005f, 0.0005f, dt, 80.0f);
    SimPID_Init(&rate_pitch, 0.05f, 0.005f, 0.0005f, dt, 80.0f);
    SimPID_Init(&rate_yaw,   0.03f, 0.002f, 0.0f,    dt, 50.0f);
    
    float target_roll = 15.0f;  // 阶跃: 目标15度
    float motor_cmd[4] = {500,500,500,500}; // 悬停
    
    printf("t_ms, roll_angle, roll_rate, pitch_angle, m0, m1\n");
    
    for (int step=0; step<3000; step++) {
        float t = step * dt;
        
        // 外环（每5步=5ms执行一次）
        float target_rate_roll = 0, target_rate_pitch = 0;
        if (step % 5 == 0) {
            target_rate_roll = angle_kp * (target_roll - quad.angle[0]);
            if (target_rate_roll >  300) target_rate_roll =  300;
            if (target_rate_roll < -300) target_rate_roll = -300;
            target_rate_pitch = angle_kp * (0 - quad.angle[1]);
        }
        
        // 内环
        float roll_corr = SimPID_Update(&rate_roll,
            target_rate_roll, quad.rate[0], dt);
        float pitch_corr = SimPID_Update(&rate_pitch,
            target_rate_pitch, quad.rate[1], dt);
        float yaw_corr = SimPID_Update(&rate_yaw,
            0, quad.rate[2], dt);
        
        // 混控
        motor_cmd[0] = 500+pitch_corr-roll_corr-yaw_corr;
        motor_cmd[1] = 500+pitch_corr+roll_corr+yaw_corr;
        motor_cmd[2] = 500-pitch_corr-roll_corr+yaw_corr;
        motor_cmd[3] = 500-pitch_corr+roll_corr-yaw_corr;
        for (int i=0; i<4; i++) {
            if (motor_cmd[i]>1000) motor_cmd[i]=1000;
            if (motor_cmd[i]<0) motor_cmd[i]=0;
        }
        
        // 动力学仿真
        Quad_Update(&quad, motor_cmd, dt);
        
        // 输出
        if (step % 10 == 0) {
            printf("%d,%.3f,%.2f,%.3f,%.0f,%.0f\n",
                step, quad.angle[0], quad.rate[0],
                quad.angle[1], motor_cmd[0], motor_cmd[1]);
        }
    }
    printf("\n=== 仿真完成 ===\n");
    printf("最终Roll角度: %.3f (目标: %.1f)\n",
           quad.angle[0], target_roll);
    printf("稳态误差: %.3f\n",
           target_roll - quad.angle[0]);
    return 0;
}

15.8.3 仿真结果分析

运行上述仿真程序后，将输出数据导入绘图工具（如Python matplotlib或Excel），绘制角度和角速度随时间的变化曲线。

良好调参的串级PID系统，阶跃响应应表现出以下特征：

指标期望值含义
上升时间50~150ms飞行器快速响应姿态变化
超调量5~15%轻微超调是可接受的，过大说明D项不足
调节时间200~500ms飞行器在0.5秒内稳定到目标角度
稳态误差< 0.5度积分项应消除大部分稳态误差
振荡次数0~1次不应出现持续振荡

如果仿真结果不理想，按以下步骤调参：

(1) 首先将Ki和Kd设为0，只调节Kp使系统能响应但略有振荡。

(2) 逐渐增大Kd以抑制振荡，观察超调量下降。

(3) 加入少量Ki消除稳态误差，注意不要过大以免引起慢速振荡。

(4) 最后微调三个参数达到最佳平衡。

🧪**** 实验 修改代码15-8中的目标角度（从5度到45度），观察不同幅值阶跃下的响应差异。大角度阶跃时，电机可能饱和（达到最大转速），此时响应变慢——这是执行器限幅导致的非线性效应，在实际飞行中需要避免过大的目标角度。

提示 仿真中使用的模型参数（转动惯量、推力系数等）可以通过实际测量或厂家数据获得更准确的值。模型越准确，仿真结果越接近真实飞行表现。但对于初步验证PID参数，简化模型已经足够。

至此，第14章和第15章完成了从PID基础理论到串级PID姿态控制的完整讲解。下一章将讨论传感器校准与飞行调试的具体操作方法。

学习资源

🎬 B站系列视频教程：手把手教你从零组装无人机

📦 百度网盘资料包（SDK、教材PDF、完整源码）：点击下载 提取码: JZS8

本系列持续更新，欢迎收藏关注。如有问题欢迎在评论区交流。

# ide # c语言 # 单片机 # 开发语言
Logo
天工造物局

北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台

更多推荐

graphify 视频音视频转写全流程解析：从 detect 结果到知识图谱文档的 Step 2.5

graphify 的核心能力是把任意语料（代码、文档、PDF、图片）转化为可查询的知识图谱，而视频与音频文件无法被直接阅读，必须先把它们转写成文本。本文围绕 `graphify` 运行手册中的转写参考文档 [transcribe.md](https://link.gitcode.com/i/d3285781588e0f1b84886dfdbc4fe7f6)，完整讲解 "Step 2.5 - Tra

avatar 天工造物局

ML-For-Beginners 终章：经典机器学习在金融、医疗、零售等八大行业中的真实落地场景

本篇技术指南基于开源课程仓库 ML-For-Beginners 的收尾课程《Postscript: Machine learning in the real world》（对应仓库目录 [9-Real-World/1-Applications](https://link.gitcode.com/i/6b23002df80861c20abf34d7db63ef41)）编写。该课此前 11 周的教学带

avatar 天工造物局

graphify 知识图谱查询实战：query / path / explain 与可审计的答案闭环

本指南以 graphify 的 Trae 版查询技能参考（[graphify/skills/trae/references/query.md](https://link.gitcode.com/i/72a7875d46ec6d3706d31791f3cdcbe0)）为主体，系统讲解如何针对**已构建好的知识图谱**执行三类查询：语义相关节点遍历（query）、两概念间最短路径（path）、单节点关

avatar 天工造物局