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

一、RC输入与输出
- 1.1 RC Input
- 1.2 RC Output
二、电机库学习
三、参考资料

一、RC输入与输出

1.1 RC Input

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

    // primary input control channels
    RC_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。可选择配置如图所示
在这里插入图片描述

二、电机库学习

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

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

在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 motors
    for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
    
    
        remove_motor(i);
    }

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

    // Supported frame types
    typedef 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 Order
    case 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 masks
    add_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个相关的影响因子配置数组。

2.3 output_min()

// output_min - sends minimum values out to the motors
void AP_Motors6DOF::output_min()
{
    
    
    int8_t i;

    // set limits flags
    limit.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 1500
    for (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 motor

    switch (_spool_state) {
    
    
    case SpoolState::SHUT_DOWN:
        // sends minimum values out to the motors
        // set motor output based on thrust requests
        for (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 flying
        for (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 requests
        for (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 motor
    for (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 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, +/- 1.0
        float   forward_thrust;             // forward thrust input value, +/- 1.0
        float   lateral_thrust;             // lateral thrust input value, +/- 1.0

        roll_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 flags
        limit.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 throttle
        if (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 motor
        for (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 now
        for (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 motor
        for (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中，用来总合最后的电机输出控制量）。

其他函数

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源码

如需转载，请标明出处

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