电机库源码分析 | Felix’s Micro Space

type

date

slug

一、预备知识

RC输入和输出

RC IN

在Sub.h（Sub.h中定义了最基本的车辆类型，即Sub类，内部包含用于控制水下无人车辆的最基本的变量和函数等。）中声明了最基本的用于运动控制的通道，共6个输入通道

这几个通道的意思，就是通过其中的通道输入来控制水下无人潜艇的某一个方向上的运动，它并不是指某一个特定的电机，通常一个通道影响着很多与它控制运动相关的电机。

channel_roll：横滚角

channel_pitch：俯仰角

channel_throttle：节气门

channel_yaw：航向角

channel_forward：前后平移

channel_lateral：左右平移

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

Channel	Meaning	ㅤ	ㅤ
1	Pitch	俯仰	Haltr2
2	Roll	横滚	Hautd1
3	Throttle	油门左上下	Ry
4	Yaw	偏航左左右	Rx
5	Forward	向前右上下	Y
6	Lateral	横向右左右	X
7	Camera Pan	ㅤ	ㅤ
8	Camera Tilt*	ㅤ	ㅤ
9	Lights 1 Level	ㅤ	ㅤ
10	Lights 2 Level	ㅤ	ㅤ
11	Video Switch	ㅤ	ㅤ

RC Out

Pixhawk2.4.8应该是属于1代的产品了，它的主输出通道有8个，辅助输出通道有6个。

其中8个MAIN OUT对应连接水下推进器，输出PWM波进行控制，更新频率为200Hz。6个AUX OUT同样也可以产生PWM控制信号，用来控制一些辅助器件如云台舵机、灯光亮度等（当然你也可以用MAIN OUT），更新频率为50Hz。

Pixhawk Label	Servo Channel
Main 1	1
Main 2	2
Main 3	3
Main 4	4
Main 5	5
Main 6	6
Main 7	7
Main 8	8
Aux 1	9
Aux 2	10
Aux 3	11
Aux 4	12
Aux 5	13
Aux 6	14

注意1：舵机不要连接到MAIN OUT 端口，过高的PWM频率可能会烧损舵机。

注意2：6个辅助 IO 口可被配置为通用IO口或PWM输出。缺省，通道5和6默认保留当做普通GPIO使用，用于控制继电器。可以通过配置BRD_PWM_COUNT 确定有多少辅助输出口。

BRD_PWM_COUNT	Aux pin configuration	ㅤ	ㅤ	ㅤ	ㅤ	ㅤ
ㅤ	1	2	3	4	5	6
0	GPIO	GPIO	GPIO	GPIO	GPIO	GPIO
2	PWM	PWM	GPIO	GPIO	GPIO	GPIO
(default) 4	PWM	PWM	PWM	PWM	GPIO	GPIO
6	PWM	PWM	PWM	PWM	PWM	PWM

注意3：伺服电机可通过MavLink DO_SET_SERVO 命令控制，继电器可通过DO_SET_RELAY命令控制。

二、电机库学习

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

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

进入这个文件之后就可以很清楚看到注释如下，指明该类为 ROV 专用电机控制函数。

当然一些最基本的函数都是直接从父类那边继承过来了，这个类里面只是实现了一些针对于ROV的专用函数。

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

2.1 setup_motors

配置电机，包括每个电机对于不同运动的影响程度（推力分配）

首先就是把最初的所有电机配置全部移除，方便后续进行更改。

进入switch-case分支结构配置具体内容，首先判断 ROV架构，Ardusub这边给出了8种配置结构，定义于 AP_Motors6DOF.h 中的枚举类型里。

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

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

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

2.2 add_motor_raw_6dof()

输入参数

motor_num：电机编号
roll_fac：横滚因子
pitch_fac：俯仰因子
yaw_fac：航向因子
throttle_fac：浮沉因子
forward_fac：前后平移
lat_fac：左右平移
testing_order：

ROV调用空中无人机的RPY方向配置方法，并且增加三个参数throttle_fac、forward_fac、lat_fac。

2.3 output_min()

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

2.4 calc_thrust_to_pwm()

将 -1~1 的 thrust_in 输入转换为 pwm 输出。1500为中间位置，range_up 和 range_down 为正向 1和反向 -1 对应的比例。

2.5 output_to_motors()

最开始的 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 output_armed_stabilizing()

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

函数进来，就先定义各个运动方向上作用的推进器（这里并不是指单独的实体推进器，而是能够在某一运动方向作用的抽象概念的推进器总合）。

然后，RPY 方向上的推进器通过姿态控制器获取期望的姿态值并且与前馈控制的调整值叠加；沉浮方向上的推进器则是通过get_throttle_bidirectional()函数获取滤波之后的输入控制量；平移方向上的推进器从set_xxx方法中获取最新的平移方向上的输入指令。以上控制量范围为-1~1。

建立两个数组分别用来保存RPY方向和平移方向上的每个电机的控制量。并且通过limit设定各个运动方向并未达到限制。然后对沉浮方向的推进器控制量进行限幅，如果达到限定值，则将对应的limit值改为true。

对于每一个电机，综合各个方向的控制量信号以及前设的运动影响因子，得出其最后的控制信号（包括RPY方向和平移方向全部的电机）。

最后，判断每一个电机是否已经启用，如果启用，那么就将旋转和平移量综合，乘以方向系数（_motor_reverse[i]在文件开头已被定义，本文中未放出，一般为1，表正向），然后储存到_thrust_rpyt_out数组之中（这个数组被定义在AP_MotorsMatrix.h中，用来总合最后的电机输出控制量）。