APM_ArduCopter源码解析学习（二）——电机库学习
一、RC输入与输出
1.1 RC Input
在开始学习ArduCopter的电机库之前，先来看一下它的RC输入和输出。在Copter.h中声明了最基本的用于运动控制的通道，共4个输入通道，分别为：
// primary input control channels
RC_Channel *channel_roll;
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
RC_Channel *channel_yaw;
这里先说明一下Copter.h中定义了最基本的无人机类型，即Copter类，内部包含用于控制无人机的最基本的变量和函数等。
这几个通道的意思，就是通过其中的通道输入来控制无人机的某一个方向上的运动，它并不是指某一个特定的电机，通常一个通道影响着很多与它控制运动相关的电机。
官方手册中给出RCInput的具体路径为： libraries/AP_HAL/examples/RCInput/RCInput.cpp
1.2 RC Output
官方手册中给出RCOutput的具体路径为： libraries/AP_HAL/examples/RCOutput/RCOutput.cpp。后期需要对这部分代码进行详细梳理。二、电机库学习
在开始之前先明确一下什么是多旋翼的推力分配（建议仔细阅读，务必了解原理）：
[飞控]从零开始建模(三)-控制分配浅析
多旋翼飞行器的控制分配
在Copter.h中定义的Copter类中，指明了其所使用的电机类型
#if FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#else
#define MOTOR_CLASS AP_MotorsMulticopter
#endif
// Motor Output
MOTOR_CLASS *motors;
由此可知ArduCopter的电机库配置位于libraries\AP_Motors路径下的AP_MotorsMulticopter.cpp/AP_MotorsMulticopter.h文件中。其内部继承关系如下所示。
AP_Motors
|---- AP_MotorsMulticopter
|---- AP_MotorsMatrix
2.1 setup_motors()
由于内部代码还是有点小长的，这里就不全部放进来了，节选部分讲解一下。
这个函数最主要的内容就是配置电机，包括每个电机对于不同运动的影响程度（推力分配）。
函数刚开始，首先就是把最初的所有电机配置全部移除，方便后续进行更改。这里的AP_MOTORS_MAX_NUM_MOTORS为最大的电机数，于AP_Motors_Class.h中定义为12.
// remove existing motors
for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
remove_motor(i);
}
- 1
- 2
- 3
- 4
然后进入一个switch()函数中进行具体的配置内容，首先判断是属于哪一种frame_class的架构，ArduCopter这边给出了14种配置结构，定义于AP_Motors_Class.h中的枚举类型里，如下所示
enum motor_frame_class {
MOTOR_FRAME_UNDEFINED = 0,
MOTOR_FRAME_QUAD = 1,
MOTOR_FRAME_HEXA = 2,
MOTOR_FRAME_OCTA = 3,
MOTOR_FRAME_OCTAQUAD = 4,
MOTOR_FRAME_Y6 = 5,
MOTOR_FRAME_HELI = 6,
MOTOR_FRAME_TRI = 7,
MOTOR_FRAME_SINGLE = 8,
MOTOR_FRAME_COAX = 9,
MOTOR_FRAME_TAILSITTER = 10,
MOTOR_FRAME_HELI_DUAL = 11,
MOTOR_FRAME_DODECAHEXA = 12,
MOTOR_FRAME_HELI_QUAD = 13,
};
根据不同的机身结构，frame_type定义了不同的机型类别。
enum motor_frame_type {
MOTOR_FRAME_TYPE_PLUS = 0,
MOTOR_FRAME_TYPE_X = 1,
MOTOR_FRAME_TYPE_V = 2,
MOTOR_FRAME_TYPE_H = 3,
MOTOR_FRAME_TYPE_VTAIL = 4,
MOTOR_FRAME_TYPE_ATAIL = 5,
MOTOR_FRAME_TYPE_PLUSREV = 6, // plus with reversed motor direction
MOTOR_FRAME_TYPE_Y6B = 10,
MOTOR_FRAME_TYPE_Y6F = 11, // for FireFlyY6
MOTOR_FRAME_TYPE_BF_X = 12, // X frame, betaflight ordering
MOTOR_FRAME_TYPE_DJI_X = 13, // X frame, DJI ordering
MOTOR_FRAME_TYPE_CW_X = 14, // X frame, clockwise ordering
MOTOR_FRAME_TYPE_I = 15, // (sideways H) octo only
MOTOR_FRAME_TYPE_NYT_PLUS = 16, // plus frame, no differential torque for yaw
MOTOR_FRAME_TYPE_NYT_X = 17, // X frame, no differential torque for yaw
MOTOR_FRAME_TYPE_BF_X_REV = 18, // X frame, betaflight ordering, reversed motors
};
以X型四旋翼进行说明：
case MOTOR_FRAME_TYPE_X:
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
break;
add_moto()这个函数的作用就是配置每一个电机在某一运动功能上的影响因子，AP_MOTORS_MOT_1从右上角开始，逆时针进行编号增加
2.2 add_motor
// add_motor using just position and prop direction - assumes that for each motor, roll and pitch factors are equal
void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order)
{
add_motor(motor_num, angle_degrees, angle_degrees, yaw_factor, testing_order);
}
void AP_MotorsMatrix::add_motor(int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order)
{
add_motor_raw(
motor_num,
cosf(radians(roll_factor_in_degrees + 90)),
cosf(radians(pitch_factor_in_degrees)),
yaw_factor,
testing_order);
}
代码内容如上，这部分程序时直接调用了AP_MotorsMatrix.cpp中的add_motor_raw()函数，这个函数原本的作用是用来配置空中无人机的RPY方向上各个电机对各方向的影响因子，但是ROV在水下运动时多了沉浮、前后平移和左右平移的功能，因此在后面添加了3个相关的影响因子配置数组。
add_motor_raw()函数原型如下：
// add_motor
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
{
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
2.3 normalise_rpy_factors
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
void AP_MotorsMatrix::normalise_rpy_factors()
{
float roll_fac = 0.0f;
float pitch_fac = 0.0f;
float yaw_fac = 0.0f;
// find maximum roll, pitch and yaw factors
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (roll_fac < fabsf(_roll_factor[i])) {
roll_fac = fabsf(_roll_factor[i]);
}
if (pitch_fac < fabsf(_pitch_factor[i])) {
pitch_fac = fabsf(_pitch_factor[i]);
}
if (yaw_fac < fabsf(_yaw_factor[i])) {
yaw_fac = fabsf(_yaw_factor[i]);
}
}
}
// scale factors back to -0.5 to +0.5 for each axis
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (!is_zero(roll_fac)) {
_roll_factor[i] = 0.5f * _roll_factor[i] / roll_fac;
}
if (!is_zero(pitch_fac)) {
_pitch_factor[i] = 0.5f * _pitch_factor[i] / pitch_fac;
}
if (!is_zero(yaw_fac)) {
_yaw_factor[i] = 0.5f * _yaw_factor[i] / yaw_fac;
}
}
}
}
该函数的作用是将影响因子约束在-0.5~0.5之间。
2.4 output_to_motors()
void AP_MotorsMatrix::output_to_motors()
{
int8_t i;
switch (_spool_state) {
case SpoolState::SHUT_DOWN: {
// no output
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_actuator[i] = 0.0f;
}
}
break;
}
case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle());
}
}
break;
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN:
// set motor output based on thrust requests
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
}
}
break;
}
// convert output to PWM and send to each motor
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, output_to_pwm(_actuator[i]));
}
}
}
这个函数也很简单，最开始的 _actuator数组就是用来储存每一个电机最后的pwm值，switch函数判断轴状态，这里只需要知道SHUT_DOWN和GROUND_IDLE状态下，推进器不工作即可。而在SPOOLING_UP、THROTTLE_UNLIMITED和SPOOLING_DOWN状态下，对每一个电机调用set_actuator_with_slew函数计算最后的pwm值并且保存到 _actuator数组中。最后通过rc_write()函数输出到每一个通道。
2.5 output_armed_stabilizing()
推力限幅及分配函数
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed_stabilizing()
{
uint8_t i; // general purpose counter
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float throttle_thrust_max; // throttle thrust maximum value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float yaw_allowed = 1.0f; // amount of yaw we can fit in
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
throttle_avg_max = _throttle_avg_max * compensation_gain;
// If thrust boost is active then do not limit maximum thrust
throttle_thrust_max = _thrust_boost_ratio + (1.0f - _thrust_boost_ratio) * _throttle_thrust_max * compensation_gain;
上述代码中运用电压和气压补偿进入通道运算。如果不用这部分做补偿，也是可行的。
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= throttle_thrust_max) {
throttle_thrust = throttle_thrust_max;
limit.throttle_upper = true;
}
// ensure that throttle_avg_max is between the input throttle and the maximum throttle
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);
// calculate the highest allowed average thrust that will provide maximum control range
throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
// calculate throttle that gives most possible room for yaw which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
// 2. the higher of:
// a) the pilot's throttle input
// b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
// To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
// Y6 : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75, ATC_RAT_PIT_IMAX = 0.75, ATC_RAT_YAW_IMAX = 0.375
// Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
// Hex : MOT_YAW_HEADROOM = 0, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25, ATC_RAT_PIT_IMAX = 0.25, ATC_RAT_YAW_IMAX = 0.25
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5, ATC_RAT_PIT_IMAX = 0.5, ATC_RAT_YAW_IMAX = 0.25
// Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
float rp_low = 1.0f; // lowest thrust value
float rp_high = -1.0f; // highest thrust value
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
// calculate the thrust outputs for roll and pitch
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
// record lowest roll + pitch command
if (_thrust_rpyt_out[i] < rp_low) {
rp_low = _thrust_rpyt_out[i];
}
// record highest roll + pitch command
if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) {
rp_high = _thrust_rpyt_out[i];
}
// Check the maximum yaw control that can be used on this channel
// Exclude any lost motors if thrust boost is enabled
if (!is_zero(_yaw_factor[i]) && (!_thrust_boost || i != _motor_lost_index)){
if (is_positive(yaw_thrust * _yaw_factor[i])) {
yaw_allowed = MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i]));
} else {
yaw_allowed = MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i]));
}
}
}
}
// calculate the maximum yaw control that can be used
// todo: make _yaw_headroom 0 to 1
float yaw_allowed_min = (float)_yaw_headroom / 1000.0f;
// increase yaw headroom to 50% if thrust boost enabled
yaw_allowed_min = _thrust_boost_ratio * 0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed_min;
// Let yaw access minimum amount of head room
yaw_allowed = MAX(yaw_allowed, yaw_allowed_min);
// Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
if (_thrust_boost && motor_enabled[_motor_lost_index]) {
// record highest roll + pitch command
if (_thrust_rpyt_out[_motor_lost_index] > rp_high) {
rp_high = _thrust_boost_ratio * rp_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
}
// Check the maximum yaw control that can be used on this channel
// Exclude any lost motors if thrust boost is enabled
if (!is_zero(_yaw_factor[_motor_lost_index])){
if (is_positive(yaw_thrust * _yaw_factor[_motor_lost_index])) {
yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index]), 0.0f)/_yaw_factor[_motor_lost_index]));
} else {
yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index], 0.0f)/_yaw_factor[_motor_lost_index]));
}
}
}
if (fabsf(yaw_thrust) > yaw_allowed) {
// not all commanded yaw can be used
yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
limit.yaw = true;
}
// add yaw control to thrust outputs
float rpy_low = 1.0f; // lowest thrust value
float rpy_high = -1.0f; // highest thrust value
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
// record lowest roll + pitch + yaw command
if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i];
}
// record highest roll + pitch + yaw command
// Exclude any lost motors if thrust boost is enabled
if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index)) {
rpy_high = _thrust_rpyt_out[i];
}
}
}
// Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
if (_thrust_boost) {
// record highest roll + pitch + yaw command
if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) {
rpy_high = _thrust_boost_ratio * rpy_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index];
}
}
// calculate any scaling needed to make the combined thrust outputs fit within the output range
if (rpy_high - rpy_low > 1.0f) {
rpy_scale = 1.0f / (rpy_high - rpy_low);
}
if (throttle_avg_max + rpy_low < 0) {
rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
}
// calculate how close the motors can come to the desired throttle
rpy_high *= rpy_scale;
rpy_low *= rpy_scale;
throttle_thrust_best_rpy = -rpy_low;
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f) {
// Full range is being used by roll, pitch, and yaw.
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f) {
limit.throttle_upper = true;
}
thr_adj = 0.0f;
} else {
if (thr_adj < 0.0f) {
// Throttle can't be reduced to desired value
// todo: add lower limit flag and ensure it is handled correctly in altitude controller
thr_adj = 0.0f;
} else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) {
// Throttle can't be increased to desired value
thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high);
limit.throttle_upper = true;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]);
}
}
// determine throttle thrust for harmonic notch
const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
// compensation_gain can never be zero
_throttle_out = throttle_thrust_best_plus_adj / compensation_gain;
// check for failed motor
check_for_failed_motor(throttle_thrust_best_plus_adj);
这部分后面有时间再进行梳理
其他函数
无。
三、参考资料
2020/12/2更新：增加了部分解释以及推力分配内容
扫一扫
954
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
