ArduPilot飞控系统架构分析
基于对ArduCopter代码的分析，我将为您详细解释ardupilot飞控系统的架构和工作原理。
1. 系统总体架构
ArduPilot是一个复杂而强大的开源无人机飞控系统，主要由以下几个部分组成：
1.1 核心组件
- 主循环(Copter.cpp): 系统入口点，负责初始化和调度各个任务
- 调度器(Scheduler): 管理不同频率的任务执行
- 飞行模式系统(Mode): 实现不同的飞行控制逻辑
- 姿态控制系统(AC_AttitudeControl): 负责稳定飞行器姿态
- 位置控制系统(AC_PosControl): 负责控制飞行器位置
- 导航系统(AC_WPNav): 负责航点导航和路径规划
- 电机输出系统(AP_Motors): 负责控制电机输出
1.2 传感器系统
- IMU(惯性测量单元): 提供加速度和角速率数据
- GPS: 提供位置、速度和时间信息
- 气压计: 提供高度信息
- 罗盘: 提供航向信息
- 光流传感器: 提供相对位移信息
- 测距仪: 提供对地高度信息
1.3 通信系统
- MAVLink协议: 与地面站通信的标准协议
- GCS(地面控制站)接口: 处理与地面站的通信
2. 主程序流程
Copter.cpp是整个系统的入口点，定义了主要的调度任务和执行流程：
2.1 初始化流程
- 硬件初始化
- 传感器初始化
- 控制器初始化
- 飞行模式初始化
- 参数加载
2.2 主循环任务
系统使用调度器(scheduler_tasks数组)来管理不同频率的任务：
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
// 高优先级任务（快速循环）
FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update), // 更新IMU数据
FAST_TASK(run_rate_controller_main), // 运行姿态控制器
FAST_TASK(motors_output_main), // 输出到电机
// 中低优先级任务
SCHED_TASK(rc_loop, 250, 130, 3), // 250Hz读取遥控器输入
SCHED_TASK(throttle_loop, 50, 75, 6), // 50Hz处理油门
// ...其他任务
}
这些任务按优先级排序，每个任务都有指定的执行频率和预期执行时间。
3. 飞行模式系统
ArduCopter支持多种飞行模式，每种模式都有不同的控制逻辑和行为：
3.1 模式基类
所有飞行模式都继承自Mode基类，该基类定义了飞行模式的通用接口：
class Mode {
public:
// 返回模式编号
virtual Number mode_number() const = 0;
// 初始化模式
virtual bool init(bool ignore_checks);
// 运行模式的主逻辑
virtual void run() = 0;
// 模式特性查询
virtual bool requires_GPS() const = 0;
virtual bool has_manual_throttle() const = 0;
// ...其他特性查询方法
};
3.2 主要飞行模式
系统支持的主要飞行模式包括：
- STABILIZE: 基本的姿态稳定模式
- ALTHOLD: 高度保持模式
- LOITER: 位置保持模式
- AUTO: 自动执行任务模式
- RTL: 自动返航模式
- LAND: 自动降落模式
- GUIDED: 引导模式，接受外部控制命令
- ACRO: 特技模式，提供更直接的角速率控制
4. 控制系统架构
ArduCopter的控制系统采用了层级控制架构：
4.1 位置控制器(AC_PosControl)
位置控制器负责将目标位置转换为目标速度和加速度：
- 输入: 目标位置、速度和加速度
- 输出: 目标姿态(倾角)
- 主要功能:
- 位置PID控制
- 速度PID控制
- 加速度控制
- 运动学路径规划
4.2 姿态控制器(AC_AttitudeControl)
姿态控制器负责将目标姿态转换为电机输出：
- 输入: 目标姿态(四元数或欧拉角)和角速率
- 输出: 电机控制信号
- 主要功能:
- 姿态P控制
- 角速率PID控制
- 角加速度限制
- 电机混控
4.3 控制流程
一个典型的控制流程如下：
- 位置控制器计算目标加速度
- 位置控制器将目标加速度转换为目标倾角
- 姿态控制器计算达到目标倾角所需的角速率
- 姿态控制器计算达到目标角速率所需的电机输出
- 电机输出被发送到ESC，控制电机转速
5. 关键算法
5.1 姿态估计
系统使用扩展卡尔曼滤波器(EKF)融合多个传感器数据，估计飞行器的姿态和位置：
- 加速度计和陀螺仪数据提供短期姿态估计
- GPS和气压计数据提供长期位置和高度修正
- 罗盘数据提供航向修正
5.2 PID控制
系统使用多层PID控制器：
- 位置控制使用P控制器
- 速度控制使用PI或PID控制器
- 角速率控制使用PID控制器
5.3 运动学路径规划
系统使用加加速度限制(jerk-limited)路径规划算法，生成平滑的运动轨迹：
void AC_PosControl::shape_pos_vel_accel(...)
这个函数计算从当前状态到目标状态的平滑轨迹，考虑速度、加速度和加加速度限制。
6. 安全机制
ArduCopter实现了多种安全机制：
- 失控保护: 当遥控信号丢失时，可以自动执行返航、降落等操作
- 电池监控: 当电池电量低时，可以触发警告或自动返航
- 地理围栏: 限制飞行器的活动范围
- EKF监控: 当状态估计不可靠时，可以触发失效保护
- 碰撞避免: 支持ADSB和其他避障系统
7. 参数系统
ArduCopter使用参数系统来存储和管理配置：
// 参数实例
Parameters g;
ParametersG2 g2;
参数可以通过地面站修改，并存储在非易失性存储器中。
8. 执行流程示例
以自动模式(AUTO)为例，执行流程如下：
- 系统初始化并进入AUTO模式
ModeAuto::run()方法被调度器定期调用- 根据当前任务类型(起飞、航点、降落等)，调用相应的处理函数
- 处理函数计算目标位置和速度
- 位置控制器根据目标生成姿态命令
- 姿态控制器根据姿态命令计算电机输出
- 电机输出被发送到ESC，控制电机转速
9. 总结
ArduPilot飞控系统是一个复杂而强大的系统，通过模块化设计和分层架构，实现了灵活的飞行控制功能。主要特点包括：
- 模块化设计: 各个功能模块相对独立，便于维护和扩展
- 多种飞行模式: 满足不同的飞行需求
- 强大的控制算法: 实现稳定的姿态和位置控制
- 完善的安全机制: 保障飞行安全
- 丰富的配置选项: 通过参数系统实现高度可定制性
这种架构使ArduPilot能够适应从简单的多旋翼到复杂的自主飞行器等各种应用场景。
起飞控制系统详细解读：
- 类结构分析：
- Mode基类 (mode.h/mode.cpp)
|- _TakeOff嵌套类 (处理手动起飞)
|- _AutoTakeoff嵌套类 (处理自动起飞)
- 关键成员变量：
-
_TakeOff类：
- _running: 标记起飞是否进行中
- take_off_start_alt: 起飞开始高度
- take_off_complete_alt: 起飞完成高度
-
_AutoTakeoff类：
- complete: 标记起飞是否完成
- no_nav_active: 导航控制是否激活
- complete_alt_cm: 完成高度(cm)
- terrain_alt: 是否使用地形高度
- 核心函数实现：
_TakeOff类关键函数：
-
start(): 初始化起飞参数
void _TakeOff::start(float alt_cm) { _running = true; take_off_start_alt = pos_control->get_pos_target_z_cm(); take_off_complete_alt = take_off_start_alt + alt_cm; } -
run(): 执行起飞控制
void _TakeOff::run() { // 计算目标爬升率 float climb_rate_cms = get_pilot_desired_climb_rate(); climb_rate_cms = constrain_float(climb_rate_cms, 0.0f, takeoff_climb_rate); // 更新位置控制器 pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms); // 检查是否达到目标高度 if (pos_control->get_pos_target_z_cm() >= take_off_complete_alt) { stop(); } }
_AutoTakeoff类关键函数：
-
start(): 初始化自动起飞
void _AutoTakeoff::start(float complete_alt_cm, bool terrain_alt) { this->complete_alt_cm = complete_alt_cm; this->terrain_alt = terrain_alt; complete = false; no_nav_active = true; no_nav_alt_cm = 1000.0f; // 1m } -
run(): 自动起飞状态机
void _AutoTakeoff::run() { if (complete) return; // 阶段1：垂直上升 if (no_nav_active && pos_control->get_pos_target_z_cm() < no_nav_alt_cm) { pos_control->set_pos_target_z_from_climb_rate_cm(takeoff_climb_rate); } // 阶段2：导航控制 else { no_nav_active = false; // 更新目标位置 Vector3f target_pos = complete_pos; if (terrain_alt) { target_pos.z = complete_alt_cm; } pos_control->input_pos_xyz(target_pos); // 检查完成条件 if (pos_control->reached_target_xy() && pos_control->reached_target_z()) { complete = true; } } }
- 与飞行控制系统的交互：
-
位置控制：通过pos_control接口
pos_control->set_pos_target_z_from_climb_rate_cm() pos_control->input_pos_xyz() -
姿态控制：通过attitude_control接口
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw() -
电机控制：通过motors接口
motors->set_desired_spool_state()
- 安全机制：
-
起飞前检查：
bool _TakeOff::triggered() const { if (!motors->armed()) return false; if (ap.land_complete) return false; if (motors->get_spool_state() != THROTTLE_UNLIMITED) return false; return true; } -
高度保护：
float climb_rate = constrain_float(climb_rate, 0.0f, max_takeoff_climb_rate);
- 参数配置：
- 最大爬升率：takeoff_climb_rate (cm/s)
- 最小起飞高度：takeoff_alt_min (cm)
- 地形高度开关：terrain_alt (bool)
这个详细解读展示了起飞控制系统的完整实现细节，包括核心算法、状态转换和系统交互。
input_vel_accel_NE_cm函数详细解读：
- 函数原型：
void AC_PosControl::input_vel_accel_NE_cm(const Vector2f &vel, const Vector2f &accel)
- 参数说明：
- vel: 东北方向的目标速度向量 (cm/s)
- accel: 东北方向的目标加速度向量 (cm/s²)
-
函数作用：
将东北平面(水平面)的目标速度和加速度输入到位置控制器，用于计算所需的推力向量 -
实现细节：
void AC_PosControl::input_vel_accel_NE_cm(const Vector2f &vel, const Vector2f &accel) {
// 转换到局部坐标系
Vector2f vel_NE = vel;
Vector2f accel_NE = accel;
// 更新目标速度和加速度
_vel_target_NE.x = vel_NE.x;
_vel_target_NE.y = vel_NE.y;
_accel_target_NE.x = accel_NE.x;
_accel_target_NE.y = accel_NE.y;
// 重置位置目标
_pos_target_NE.x = _inav.get_position_xy_cm().x;
_pos_target_NE.y = _inav.get_position_xy_cm().y;
// 标记已更新
_flags.reset_pos_target = true;
_flags.reset_vel_target = true;
_flags.reset_accel_target = true;
}
-
调用链分析：
input_vel_accel_NE_cm()
├── _inav.get_position_xy_cm() // 获取当前位置
├── _pid_vel_xy.set_input_filter_all() // 更新PID滤波器
└── _pid_vel_xy.set_desired_rate() // 设置目标速率 -
关键子函数解读：
(1) _inav.get_position_xy_cm()
- 作用：从惯性导航系统获取东北方向的当前位置(cm)
- 实现：
Vector2f AP_InertialNav::get_position_xy_cm() const {
return Vector2f(_position.x, _position.y);
}
(2) _pid_vel_xy.set_input_filter_all()
- 作用：设置PID控制器的输入滤波器
- 参数：滤波时间常数
- 实现：
void AC_PID::set_input_filter_all(float time_const) {
_filter.set_cutoff_frequency(1.0f / (2.0f * M_PI * time_const));
}
(3) _pid_vel_xy.set_desired_rate()
- 作用：设置PID控制器的目标速率
- 实现：
void AC_PID::set_desired_rate(float desired_rate) {
_desired_rate = desired_rate;
_dt = 1.0f / _freq_hz;
}
- 控制逻辑分析：
- 采用串级PID控制结构：
外环：位置控制 → 生成速度目标
内环：速度控制 → 生成加速度目标 - 前馈控制：直接输入加速度目标提高响应速度
- 坐标系转换：将全局NE坐标转换为机体坐标系
- 数学原理：
实现了一个二阶控制系统：
r(t) → [位置PID] → v_d(t) → [速度PID] → a_d(t) → 系统
反馈 ← [位置估计] ← [速度估计] ←
其中：
- r(t): 位置参考输入
- v_d(t): 期望速度
- a_d(t): 期望加速度
- 性能优化：
- 使用前馈补偿减少相位滞后
- 动态调整PID参数适应不同飞行状态
- 采用非线性滤波器平滑指令
- 使用场景：
- 精确位置控制模式
- 速度控制模式
- 自动起飞/降落过程
- 航点导航控制
- /**
-
@brief 根据给定的推力向量、航向角和航向速率来控制飞行器的姿态
-
此函数将输入的推力向量、航向角（单位：厘度）和航向速率（单位：厘度每秒）转换为目标姿态，
-
根据是否启用体轴角速度前馈控制，采用不同的方式计算目标角速度，最终调用四元数姿态控制器进行姿态控制。
-
@param thrust_vector 表示期望的推力向量
-
@param heading_angle_cd 期望的航向角，单位为厘度
-
@param heading_rate_cds 期望的航向速率，单位为厘度每秒
*/
void AC_AttitudeControl::input_thrust_vector_heading_cd(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds)
{
// 获取最大偏航角速度限制，若为 0 则表示该限制未启用
// a zero _angle_vel_yaw_max means that setting is disabled
const float slew_yaw_max_rads = get_slew_yaw_max_rads();// 将公共接口传入的厘度单位转换为弧度
// 对航向速率进行限制，确保其在最大偏航角速度限制范围内
const float heading_rate_rads = constrain_float(cd_to_rad(heading_rate_cds), -slew_yaw_max_rads, slew_yaw_max_rads);
const float heading_angle_rad = cd_to_rad(heading_angle_cd);// 更新姿态目标
update_attitude_target();// 计算姿态目标的欧拉角
_attitude_target.to_euler(_euler_angle_target_rad);// 将推力向量和航向角转换为四元数表示的期望姿态
const Quaternion desired_attitude_quat = attitude_from_thrust_vector(thrust_vector, heading_angle_rad);if (_rate_bf_ff_enabled) {
// 计算横滚和俯仰方向的角度误差
// calculate the angle error in x and y.
Vector3f attitude_error;
float thrust_vector_diff_angle;
Quaternion thrust_vec_correction_quat;
float returned_thrust_vector_angle;
thrust_vector_rotation_angles(desired_attitude_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle);// 当偏航加速度限制启用时，偏航输入整形器会限制偏航轴的角加速度，使输出速率平滑趋近于输入速率 // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing // the output rate towards the input rate. _ang_vel_target_rads.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target_rads.x, _dt); _ang_vel_target_rads.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target_rads.y, _dt); _ang_vel_target_rads.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target_rads.z, heading_rate_rads, slew_yaw_max_rads, _dt); // 限制角速度，确保其不超过设定的最大横滚、俯仰和偏航角速度 ang_vel_limit(_ang_vel_target_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), slew_yaw_max_rads);} else {
// 若未启用体轴角速度前馈控制，直接将期望姿态设置为目标姿态
// set persisted quaternion target attitude
_attitude_target = desired_attitude_quat;// 将速率前馈请求置零 _euler_rate_target_rads.zero(); _ang_vel_target_rads.zero();}
// 将体轴角速度转换为期望姿态的欧拉角导数
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target_rads, _euler_rate_target_rads);// 调用四元数姿态控制器进行姿态控制
attitude_controller_run_quat();
}
详细解读 input_thrust_vector_heading_cd 函数及其子函数：
- input_thrust_vector_heading_cd 函数：
- 功能：根据给定的推力向量和航向角设置姿态目标
- 参数：
- thrust_vector: 期望的推力向量(在机体坐标系中)
- heading_angle_cd: 航向角(单位：厘度)
- heading_rate_cds: 航向角速率(单位：厘度/秒)
- 实现逻辑：
- 将输入的单位从厘度转换为弧度
- 更新姿态目标
- 计算当前姿态目标的欧拉角
- 使用 attitude_from_thrust_vector() 将推力向量和航向角转换为四元数姿态
- 根据是否启用前馈控制(_rate_bf_ff_enabled)选择不同的控制策略
- 调用 attitude_controller_run_quat() 执行姿态控制
- attitude_from_thrust_vector() 子函数：
- 功能：根据推力向量和航向角计算期望的四元数姿态
- 参数：
- thrust_vector: 推力向量
- heading_angle_rad: 航向角(弧度)
- 实现逻辑：
- 标准化推力向量
- 计算当前推力向量与目标推力向量之间的旋转轴和角度
- 创建表示推力向量旋转的四元数
- 创建表示航向旋转的四元数
- 返回组合后的四元数姿态
- thrust_vector_rotation_angles() 子函数：
- 功能：计算从当前姿态到目标姿态的旋转角度
- 参数：
- attitude_target: 目标姿态四元数
- attitude_body: 当前姿态四元数
- 输出参数：
- thrust_vector_correction: 推力向量修正四元数
- attitude_error: 姿态误差(弧度)
- thrust_angle: 当前推力角度
- thrust_error_angle: 推力误差角度
- 实现逻辑：
- 计算当前和目标推力向量
- 计算旋转轴和角度
- 计算姿态误差
- input_shaping_angle() 子函数：
- 功能：根据角度误差计算角速度命令，带有加速度限制
- 参数：
- error_angle: 角度误差
- input_tc: 时间常数
- accel_max: 最大加速度
- 其他控制参数
- 实现逻辑：
- 使用平方根控制器计算期望角速度
- 应用加速度限制
- input_shaping_ang_vel() 子函数：
- 功能：对角速度命令进行平滑处理，带有加速度限制
- 参数：
- target_ang_vel: 当前目标角速度
- desired_ang_vel: 期望角速度
- accel_max: 最大加速度
- dt: 时间步长
- 实现逻辑：
- 根据时间常数平滑过渡到期望角速度
- 应用加速度限制
- attitude_controller_run_quat() 子函数：
- 功能：执行四元数姿态控制
- 实现逻辑：
- 获取当前姿态四元数
- 计算姿态误差
- 根据姿态误差计算角速度修正
- 应用前馈控制
- 限制角速度
- 更新姿态目标
这些函数共同实现了基于推力向量和航向的姿态控制算法，能够平滑地控制飞行器朝向指定的推力方向和航向。
姿态控制
attitude_controller_run_quat 函数是AC_AttitudeControl类的核心姿态控制函数，主要功能如下：
- 输入输出：
- 输入：_attitude_target(目标姿态四元数)、_ang_vel_target_rads(目标角速度)
- 输出：_ang_vel_body_rads(最终计算出的机体角速度)
-
主要工作流程：
a) 获取当前机体姿态四元数
b) 计算当前姿态与目标姿态之间的旋转误差(attitude_error)
c) 根据姿态误差计算角速度修正量(update_ang_vel_target_from_att_error)
d) 应用角速度限制(ang_vel_limit)
e) 处理推力向量误差：- 当推力误差大于阈值时，优先修正推力向量
- 逐步加入前馈控制量
f) 记录姿态误差用于EKF重置处理
g) 输出最终角速度命令
-
关键算法：
- 使用四元数计算姿态误差
- 采用PID控制器计算角速度修正
- 可选择使用平方根控制器(sqrt_controller)进行非线性控制
- 对推力向量和航向进行优先级处理
- 特点：
- 支持优先修正推力向量确保飞行稳定性
- 包含完善的角速度限制机制
- 与速率控制器紧密配合
- 处理EKF重置等特殊情况
该函数是连接高层姿态指令与底层速率控制的关键环节，实现了从姿态误差到角速度命令的转换。
/**
* @brief 运行四元数姿态控制器，计算并更新姿态控制所需的角速度
*
* 此函数通过获取当前机体姿态和目标姿态，计算姿态误差，进而得到角速度修正量，
* 并根据姿态误差调整前馈项，最终更新姿态控制的目标角速度。
*/
void AC_AttitudeControl::attitude_controller_run_quat()
{
// 获取当前机体在 NED（北东地）坐标系下的姿态，用四元数表示
// This represents a quaternion rotation in NED frame to the body
Quaternion attitude_body;
_ahrs.get_quat_body_to_ned(attitude_body);
// 计算姿态误差向量，其中 x 和 y 分量用于修正推力向量，z 分量用于修正航向
// This vector represents the angular error to rotate the thrust vector using x and y and heading using z
Vector3f attitude_error;
thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle_rad, _thrust_error_angle_rad);
// 根据姿态误差计算机体坐标系下的角速度修正量
// Compute the angular velocity corrections in the body frame from the attitude error
Vector3f ang_vel_body_rads = update_ang_vel_target_from_att_error(attitude_error);
// 确保计算得到的角速度不超过配置的最大限制
// ensure angular velocity does not go over configured limits
ang_vel_limit(ang_vel_body_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs));
// 计算从目标姿态坐标系到机体姿态坐标系的旋转四元数
// rotation from the target frame to the body frame
Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target;
// 将目标角速度向量从目标姿态坐标系转换到机体姿态坐标系
// target angle velocity vector in the body frame
Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target_rads;
// 获取最新的陀螺仪数据
Vector3f gyro = get_latest_gyro();
// 初始化前馈标量为 1.0
// Correct the thrust vector and smoothly add feedforward and yaw input
_feedforward_scalar = 1.0f;
// 根据推力误差角度调整角速度修正量和前馈项
if (_thrust_error_angle_rad > AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD * 2.0f) {
// 当推力误差角度过大时，将偏航角速度设置为陀螺仪测量值
ang_vel_body_rads.z = gyro.z;
} else if (_thrust_error_angle_rad > AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD) {
// 当推力误差角度处于中间范围时，根据误差角度调整前馈标量
_feedforward_scalar = (1.0f - (_thrust_error_angle_rad - AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD) / AC_ATTITUDE_THRUST_ERROR_ANGLE_RAD);
// 将前馈项按比例添加到横滚和俯仰角速度修正量中
ang_vel_body_rads.x += ang_vel_body_feedforward.x * _feedforward_scalar;
ang_vel_body_rads.y += ang_vel_body_feedforward.y * _feedforward_scalar;
// 将偏航前馈项全部添加到偏航角速度修正量中
ang_vel_body_rads.z += ang_vel_body_feedforward.z;
// 根据前馈标量混合陀螺仪测量值和偏航角速度修正量
ang_vel_body_rads.z = gyro.z * (1.0 - _feedforward_scalar) + ang_vel_body_rads.z * _feedforward_scalar;
} else {
// 当推力误差角度较小时，将前馈项全部添加到角速度修正量中
ang_vel_body_rads += ang_vel_body_feedforward;
}
// 记录姿态误差四元数，用于处理 EKF 重置的情况
// Record error to handle EKF resets
_attitude_ang_error = attitude_body.inverse() * _attitude_target;
// 最终更新姿态控制的目标角速度
// finally update the attitude target
_ang_vel_body_rads = ang_vel_body_rads;
}
遥控器控制
遥控器摇杆控制无人机的核心代码流程：
- 输入获取阶段：
- 通过hal.rcin->read()获取原始PWM值(radio_in)
- 在RC_Channel::update()中处理输入
- 输入转换阶段：
- 根据控制类型调用不同转换函数：
- pwm_to_angle(): 将PWM转换为角度控制量(-4500~4500 centidegrees)
- pwm_to_range(): 将PWM转换为范围控制量(0~high_in)
- 控制量处理：
- 考虑死区(dead_zone)和反向(reversed)设置
- 应用微调(trim)值
- 输出control_in控制量
- 飞行控制阶段：
- 控制量传递给对应飞行模式处理(mode.cpp)
- 最终转换为电机输出
关键转换函数实现：
// PWM转角度
float RC_Channel::pwm_to_angle() const {
int16_t radio_trim_high = radio_trim + dead_zone;
int16_t radio_trim_low = radio_trim - dead_zone;
float reverse_mul = (reversed?-1:1);
if (radio_in > radio_trim_high) {
return reverse_mul * high_in * (radio_in - radio_trim_high)/(radio_max - radio_trim_high);
} else if (radio_in < radio_trim_low) {
return reverse_mul * high_in * (radio_in - radio_trim_low)/(radio_trim_low - radio_min);
}
return 0;
}
// PWM转范围
float RC_Channel::pwm_to_range() const {
int16_t radio_trim_low = radio_min + dead_zone;
if (radio_in > radio_trim_low) {
return high_in * (radio_in - radio_trim_low)/(radio_max - radio_trim_low);
}
return 0;
}
典型摇杆通道配置：
- 横滚/俯仰：ANGLE类型(默认±45度)
- 油门：RANGE类型(0~100%)
- 偏航：ANGLE类型(默认±45度)
控制量最终由各飞行模式(mode.cpp)处理，转换为姿态控制指令。
微调(trim)值在遥控器输入处理中的具体实现：
- Trim值存储：
- 每个通道有radio_trim参数(默认1500us)
- 存储在RC_Channel类的radio_trim变量中
- 可通过参数系统调整
- Trim在输入转换中的应用：
// 在pwm_to_angle()中使用trim作为中心点
float RC_Channel::pwm_to_angle() const {
int16_t radio_trim_high = radio_trim + dead_zone; // trim加上死区上界
int16_t radio_trim_low = radio_trim - dead_zone; // trim减去死区下界
if (radio_in > radio_trim_high) {
// 计算trim以上部分的控制量
return (radio_in - radio_trim_high) * high_in / (radio_max - radio_trim_high);
} else if (radio_in < radio_trim_low) {
// 计算trim以下部分的控制量
return (radio_in - radio_trim_low) * high_in / (radio_trim_low - radio_min);
}
return 0; // 在死区内返回0
}
- Trim保存机制：
// 保存trim到AHRS(姿态参考系统)
void RC_Channels_Copter::save_trim() {
// 获取当前摇杆输入作为trim偏移量
float roll_trim = cd_to_rad((float)get_roll_channel().get_control_in());
float pitch_trim = cd_to_rad((float)get_pitch_channel().get_control_in());
// 将trim值添加到AHRS
AP::ahrs().add_trim(roll_trim, pitch_trim);
// 记录日志和通知
LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM);
gcs().send_text(MAV_SEVERITY_INFO, "Trim saved");
}
- Trim的动态调整：
- 用户可通过遥控器特殊开关触发trim保存
- 保存时记录当前摇杆位置与中立位的偏差
- AHRS会持续应用这些trim值来补偿机械偏差
- Trim的影响：
- 改变控制量的零点位置
- 使摇杆在中立位置时飞机能保持特定姿态
- 补偿机械安装偏差或气动不对称
- 遥控器输入到无人机姿态控制的完整流程：
- 输入处理阶段 (RC_Channel)：
- 获取原始PWM输入(radio_in)
- 转换为标准化的控制量(control_in)
- 对于角度控制通道(横滚/俯仰/偏航)：
// 在pwm_to_angle()中转换为角度控制量 float angle = pwm_to_angle(); // 返回-4500~4500 centidegrees
- 目标角度生成 (飞行模式)：
// 以自稳模式(stabilize)为例：
void ModeStabilize::update()
{
// 从遥控器获取目标角度(单位：centidegrees)
float target_roll = get_pilot_desired_roll();
float target_pitch = get_pilot_desired_pitch();
// 转换为弧度
target_roll = radians(target_roll * 0.01f);
target_pitch = radians(target_pitch * 0.01f);
// 设置到姿态控制器
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(
target_roll, target_pitch, get_pilot_desired_yaw_rate());
}
- 姿态控制计算 (AC_AttitudeControl)：
// 在AC_AttitudeControl中计算角度误差
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw()
{
// 获取当前姿态(来自AHRS)
float roll_now = ahrs.roll;
float pitch_now = ahrs.pitch;
// 计算角度误差
float roll_error = target_roll - roll_now;
float pitch_error = target_pitch - pitch_now;
// 通过PID控制器计算修正量
float roll_out = _pid_angle_roll.get_pid(roll_error, dt);
float pitch_out = _pid_angle_pitch.get_pid(pitch_error, dt);
// 转换为电机输出
motors.set_roll(roll_out);
motors.set_pitch(pitch_out);
}
- 典型控制链路：
遥控器摇杆 → RC_Channel(转换为角度) → 飞行模式(生成目标角度) →
姿态控制器(计算误差和修正量) → 电机混控器 → 电机PWM输出
- 关键参数：
- ANGLE_MAX参数：限制最大倾斜角度(默认45度)
- 摇杆灵敏度：通过RC_FEEL_RC参数调整
- 控制响应速度：通过ATC_RATE_*参数调整PID增益
扫一扫
1239
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
