无人机电机的代码解析-ardupilot飞控

最新推荐文章于 2025-07-02 23:28:40 发布

fengforky

最新推荐文章于 2025-07-02 23:28:40 发布

阅读量963

点赞数 14

CC 4.0 BY-SA版权

文章标签：无人机

本文链接：https://blog.youkuaiyun.com/fengforky/article/details/148650520

AP_Motors_Class.cpp 代码详解：

构造函数关键部分

AP_Motors::AP_Motors(uint16_t speed_hz) :
    _speed_hz(speed_hz),
    _throttle_filter(),
    _throttle_slew(),
    _throttle_slew_filter(),
    _spool_desired(DesiredSpoolState::SHUT_DOWN),
    _spool_state(SpoolState::SHUT_DOWN)
{
    _singleton = this;  // 设置单例实例
    _throttle_filter.set_cutoff_frequency(0.0f); // 初始化油门滤波器
    _throttle_slew_filter.set_cutoff_frequency(AP_MOTORS_SLEW_FILTER_CUTOFF); // 50Hz滤波
}

初始化电机控制频率和滤波器
采用单例模式确保全局唯一实例
默认状态为关闭(SHUT_DOWN)

PWM输出核心方法

void AP_Motors::rc_write(uint8_t chan, uint16_t pwm) {
    SRV_Channel::Function function = SRV_Channels::get_motor_function(chan);
    if ((1U<<chan) & _motor_pwm_scaled.mask) {
        SRV_Channels::set_output_scaled(function, float(pwm) - _motor_pwm_scaled.offset);
    } else {
        SRV_Channels::set_output_pwm(function, pwm); // 原始PWM输出
    }
}

支持两种输出模式：原始PWM和缩放输出
通过motor_pwm_scaled.mask判断使用哪种模式
与SRV_Channel系统集成

电机状态管理

void AP_Motors::armed(bool arm) {
    if (_armed != arm) {
        _armed = arm;
        AP_Notify::flags.armed = arm; // 更新通知系统
        if (!arm) {
            save_params_on_disarm(); // 解锁时保存参数
        }
    }
}

控制电机解锁状态
状态变化时触发通知系统
解锁时自动保存参数

PWM类型设置(代码片段)

case PWMType::DSHOT600:
    hal.rcout->set_output_mode(mask, AP_HAL::RCOutput::MODE_PWM_DSHOT600);
    break;
case PWMType::PWM_RANGE:
    _motor_pwm_scaled.offset = 1000.0;
    SRV_Channels::set_range(SRV_Channels::get_motor_function(i), 1000);
    break;

支持多种数字协议(DSHOT150-1200)
PWM_RANGE模式将1000-2000映射为0-1000
通过HAL层抽象实现跨平台支持

安全限制检查

void AP_Motors::set_limit_flag_pitch_roll_yaw(bool flag) {
    limit.roll = flag;  // 横滚限位
    limit.pitch = flag; // 俯仰限位 
    limit.yaw = flag;   // 偏航限位
}

各轴独立限位保护
在输出计算前进行检查
防止控制量超出安全范围

电机测试功能

bool AP_Motors::output_test_seq(uint8_t motor_seq, int16_t pwm) {
    if (armed() && _interlock) {
        _output_test_seq(motor_seq, pwm);
        return true;
    }
    output_min(); // 安全保护
    return false;
}

需在解锁状态才能测试
有硬件互锁保护
测试失败时输出最小值保障安全

该实现展示了ArduPilot电机控制系统的关键设计：
7. 硬件抽象层(HAL)隔离平台差异
8. 状态机管理电机工作流程
9. 多种PWM协议统一接口
10. 多层次安全保护机制
11. 良好的扩展性支持新型电机

无人机起飞和平衡控制原理详解：

控制系统架构

三级闭环控制结构：
┌─────────────┐ ┌─────────────┐ ┌─────────────┐
│ 姿态控制器 │───▶│ 角速率控制器 │───▶│ 电机控制 │
└─────────────┘ └─────────────┘ └─────────────┘
(外环/角度环) (中环/角速率环) (内环/执行器)

起飞过程

解锁流程：
AP_Motors::armed(true) → 电机进入怠速状态(SpoolState::GROUND_IDLE)
油门提升：
遥控器油门信号 → AP_Motors::set_radio_passthrough() → 电机转速提升
缓启动：
_throttle_slew_filter限制油门变化率，防止瞬时大油门

姿态稳定机制

传感器反馈：
AHRS(姿态参考系统)提供当前欧拉角(roll,pitch,yaw)和角速率
姿态误差计算：
AC_AttitudeControl::thrust_heading_rotation_angles()计算目标与实际姿态差
PID控制：
├─ 角度环(P控制)：产生角速率目标值
│ 例：roll_rate_target = _p_angle_roll.kP() * roll_error
│
└─ 角速率环(PID控制)：产生电机控制量
│ 使用PID.h中的算法：
│ output = error * kP + ∫error*dt * kI + d(error)/dt * kD
└─ 通过get_rate_roll_pid()等函数实现

电机混控

控制量分配：
AP_Motors::rc_write()将控制量转换为PWM信号
混控算法：
根据机型(四轴/六轴等)将滚转、俯仰、偏航控制量分配到各电机
限幅保护：
AP_Motors::set_limit_flag_pitch_roll_yaw()防止超出物理限制

抗扰动机制

积分抗饱和：PID中的积分项(ki)有限幅
低通滤波：_throttle_filter滤除高频噪声
前馈补偿：_rate_bf_ff_enabled时加入角速率前馈

典型控制流程示例

传感器检测到无人机向右倾斜(roll+)
姿态控制器计算滚转误差：roll_error = target_roll - current_roll
角速率控制器输出：roll_rate = PID(roll_error)
电机混控：左侧电机增速，右侧电机减速
产生恢复力矩使无人机回平

关键参数

角度环P值(ANG_RLL_P, ANG_PIT_P, ANG_YAW_P)
角速率环PID值(RAT_RLL_P, RAT_RLL_I, RAT_RLL_D)
最大角加速度(ACCEL_R_MAX, ACCEL_P_MAX)
电机怠速值(MOT_SPIN_ARMED)

这套控制系统在ArduPilot中实现约400Hz的闭环控制频率，确保飞行稳定性。

无人机电机混控系统详解：

控制量转换流程

输入：来自姿态控制器的三轴控制量(roll, pitch, yaw) + 油门量
归一化：将控制量转换为-1~+1范围
混控矩阵：根据机型配置将控制量分配到各电机
输出：PWM信号(1000~2000μs)

四轴X型混控示例
电机编号布局：

混控公式：

motor1 = throttle + roll*0.707 - pitch*0.707 + yaw
motor2 = throttle - roll*0.707 - pitch*0.707 - yaw  
motor3 = throttle - roll*0.707 + pitch*0.707 + yaw
motor4 = throttle + roll*0.707 + pitch*0.707 - yaw

(其中0.707是sin(45°)，实现滚转和俯仰的耦合分配)

六轴混控示例
电机编号布局：

混控公式增加两个电机，分配系数调整：

motor1 = throttle + roll*0.5 - pitch*0.866 + yaw
motor2 = throttle - roll*0.5 - pitch*0.866 - yaw
motor3 = throttle - roll*1.0 + pitch*0.0 + yaw  
motor4 = throttle + roll*1.0 + pitch*0.0 - yaw
motor5 = throttle + roll*0.5 + pitch*0.866 + yaw
motor6 = throttle - roll*0.5 + pitch*0.866 - yaw

(0.866=sin(60°)，实现六轴的力矩平衡)

PWM信号生成关键代码

rc_write()函数处理：

void AP_Motors::rc_write(uint8_t chan, uint16_t pwm) {
    SRV_Channel::Function function = SRV_Channels::get_motor_function(chan);
    SRV_Channels::set_output_pwm(function, pwm); // 实际输出PWM
}

信号限幅保护：

pwm = constrain_int16(pwm, _pwm_min, _pwm_max); // 通常1000-2000μs

混控特性

动态调整：根据电机数量自动计算分配系数
故障保护：单个电机失效时自动降权
转向补偿：正反桨配置自动反转yaw控制符号
怠速处理：确保停转后能可靠重启(_spin_min_ratio)

实际飞行中的混控过程示例：

需要向右滚转时：
- 姿态控制器输出正roll值
- 左侧电机增速(1,4号)，右侧电机减速(2,3号)
- 产生向右的滚转力矩
需要前进时：
- 姿态控制器输出负pitch值
- 后方电机增速(3,4号)，前方电机减速(1,2号)
- 产生向前的俯仰力矩

关键参数：

MOT_SLEW_MAX：电机转速变化率限制(防止瞬时大电流)
MOT_SPIN_ARMED：怠速PWM值(确保电机随时可响应)
MOT_YAW_HEADROOM：偏航控制余量(防止饱和)