Ardusub源码解析学习（二）——电机库学习

一、RC输入与输出
- 1.1 RC Input
- 1.2 RC Output
二、电机库学习
- 2.1 setup_motors()
- 2.2 add_motor_raw_6dof()
- 2.3 output_min()
- 2.4 calc_thrust_to_pwm()
- 2.5 output_to_motors()
- 2.6 get_current_limit_max_throttle()
- 2.7 output_armed_stabilizing()
- 其他函数
三、参考资料

一、RC输入与输出

1.1 RC Input

在开始学习Ardusub的电机库之前，先来看一下它的RC输入和输出。在Sub.h中声明了最基本的用于运动控制的通道，共6个输入通道，分别为：

    // primary input control channelsRC_Channel *channel_roll;RC_Channel *channel_pitch;RC_Channel *channel_throttle;RC_Channel *channel_yaw;RC_Channel *channel_forward;RC_Channel *channel_lateral;

这里先说明一下Sub.h中定义了最基本的车辆类型，即Sub类，内部包含用于控制水下无人车辆的最基本的变量和函数等。

这几个通道的意思，就是通过其中的通道输入来控制水下无人潜艇的某一个方向上的运动，它并不是指某一个特定的电机，通常一个通道影响着很多与它控制运动相关的电机。前四个大家应该也很熟悉了，空中无人机基本上都包含这四个输入控制通道，最后两个我在上一篇博文里面有简单提到，这是水下无人机的前后平移以及左右平移控制的RC输入通道。

官方手册中说明RC输入是一组控制通道，代表操作员的输入，例如前进和偏航命令等。尽管水下无人车系统Ardusub可能并不接受RC接收器，但是使用操纵杆（类似游戏机手柄等）的输入控制逻辑是类似的。除了最基本的运动控制通道外，官方还列出了其他默认的输入通道映射，如下表所示。

Channel	Meaning
1	Pitch
2	Roll
3	Throttle
4	Yaw
5	Forward
6	Lateral
7	Camera Pan
8	Camera Tilt
9	Lights 1 Level
10	Lights 2 Level
11	Video Switch

1.2 RC Output

Pixhawk2.4.8应该是属于1代的产品了，它的主输出通道有8个，辅助输出通道有6个。其中8个MAIN OUT对应连接水下推进器，输出PWM波进行控制，更新频率为200Hz。6个AUX OUT同样也可以产生PWM控制信号，用来控制一些辅助器件如云台舵机、灯光亮度等（当然你也可以用MAIN OUT），但需要注意的是其通道5和6默认保留当做普通GPIO使用。

AUX OUT的默认PWM输出个数配置由BRD_PWM_COUNT参数决定，默认为4。可选择配置如图所示

二、电机库学习

在开始之前先明确一下什么是多旋翼的推力分配（建议仔细阅读，务必了解原理）：
[飞控]从零开始建模(三)-控制分配浅析
多旋翼飞行器的控制分配

在Sub.h中定义的Sub类中，指明了其所使用的电机类型

AP_Motors6DOF motors;

由此可知Ardusub的电机库配置位于libraries\AP_Motors路径下的AP_Motors6DOF.cpp/AP_Motors6DOF.h文件中。其内部继承关系如下所示。

AP_Motors
|---- AP_MotorsMulticopter
|---- AP_MotorsMatrix
|---- AP_Motors6DOF

进入这个文件之后就可以很清楚看到注释如下，指明该类为ROV专用电机控制函数，当然一些最基本的函数都是直接从父类那边继承过来了，这个类里面只是实现了一些针对于ROV的专用函数。

/// @file   AP_Motors6DOF.h
/// @brief Motor control class for ROVs with direct control over 6DOF (or fewer) in movement

在AP_Motors6DOF.h中定义了AP_Motors6DOF类，它向上继承自AP_MotorsMatrix类，并向内部添加了一些ROV控制专属的变量和函数。这边主要来学习一下AP_Motors6DOF.cpp文件中的代码内容。

2.1 setup_motors()

由于内部代码还是有点小长的，这里就不全部放进来了，节选部分讲解一下。

这个函数最主要的内容就是配置电机，包括每个电机对于不同运动的影响程度（推力分配）。

函数刚开始，首先就是把最初的所有电机配置全部移除，方便后续进行更改。这里的AP_MOTORS_MAX_NUM_MOTORS为最大的电机数，于AP_Motors_Class.h中定义为12.

    // remove existing motorsfor (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {remove_motor(i);}

然后进入一个switch()函数中进行具体的配置内容，首先判断是属于哪一种ROV的架构，Ardusub这边给出了8种配置结构，定义于AP_Motors6DOF.h中的枚举类型里，如下所示

    // Supported frame typestypedef enum {SUB_FRAME_BLUEROV1,SUB_FRAME_VECTORED,SUB_FRAME_VECTORED_6DOF,SUB_FRAME_VECTORED_6DOF_90DEG,SUB_FRAME_SIMPLEROV_3,SUB_FRAME_SIMPLEROV_4,SUB_FRAME_SIMPLEROV_5,SUB_FRAME_CUSTOM} sub_frame_t;

具体每一个对应的类型给出如下，上面最后面的CUSTOM是支持的用户自定义架构。

以vectored6dof举例来说，其配置内容如下：

        //                 Motor #              Roll Factor     Pitch Factor    Yaw Factor      Throttle Factor     Forward Factor      Lateral Factor  Testing Ordercase SUB_FRAME_VECTORED_6DOF:add_motor_raw_6dof(AP_MOTORS_MOT_1,     0,              0,              1.0f,           0,                  -1.0f,              1.0f,           1);add_motor_raw_6dof(AP_MOTORS_MOT_2,     0,              0,              -1.0f,          0,                  -1.0f,              -1.0f,          2);add_motor_raw_6dof(AP_MOTORS_MOT_3,     0,              0,              -1.0f,          0,                  1.0f,               1.0f,           3);add_motor_raw_6dof(AP_MOTORS_MOT_4,     0,              0,              1.0f,           0,                  1.0f,               -1.0f,          4);add_motor_raw_6dof(AP_MOTORS_MOT_5,     1.0f,           -1.0f,          0,              -1.0f,              0,                  0,              5);add_motor_raw_6dof(AP_MOTORS_MOT_6,     -1.0f,          -1.0f,          0,              -1.0f,              0,                  0,              6);add_motor_raw_6dof(AP_MOTORS_MOT_7,     1.0f,           1.0f,           0,              -1.0f,              0,                  0,              7);add_motor_raw_6dof(AP_MOTORS_MOT_8,     -1.0f,          1.0f,           0,              -1.0f,              0,                  0,              8);break;

add_motor_raw_6dof()这个函数的作用就是配置每一个电机在某一运动功能上的影响因子，或者简单点说就是进行推力分配，一般为-1~1。以motor5来说，其在ROV运动时对于ROLL方向的运动的影响为1，而对于PITCH方向上的运动则是反作用的-1，在油门Throttle控制沉浮上它的影响因子则为-1，这边的正负与水下推进器的安装方向和正反螺旋桨有关。最后的值是用来进行电机测试时的编号。

2.2 add_motor_raw_6dof()

void AP_Motors6DOF::add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float throttle_fac, float forward_fac, float lat_fac, uint8_t testing_order)
{//Parent takes care of enabling output and setting up masksadd_motor_raw(motor_num, roll_fac, pitch_fac, yaw_fac, testing_order);//These are additional parameters for an ROV_throttle_factor[motor_num] = throttle_fac;_forward_factor[motor_num] = forward_fac;_lateral_factor[motor_num] = lat_fac;
}

代码内容如上，这部分程序时直接调用了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 providedif (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {// increment number of motors if this motor is being newly motor_enabledif (!motor_enabled[motor_num]) {motor_enabled[motor_num] = true;}// set roll, pitch, thottle factors and opposite motor (for stability patch)_roll_factor[motor_num] = roll_fac;_pitch_factor[motor_num] = pitch_fac;_yaw_factor[motor_num] = yaw_fac;// set order that motor appears in test_test_order[motor_num] = testing_order;// call parent class methodadd_motor_num(motor_num);}
}

拓展阅读：ArduCopter如何进行推力分配（可以直接跳过）

内容类似就不再解释了，可以简单看看。在AP_MotorsMatrix.cpp中有

void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
{// remove existing motorsfor (int8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {remove_motor(i);}bool success = true;switch (frame_class) {case MOTOR_FRAME_QUAD:switch (frame_type) {case MOTOR_FRAME_TYPE_PLUS:add_motor(AP_MOTORS_MOT_1,  90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);add_motor(AP_MOTORS_MOT_3,   0, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  1);add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW,  3);break;...
}

// 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);
}// add_motor using position and prop direction. Roll and Pitch factors can differ (for asymmetrical frames)
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);
}

2.3 output_min()

// output_min - sends minimum values out to the motors
void AP_Motors6DOF::output_min()
{int8_t i;// set limits flagslimit.roll = true;limit.pitch = true;limit.yaw = true;limit.throttle_lower = false;limit.throttle_upper = false;// fill the motor_out[] array for HIL use and send minimum value to each motor// ToDo find a field to store the minimum pwm instead of hard coding 1500for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {if (motor_enabled[i]) {rc_write(i, 1500);}}
}

该函数的作用是设置最小输出给电机，由于ROV在水下有沉浮功能，而它也是依赖水下推进器的正反转完成的，因此那一个以20ms为PWM控制周期的推进器举例来说，区别于空中无人机上螺旋桨只有一个方向的旋转功能，水下推进器需要在不同转向上达到最大转数，因此其最小转速反而是在中间位置，即pwm = 1500的时候（不考虑安全限制1000为反向最大，2000为正向最大）。前面的limit是配置是否已经达到某一运动的极限限制，如推进器在达到pwm=1000或2000时已经达到了最大的运动能力。

关于水下推进器的控制详见我这篇博客：STM32通过PWM控制ESC30C电调

2.4 calc_thrust_to_pwm()

int16_t AP_Motors6DOF::calc_thrust_to_pwm(float thrust_in) const
{return constrain_int16(1500 + thrust_in * 400, _throttle_radio_min, _throttle_radio_max);
}

顾名思义，该函数的作用就是将 -1~1的thrust_in输入转换为pwm输出，由于增加了安全限制，因此此处pwm控制范围为1100到1900。后面两个参数用来限幅。

2.5 output_to_motors()

void AP_Motors6DOF::output_to_motors()
{int8_t i;int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];    // final pwm values sent to the motorswitch (_spool_state) {case SpoolState::SHUT_DOWN:// sends minimum values out to the motors// set motor output based on thrust requestsfor (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {if (motor_enabled[i]) {motor_out[i] = 1500;}}break;case SpoolState::GROUND_IDLE:// sends output to motors when armed but not flyingfor (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {if (motor_enabled[i]) {motor_out[i] = 1500;}}break;case SpoolState::SPOOLING_UP:case SpoolState::THROTTLE_UNLIMITED:case SpoolState::SPOOLING_DOWN:// set motor output based on thrust requestsfor (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {if (motor_enabled[i]) {motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);}}break;}// send output to each motorfor (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {if (motor_enabled[i]) {rc_write(i, motor_out[i]);}}
}

这个函数也很简单，最开始的motor_out数组就是用来储存每一个电机最后的pwm值，switch函数判断轴状态，关于轴状态内容详见我的上一篇博文。这里只需要知道SHUT_DOWN和GROUND_IDLE状态下，推进器不工作即可。而在SPOOLING_UP、THROTTLE_UNLIMITED和SPOOLING_DOWN状态下，对每一个电机调用calc_thrust_to_pwm()函数计算最后的pwm值并且保存到motor_out数组中。最后通过rc_write()函数输出到每一个通道。

2.6 get_current_limit_max_throttle()

float AP_Motors6DOF::get_current_limit_max_throttle()
{return 1.0f;
}

过于简单就不解释了，仅把代码附一下。

2.7 output_armed_stabilizing()

推力限幅及分配函数

这个函数是最主要的，内部分为电机部分以及电流部分。这边就讲一下电机的控制部分，电流部分的话大家自己去研究一下也蛮简单的。

这段代码开头首先判断是什么类型的框架结构，如果是vectored类型的，则调用output_armed_stabilizing_vectored()函数，如果是vectored_6odf类型的，那么久调用output_armed_stabilizing_vectored_6dof()，否则，就直接进入else部分内容程序（具体框架见2.1小节）。这几个函数内部程序大致上是相似的，因此这边仅以else内部的代码作为讲解。

    if ((sub_frame_t)_last_frame_class == SUB_FRAME_VECTORED) {output_armed_stabilizing_vectored();} else if ((sub_frame_t)_last_frame_class == SUB_FRAME_VECTORED_6DOF) {output_armed_stabilizing_vectored_6dof();} else {uint8_t i;                          // general purpose counterfloat   roll_thrust;                // roll thrust input value, +/- 1.0float   pitch_thrust;               // pitch thrust input value, +/- 1.0float   yaw_thrust;                 // yaw thrust input value, +/- 1.0float   throttle_thrust;            // throttle thrust input value, +/- 1.0float   forward_thrust;             // forward thrust input value, +/- 1.0float   lateral_thrust;             // lateral thrust input value, +/- 1.0roll_thrust = (_roll_in + _roll_in_ff);pitch_thrust = (_pitch_in + _pitch_in_ff);yaw_thrust = (_yaw_in + _yaw_in_ff);throttle_thrust = get_throttle_bidirectional();forward_thrust = _forward_in;lateral_thrust = _lateral_in;float rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor// initialize limits flagslimit.roll = false;limit.pitch = false;limit.yaw = false;limit.throttle_lower = false;limit.throttle_upper = false;// sanity check throttle is above zero and below current limited throttleif (throttle_thrust <= -_throttle_thrust_max) {throttle_thrust = -_throttle_thrust_max;limit.throttle_lower = true;}if (throttle_thrust >= _throttle_thrust_max) {throttle_thrust = _throttle_thrust_max;limit.throttle_upper = true;}// calculate roll, pitch and yaw for each motorfor (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {if (motor_enabled[i]) {rpy_out[i] = roll_thrust * _roll_factor[i] +pitch_thrust * _pitch_factor[i] +yaw_thrust * _yaw_factor[i];}}// calculate linear command for each motor// linear factors should be 0.0 or 1.0 for nowfor (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {if (motor_enabled[i]) {linear_out[i] = throttle_thrust * _throttle_factor[i] +forward_thrust * _forward_factor[i] +lateral_thrust * _lateral_factor[i];}}// Calculate final output for each motorfor (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {if (motor_enabled[i]) {_thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpy_out[i] + linear_out[i]),-1.0f,1.0f);}}}

函数进来，就先定义各个运动方向上作用的推进器（这里并不是指单独的实体推进器，而是能够在某一运动方向作用的抽象概念的推进器总合）。
然后，RPY方向上的推进器通过姿态控制器获取期望的姿态值并且与前馈控制的调整值叠加；沉浮方向上的推进器则是通过get_throttle_bidirectional()函数获取滤波之后的输入控制量；平移方向上的推进器从set_xxx方法中获取最新的平移方向上的输入指令。以上控制量范围为-1~1。
建立两个数组分别用来保存RPY方向和平移方向上的每个电机的控制量。并且通过limit设定各个运动方向并未达到限制。然后对沉浮方向的推进器控制量进行限幅，如果达到限定值，则将对应的limit值改为true。
对于每一个电机，综合各个方向的控制量信号以及前设的运动影响因子，得出其最后的控制信号（包括RPY方向和平移方向全部的电机）。
最后，判断每一个电机是否已经启用，如果启用，那么就将旋转和平移量综合，乘以方向系数（_motor_reverse[i]在文件开头已被定义，本文中未放出，一般为1，表正向），然后储存到_thrust_rpyt_out数组之中（这个数组被定义在AP_MotorsMatrix.h中，用来总合最后的电机输出控制量）。

注意_roll_in 、_pitch_in 等值都是通过set_XXX()函数赋值得到的。

    // set_roll, set_pitch, set_yaw, set_throttlevoid                set_roll(float roll_in) { _roll_in = roll_in; };        // range -1 ~ +1void                set_roll_ff(float roll_in) { _roll_in_ff = roll_in; };    // range -1 ~ +1void                set_pitch(float pitch_in) { _pitch_in = pitch_in; };    // range -1 ~ +1void                set_pitch_ff(float pitch_in) { _pitch_in_ff = pitch_in; };  // range -1 ~ +1void                set_yaw(float yaw_in) { _yaw_in = yaw_in; };            // range -1 ~ +1void                set_yaw_ff(float yaw_in) { _yaw_in_ff = yaw_in; };      // range -1 ~ +1void                set_throttle(float throttle_in) { _throttle_in = throttle_in; };   // range 0 ~ 1void                set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max, 0.0f, 1.0f); };   // range 0 ~ 1void                set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }void                set_forward(float forward_in) { _forward_in = forward_in; }; // range -1 ~ +1void                set_lateral(float lateral_in) { _lateral_in = lateral_in; };     // range -1 ~ +1

其他函数

get_motor_angular_factors()：获取各个电机姿态控制的影响因子。

Vector3f AP_Motors6DOF::get_motor_angular_factors(int motor_number) {if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {return Vector3f(0,0,0);}return Vector3f(_roll_factor[motor_number], _pitch_factor[motor_number], _yaw_factor[motor_number]);
}

motor_is_enabled()：判断电机是否已启动以及电机数量是否正确。

bool AP_Motors6DOF::motor_is_enabled(int motor_number) {if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {return false;}return motor_enabled[motor_number];
}

set_reversed()：改变电机方向。

bool AP_Motors6DOF::set_reversed(int motor_number, bool reversed) {if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {return false;}if (reversed) {_motor_reverse[motor_number].set_and_save(-1);} else {_motor_reverse[motor_number].set_and_save(1);}return true;
}

好吧其实是我懒得写了，有机会再更新吧

三、参考资料

Ardusub官方手册
Ardupilot源码

2020/10/28更新：增加了部分解释以及推力分配内容

Ardusub源码解析学习（二）——电机库相关推荐

Ardusub源码解析学习（三）——车辆类型
APM_Sub源码解析学习(三)--车辆类型一.前言二.class AP_HAL::HAL 三.class AP_Vehicle 3.1 .h 3.2 .cpp 四.class Sub 4.1 . ...
Ardusub源码解析学习（一）——Ardusub主程序
APM_Sub源码解析学习(一)--Ardusub主程序前言一.准备工作二.Ardusub.cpp解析 2.1 scheduler table 2.2 scheduler get_schedul ...
APM_ArduCopter源码解析学习（三）——无人机类型
APM_ArduCopter源码解析学习(三)--无人机类型一.前言二.class AP_HAL::HAL 三.class AP_Vehicle 3.1 .h 3.2 .cpp 四.class C ...
Tomcat源码解析系列二：Tomcat总体架构
Tomcat即是一个HTTP服务器,也是一个servlet容器,主要目的就是包装servlet,并对请求响应相应的servlet,纯servlet的web应用似乎很好理解Tomcat是如何装载serv ...
STL源码剖析学习二：空间配置器（allocator）
STL源码剖析学习二:空间配置器(allocator) 标准接口: vlaue_type pointer const_pointer reference const_reference size_ty ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（二）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(二) 3. Joint optimization 3.1 Marginalization ...
PyCrypto密码学库源码解析（二）RSA参数生成
Python Crypto库源码解析(二) RSA参数生成 * 版权声明 * 引用请注明出处,转载请联系: h0.1c@foxmail.com 本文主要讲解pycrypto库中RSA参数生成的实现方法 ...
【vuejs深入三】vue源码解析之二 htmlParse解析器的实现
写在前面一个好的架构需要经过血与火的历练,一个好的工程师需要经过无数项目的摧残. 昨天博主分析了一下在vue中,最为基础核心的api,parse函数,它的作用是将vue的模板字符串转换成ast,从而 ...
【DETR源码解析】二、Backbone模块
目录前言一.Backbone整体结构一.CNN-Backbone 二.Positional Encoding Reference 前言最近在看DETR的源码,断断续续看了一星期左右,把主要的模 ...
谷歌BERT预训练源码解析（二）：模型构建
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明. 本文链接:https://blog.csdn.net/weixin_39470744/arti ...

Ardusub源码解析学习（二）——电机库