当前位置：首页 > article >正文

Ardusub源码剖析——control_manual.cpp

article 2024/12/5 3:35:10

代码

#include "Sub.h"

// manual_init - initialise manual controller
bool Sub::manual_init()
{
    // set target altitude to zero for reporting
    pos_control.set_pos_target_z_cm(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;
}

// 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());
}

剖析

Sub::manual_init()

#include "Sub.h"

代码包含 Sub 类的定义，以便在这个源文件中使用 Sub 类及其成员函数和成员变量。

// manual_init - initialise manual controller
bool Sub::manual_init()
{

manual_init 函数声明。它是一个成员函数，属于 Sub 类，不接受任何参数，并返回一个布尔值（bool）。返回值用来指示初始化是否成功，在这个例子中，函数总是返回 true。

    // set target altitude to zero for reporting
    pos_control.set_pos_target_z_cm(0);

设置位置控制系统的目标高度为0厘米。

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

在手动模式下，原本用于姿态保持的输入现在变成了推力输入。为了避免在手动控制模式下出现混乱的行为（特别是横滚和俯仰），需要将控制输入设置为中性。调用 set_neutral_controls 函数，它负责将所有控制输入设置为中性。这意味着没有横滚、俯仰或偏航的输入，从而确保潜水器在手动控制开始时保持稳定，不会出现突然的动作。

    return true;

返回 true，表示手动控制初始化成功。

Sub::manual_run()

// manual_run - runs the manual (passthrough) controller
// should be called at 100hz or more
void Sub::manual_run()
{

这是 manual_run 函数的声明和注释。注释说明了这个函数的作用是运行手动控制器，并且建议该函数应该以至少100Hz的频率被调用。

    // 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;
    }

这段代码检查潜水器的电机是否已经启动（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();: 放松姿态控制器，在电机未启动时防止不必要的控制动作。

    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);

这些行设置了潜水器的横滚（roll）、俯仰（pitch）和偏航（yaw）输入。输入值来自于对应的通道（channel），并且已经被标准化（normalized）处理，意味着它们是介于-1到1之间的值。

channel_roll->norm_input(): 获取标准化的横滚输入。
channel_pitch->norm_input(): 获取标准化的俯仰输入。
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());