Ardusub源码解析学习（五）——从manual model开始

---

从本篇开始，将会陆续对Ardusub中各种模式进行介绍，stabilize model在系列第一篇博文已经介绍过了（如果有时间考虑再重新写一篇？），这边就直接从手动模式开始介绍吧（因为算是最简单的了...）。

---

manual model：手动控制模式。Pixhawk烧写进Ardusub固件之后默认都是从这个模式进行启动。顾名思义，该模式下ROV的各种控制直接通过手柄实现，系统将不会自动帮你稳定机器人姿态。

manual_init()

// manual_init - initialise manual controller
bool Sub::manual_init()
{
    // set target altitude to zero for reporting
    pos_control.set_alt_target(0);

    // attitude hold inputs become thrust inputs in manual mode
    // set to neutral to prevent chaotic behavior (esp. roll/pitch)
    set_neutral_controls();

    return true;
}

一般来说，模式控制的程序分为init()和run()两部分，manual模式下的init()函数比较简单，跟stabilize 一样，首先是将期望高度归零。然后将姿态保持输入转变为推力输入，并设置为空挡以防止不必要的震荡和摆动。

如下所示为set_neutral_controls()内部程序。

void Sub::set_neutral_controls()
{
    uint32_t tnow = AP_HAL::millis();

    for (uint8_t i = 0; i < 6; i++) {
        RC_Channels::set_override(i, 1500, tnow);
    }

    // Clear pitch/roll trim settings
    pitchTrim = 0;
    rollTrim  = 0;
}

manual_run()

// manual_run - runs the manual (passthrough) controller
// should be called at 100hz or more
void Sub::manual_run()
{
    // if not armed set throttle to zero and exit immediately
    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        attitude_control.set_throttle_out(0,true,g.throttle_filt);
        attitude_control.relax_attitude_controllers();
        return;
    }

    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    motors.set_roll(channel_roll->norm_input());
    motors.set_pitch(channel_pitch->norm_input());
    motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P);
    motors.set_throttle(channel_throttle->norm_input());
    motors.set_forward(channel_forward->norm_input());
    motors.set_lateral(channel_lateral->norm_input());
}

前面部分和stabilize model一致（具体看这里），来说说后面的set_xxx()部分好了。注意该形式函数是对输入通道进行设置，分别为横滚、俯仰、偏航、沉浮、前后平移和左右平移6个通道，注意每个通道可以对应多个电机（具体看这里）。括号内输入的参数要求范围在-1~1之间。

那么重点来看一下内部的norm_input()。这个函数位于库中的RC_Channel.cpp中（注意RC_Channel.cpp用于控制单个通道，而RC_Channels.cpp用于批量控制多个通道）。

简单来说，这段程序首先判断是否将通道输入反向，并根据结果将值赋给reverse_mul 这个变量。然后对通道输入的值进行限幅和计算，并将输入值radio_in转换为-1~1之间的输出量，输出量偏离0值的大小表示希望尽快向某一方向进行运动。

float RC_Channel::norm_input() const
{
    float ret;
    int16_t reverse_mul = (reversed?-1:1);
    if (radio_in < radio_trim) {
        if (radio_min >= radio_trim) {
            return 0.0f;
        }
        ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
    } else {
        if (radio_max <= radio_trim) {
            return 0.0f;
        }
        ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim);
    }
    return constrain_float(ret, -1.0f, 1.0f);
}

在同一文件中的变量表中可以找到对应值

    // @Param: MIN
    // @DisplayName: RC min PWM
    // @Description: RC minimum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: PWM
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("MIN",  1, RC_Channel, radio_min, 1100),

    // @Param: TRIM
    // @DisplayName: RC trim PWM
    // @Description: RC trim (neutral) PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: PWM
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("TRIM", 2, RC_Channel, radio_trim, 1500),

    // @Param: MAX
    // @DisplayName: RC max PWM
    // @Description: RC maximum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: PWM
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("MAX",  3, RC_Channel, radio_max, 1900),

    // @Param: REVERSED
    // @DisplayName: RC reversed
    // @Description: Reverse channel input. Set to 0 for normal operation. Set to 1 to reverse this input channel.
    // @Values: 0:Normal,1:Reversed
    // @User: Advanced
    AP_GROUPINFO("REVERSED",  4, RC_Channel, reversed, 0),