ArduPilot plane 俯仰姿态稳定器源码逐行解析：从期望角度到升降舵 PWM_角度环角速度环
ArduPilot plane 俯仰姿态稳定器源码逐行解析：从期望角度到升降舵 PWM
作者：嵌入式 | 2025-08-22
本文基于 ArduPilot master 分支（commit 近似 2025-08-22），带你把 Plane 俯仰通道的所有代码“串”成一条线。
建议配合 VSCode 或 Sourcetrail 阅读，效果更佳。
一、为什么要写这篇博客
- Plane 的俯仰控制器横跨
Mode::run()→Plane::stabilize_pitch()→AP_PitchController，文件多、宏多、情况多。 - 官方 Wiki 只讲参数，不讲实现；源码注释又太“干”。
- 本文把所有相关代码一次性贴出，按执行顺序加上中文注释，方便二次开发、调参和故障排查。
二、整体调用链
Mode::run()└── stabilize_stick_mixing_fbw() （可选：FBW 杆量混合）└── stabilize_roll()└── stabilize_pitch() ← 本文主角├── takeoff_tail_hold() （尾坐起降锁尾）└── stabilize_pitch_get_pitch_out()└── AP_PitchController::get_servo_out()└── _get_rate_out()
三、完整源码 + 逐段中文讲解
以下代码为完整函数，已按出现顺序排放。
讲解以【注：xxx】形式插在代码行间，方便复制进 IDE。
- 飞行模式调度器
Mode::run()
// Mode.cpp
void Mode::run()
{/* 【注】stick_mixing 参数：0: Disabled1: FBW (FlyByWire) —— 用杆量修正姿态目标2: DIRECT_REMOVED（历史遗留，等价 FBW）旧版本曾经支持 DIRECT，现已删除。*/if ((plane.g.stick_mixing == StickMixing::FBW) ||(plane.g.stick_mixing == StickMixing::DIRECT_REMOVED)){plane.stabilize_stick_mixing_fbw(); // 【注】把滚转/俯仰杆量叠加到 nav_roll/nav_pitch}/* 【注】三大姿态环依次执行 */plane.stabilize_roll();plane.stabilize_pitch(); // 下文重点plane.stabilize_yaw();
}
- 俯仰主函数
Plane::stabilize_pitch()
// Plane.cpp
void Plane::stabilize_pitch()
{/* -------- 2.1 尾坐起降起飞锁尾 -------- */int8_t force_elevator = takeoff_tail_hold(); // 【注】尾坐机在起飞阶段固定升降舵if (force_elevator != 0) {// 百分比 -> centidegrees (-4500..4500)SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45 * force_elevator);return; // 【注】直接退出，不再走闭环}/* -------- 2.2 正常闭环 -------- */const float pitch_out = stabilize_pitch_get_pitch_out();SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
}
- 计算期望舵面角
stabilize_pitch_get_pitch_out()
// Plane.cpp
float Plane::stabilize_pitch_get_pitch_out()
{/* 【注】speed_scaler = 标定空速 / 当前空速，用于增益随速调参 */const float speed_scaler = get_speed_scaler();#if HAL_QUADPLANE_ENABLED/* ---- 3.1 VTOL 控制器接管 ---- */if (!quadplane.use_fw_attitude_controllers()) {const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();/* 【注】把 VTOL 角度环 P 缩放 FW 的 FeedForward */if (quadplane.option_is_set(QuadPlane::OPTION::SCALE_FF_ANGLE_P)) {const float mc_angP =quadplane.attitude_control->get_angle_pitch_p().kP() *quadplane.attitude_control->get_last_angle_P_scale().y;if (is_positive(mc_angP)) {pitchController.set_ff_scale(MIN(1.0f, 1.0f / (mc_angP * pitchController.tau())));}}const int32_t pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler);pitchController.decay_I(); // 【注】防止 FW 与 VTOL 积分对顶return pitch_out;}
#endif // HAL_QUADPLANE_ENABLED/* ---- 3.2 计算期望俯仰角 demanded_pitch ---- */
#if HAL_QUADPLANE_ENABLEDconst bool quadplane_in_frwd_transition = quadplane.in_frwd_transition();
#elseconst bool quadplane_in_frwd_transition = false;
#endifint32_t demanded_pitch =nav_pitch_cd // 【注】导航给出的目标俯仰角+ int32_t(g.pitch_trim * 100.0f) // 【注】机体安装角+ SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; // 【注】油门→俯仰前馈bool disable_integrator = false;if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) {disable_integrator = true; // 【注】手杆介入时冻结积分}/* ---- 3.3 着陆拉飘(flare)逻辑 ---- */if (!quadplane_in_frwd_transition &&!control_mode->is_vtol_mode() &&!control_mode->does_auto_throttle() &&flare_mode == FlareMode::ENABLED_PITCH_TARGET &&throttle_at_zero()){demanded_pitch = landing.get_pitch_cd(); // 【注】固定拉飘角度}/* ---- 3.4 角度误差 -> 角速度 ---- */return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, // 【注】角度误差 centidegreesspeed_scaler,disable_integrator,ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)));
}
- 俯仰角 → 角速度
AP_PitchController::get_servo_out()
// AP_PitchController.cpp
float AP_PitchController::get_servo_out(int32_t angle_err,float scaler,bool disable_integrator,bool ground_mode)
{/* ---- 4.1 基本检查 ---- */if (gains.tau < 0.05f) {gains.tau.set(0.05f); // 【注】防 tau 过小}const float aspeed = get_airspeed();bool inverted = false;const float rate_offset = _get_coordination_rate_offset(aspeed, inverted); // 【注】协调转弯补偿/* ---- 4.2 期望角速度 desired_rate ---- */angle_err_deg = angle_err * 0.01f; // centidegrees -> degreesfloat desired_rate = angle_err_deg / gains.tau; // 【注】角度误差 / tau -> deg/s/* ---- 4.3 角速度限幅 ---- */if (!inverted) {desired_rate += rate_offset;if (gains.rmax_neg && desired_rate < -gains.rmax_neg)desired_rate = -gains.rmax_neg;else if (gains.rmax_pos && desired_rate > gains.rmax_pos)desired_rate = gains.rmax_pos;} else {desired_rate = -desired_rate + rate_offset; // 【注】倒飞时反向}/* ---- 4.4 大坡度减载 ---- */const AP_AHRS &_ahrs = AP::ahrs();float roll_wrapped = labs(_ahrs.roll_sensor);if (roll_wrapped > 9000) roll_wrapped = 18000 - roll_wrapped;const float roll_limit_margin = MIN(aparm.roll_limit * 100 + 500.0f, 8500.0f);if (roll_wrapped > roll_limit_margin && labs(_ahrs.pitch_sensor) < 7000) {float roll_prop = (roll_wrapped - roll_limit_margin) / (9000.0f - roll_limit_margin);desired_rate *= (1.0f - roll_prop); // 【注】>roll_limit 时线性削弱}/* ---- 4.5 进入角速度环 ---- */return _get_rate_out(desired_rate, scaler, disable_integrator, aspeed, ground_mode);
}
- 角速度 PID
_get_rate_out()
// AP_FW_Controller.cpp
float AP_FW_Controller::_get_rate_out(float desired_rate,float scaler,bool disable_integrator,float aspeed,bool ground_mode)
{const float dt = AP::scheduler().get_loop_period_s();const float eas2tas = AP::ahrs().get_EAS2TAS();bool limit_I = fabsf(_last_out) >= 45.0f; // 【注】舵面饱和时锁积分const float rate = get_measured_rate(); // 【注】实际角速度 deg/sconst float old_I = rate_pid.get_i();/* ---- 5.1 失速保护 ---- */const bool underspeed = is_underspeed(aspeed);if (underspeed) limit_I = true;/* ---- 5.2 PID 计算（rad 内部单位）---- */rate_pid.update_all(radians(desired_rate) * scaler * scaler,rate * scaler * scaler,dt,limit_I);if (underspeed) {rate_pid.set_integrator(old_I); // 【注】失速时积分冻结}/* ---- 5.3 FeedForward 处理 ---- */const float ff = degrees(ff_scale * rate_pid.get_ff() / (scaler * eas2tas));ff_scale = 1.0f;if (disable_integrator) {rate_pid.reset_I();}/* ---- 5.4 日志结构体转换 ---- */_pid_info = rate_pid.get_pid_info();auto &pinfo = _pid_info;const float deg_scale = degrees(1);pinfo.FF = ff;pinfo.P *= deg_scale;pinfo.I *= deg_scale;pinfo.D *= deg_scale;pinfo.DFF *= deg_scale;pinfo.target = desired_rate;pinfo.actual = rate; // 【注】日志中显示真实 deg/s/* ---- 5.5 输出和限幅 ---- */float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;if (ground_mode) {/* 【注】地面滑行抑制 D 和部分 P，防止抖动 */out -= pinfo.D + 0.5f * pinfo.P;}_last_out = out;/* ---- 5.6 自动调参 ---- */if (autotune != nullptr && autotune->running && aspeed > aparm.airspeed_min) {autotune->update(pinfo, scaler, angle_err_deg);}return constrain_float(out * 100.0f, -4500.0f, 4500.0f); // 【注】centidegrees
}
四、一张图总结
（建议保存到本地查看）
┌-------- 上层导航 --------┐
│ nav_pitch_cd │
└----------┬---------------┘│ + pitch_trim│ + kff_throttle_to_pitch▼demanded_pitch ──┬──► flare? → landing pitch│└──► 角度误差 = demanded - ahrs.pitch_sensor▼AP_PitchController::get_servo_out()▼desired_rate = angle_err / tau▼_get_rate_out()┌----------------------------------------┐│ PID(FF+P+I+D+DFF) ││ • scaler² 修正增益 ││ • eas2tas 修正真速 ││ • ground_mode 抑制 D/P ││ • underspeed 冻结积分 │└----------------------------------------┘▼centidegrees → SRV_Channels → PWM → 升降舵
五、如何调试
- 打开
LOG_BITMASK的 PID 日志，关注PIDR,PIDD,PIDP消息。 PTCH2SRV_RMAX_UP/DN限制最大俯仰角速度，防止高速俯冲。LAND_PITCH_CD调拉飘角度，TKOFF_TDRAG_ELEV调尾坐锁尾角度。- 在 STABILIZE 模式下大幅度拉杆，观察
PIQP.I是否被冻结。 - 使用
AUTOTUNE模式让飞控自己整定PTCH2SRV_*参数。
六、结语
俯仰控制器是固定翼飞控最复杂的单轴之一：既要跟踪导航指令，又要兼顾失速保护、协调转弯、着陆拉飘、手杆混合、VTOL 切换……
希望本文的“代码+注释”方式能让你在最短时间内看懂整个链路。