Ardusub源码剖析——control_althold.cpp

代码

#include "Sub.h"


/*
 * control_althold.pde - init and run calls for althold, flight mode
 */

// althold_init - initialise althold controller
bool Sub::althold_init()
{
    if(!control_check_barometer()) {
        return false;
    }

    // initialize vertical maximum speeds and acceleration
    // sets the maximum speed up and down returned by position controller
    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
    pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

    // initialise position and desired velocity
    pos_control.init_z_controller();

    last_pilot_heading = ahrs.yaw_sensor;

    return true;
}

// althold_run - runs the althold controller
// should be called at 100hz or more
void Sub::althold_run()
{
    uint32_t tnow = AP_HAL::millis();

    // initialize vertical speeds and acceleration
    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
        attitude_control.set_throttle_out(0,true,g.throttle_filt);
        attitude_control.relax_attitude_controllers();
        pos_control.relax_z_controller(motors.get_throttle_hover());
        last_pilot_heading = ahrs.yaw_sensor;
        return;
    }

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

    // get pilot desired lean angles
    float target_roll, target_pitch;

    // Check if set_attitude_target_no_gps is valid
    if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
        float target_yaw;
        Quaternion(
            set_attitude_target_no_gps.packet.q
        ).to_euler(
            target_roll,
            target_pitch,
            target_yaw
        );
        target_roll = degrees(target_roll);
        target_pitch = degrees(target_pitch);
        target_yaw = degrees(target_yaw);

        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
        return;
    }

    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());

    // get pilot's desired yaw rate
    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

    // call attitude controller
    if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
        last_pilot_heading = ahrs.yaw_sensor;
        last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading

    } else { // hold current heading

        // this check is required to prevent bounce back after very fast yaw maneuvers
        // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
        if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
            target_yaw_rate = 0; // Stop rotation on yaw axis

            // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
            attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
            last_pilot_heading = ahrs.yaw_sensor; // update heading to hold

        } else { // call attitude controller holding absolute absolute bearing
            attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
        }
    }

    control_depth();

    motors.set_forward(channel_forward->norm_input());
    motors.set_lateral(channel_lateral->norm_input());
}

void Sub::control_depth() {
    // Hold actual position until zero derivative is detected
    static bool engageStopZ = true;
    // Get last user velocity direction to check for zero derivative points
    static bool lastVelocityZWasNegative = false;
    if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5%
        // output pilot's throttle
        attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
        // reset z targets to current values
        pos_control.relax_z_controller(channel_throttle->norm_input());
        engageStopZ = true;
        lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z());
    } else { // hold z

        if (ap.at_bottom) {
            pos_control.init_z_controller();
            pos_control.set_pos_target_z_cm(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
        }

        // Detects a zero derivative
        // When detected, move the altitude set point to the actual position
        // This will avoid any problem related to joystick delays
        // or smaller input signals
        if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) {
            engageStopZ = false;
            pos_control.init_z_controller();
        }

        pos_control.update_z_controller();
    }
}

剖析

Sub::althold_init()

#include "Sub.h"

包含 Sub 类的定义，以便在这个源文件中使用 Sub 类及其成员。

// althold_init - initialise althold controller
bool Sub::althold_init()
{

这是 althold_init 函数的声明。它不接受任何参数，返回一个布尔值，表示初始化是否成功。

    if(!control_check_barometer()) {
        return false;
    }

这行代码检查气压计是否正常工作。control_check_barometer 用于验证气压计的读数是否有效。如果气压计检查失败，则初始化过程无法继续，函数返回 false。

    // initialize vertical maximum speeds and acceleration
    // sets the maximum speed up and down returned by position controller
    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
    pos_control.set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

这些行设置了垂直方向上的最大速度和加速度。它们定义了位置控制器可以返回的最大上升和下降速度，以及用于纠正位置的速度和加速度。

• pos_control.set_max_speed_accel_z(): 设置最大速度和加速度。
• get_pilot_speed_dn(): 获取飞行员设定的下降速度。
• g.pilot_speed_up: 飞行员设定的上升速度。
• g.pilot_accel_z: 飞行员设定的垂直加速度。

    // initialise position and desired velocity
    pos_control.init_z_controller();

初始化垂直位置控制器，设置当前位置和期望的速度。这确保了潜水器在进入高度保持模式时能够从当前状态平滑过渡。

    last_pilot_heading = ahrs.yaw_sensor;

记录当前的航向（yaw）作为最后飞行员指定的航向。ahrs 是一个姿态和航向参考系统（Attitude and Heading Reference System）对象，yaw_sensor 是其航向传感器的读数。

    return true;

如果所有的初始化步骤都成功完成，则返回 true。

Sub::althold_run()

// althold_run - runs the althold controller
// should be called at 100hz or more
void Sub::althold_run()
{

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

    uint32_t tnow = AP_HAL::millis();

获取当前系统时间（以毫秒为单位）。tnow 变量存储当前时间

    // initialize vertical speeds and acceleration
    pos_control.set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);

设置垂直方向上的最大速度和加速度。这些值决定了潜水器在垂直方向上响应指令的速度和加速度。

• get_pilot_speed_dn(): 获取飞行员设定的下降速度。
• g.pilot_speed_up: 全局变量，代表飞行员设定的上升速度。
• g.pilot_accel_z: 全局变量，代表飞行员设定的垂直加速度。

    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);

如果电机未启动（armed），则将电机设置为地面空闲状态。

        // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
        attitude_control.set_throttle_out(0,true,g.throttle_filt);
        attitude_control.relax_attitude_controllers();
        pos_control.relax_z_controller(motors.get_throttle_hover());

如果潜水器未启动，它不会稳定横滚（roll）、俯仰（pitch）和偏航（yaw）。因此，以下操作会被执行：

• 设置油门输出为0，并应用滤波器。
• 放松姿态控制器，以防止不必要的控制动作。
• 放松垂直位置控制器，并将其设置到悬停油门值。

        last_pilot_heading = ahrs.yaw_sensor;

记录当前的航向作为最后飞行员指定的航向。

        return;
    }

如果电机未启动，则退出函数。

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

如果电机已启动，则设置电机为油门无限制状态，这意味着潜水器可以根据控制输入全速运行。

// get pilot desired lean angles
float target_roll, target_pitch;

声明两个浮点变量 target_roll 和 target_pitch，用于存储飞行员期望的横滚和俯仰角度。

// Check if set_attitude_target_no_gps is valid
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {

检查 set_attitude_target_no_gps 消息是否在最近5秒内收到。set_attitude_target_no_gps 是一个结构体，包含一个四元数（quaternion）用于表示姿态和一个时间戳（last_message_ms）。如果该消息是有效的（即在5秒之内收到的），则执行以下代码块。

    float target_yaw;
    Quaternion(
        set_attitude_target_no_gps.packet.q
    ).to_euler(
        target_roll,
        target_pitch,
        target_yaw
    );

使用 Quaternion 类将收到的四元数转换为欧拉角（Euler angles），即横滚（roll）、俯仰（pitch）和偏航（yaw）角度。这些角度存储在 target_roll、target_pitch 和 target_yaw 变量中。

    target_roll = degrees(target_roll);
    target_pitch = degrees(target_pitch);
    target_yaw = degrees(target_yaw);

将欧拉角从弧度转换为度。degrees() 函数是用来执行这个转换的。

    attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
    return;

将转换后的角度（乘以100，转换成期望的单位）输入到姿态控制器中，并立即返回，结束 althold_run 函数的执行。true 参数表示这是一个直接设置姿态的请求，而不是一个增量变化。

get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());

如果 set_attitude_target_no_gps 消息不是在最近5秒内收到的，则调用 get_pilot_desired_lean_angles 函数来获取飞行员的期望倾斜角度。这个函数基于遥控器的输入来计算期望的横滚和俯仰角度。

• channel_roll->get_control_in(): 获取遥控器横滚通道的输入值。
• channel_pitch->get_control_in(): 获取遥控器俯仰通道的输入值。
• attitude_control.get_althold_lean_angle_max(): 获取高度保持模式下的最大倾斜角度。

// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

获取飞行员期望的偏航速率（围绕垂直轴的旋转速率）。get_pilot_desired_yaw_rate 函数接收来自遥控器偏航通道的输入值，并返回期望的偏航速率。

// call attitude controller
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
    attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
    last_pilot_heading = ahrs.yaw_sensor;
    last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading

如果期望的偏航速率不为零（即飞行员正在请求改变朝向），则调用姿态控制器，使用飞行员指定的横滚、俯仰和偏航速率。同时记录当前航向作为最后飞行员指定的航向，并更新最后飞行员偏航输入的时间戳。

} else { // hold current heading

如果期望的偏航速率为零（即飞行员没有请求改变朝向），则尝试保持当前的朝向。

    // this check is required to prevent bounce back after very fast yaw maneuvers
    // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
    if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
        target_yaw_rate = 0; // Stop rotation on yaw axis

        // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
        last_pilot_heading = ahrs.yaw_sensor; // update heading to hold

如果自上次飞行员请求偏航以来不到250毫秒，则设置偏航速率为零以停止偏航轴的旋转，并调用姿态控制器来减速偏航轴。同时更新最后飞行员指定的航向。

    } else { // call attitude controller holding absolute heading
        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
    }

如果自上次飞行员请求偏航以来已经超过250毫秒，则调用姿态控制器以保持绝对航向（即最后飞行员指定的航向）。

control_depth();

调用 control_depth 函数来控制潜水器的深度

motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input());

设置潜水器的前进和侧向动力。channel_forward->norm_input() 和 channel_lateral->norm_input() 分别获取飞行员指定的前进和侧向通道的标准化输入值，并使用这些值来设置电机的推力方向。

Sub::control_depth()

void Sub::control_depth() {

定义 Sub 类的成员函数 control_depth，没有返回值。

    // Hold actual position until zero derivative is detected
    static bool engageStopZ = true;

声明一个静态布尔变量 engageStopZ，用于确定是否激活深度保持。当检测到速度的零导数时，它将保持实际位置。

    // Get last user velocity direction to check for zero derivative points
    static bool lastVelocityZWasNegative = false;

声明一个静态布尔变量 lastVelocityZWasNegative，用于记录用户上一次的垂直速度方向（正或负），以检测速度的零导数点。

    if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5%

检查遥控器油门通道的标准化输入是否大于5%（即飞行员是否在请求垂直移动）。norm_input() 返回一个介于0和1之间的值，其中0.5代表中立位置。如果输入与0.5的差值的绝对值大于0.05，表示油门输入超过了5%。

        // output pilot's throttle
        attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);

如果飞行员请求垂直移动，则将飞行员的油门输入输出到姿态控制器。set_throttle_out 函数用于设置油门输出，g.throttle_filt 是一个滤波参数。

        // reset z targets to current values
        pos_control.relax_z_controller(channel_throttle->norm_input());

重置Z轴的目标位置到当前值，这是为了在新的油门输入下重新开始深度控制。

        engageStopZ = true;
        lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z());
    } else { // hold z

如果油门输入低于5%，则进入保持深度的模式。同时更新 engageStopZ 和 lastVelocityZWasNegative 变量。

        if (ap.at_bottom) {
            pos_control.init_z_controller();
            pos_control.set_pos_target_z_cm(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
        }

如果潜水器到达了底部，初始化Z轴控制器，并将目标深度设置为当前高度加上10厘米，以保持潜水器在底部上方10厘米的位置。

        // Detects a zero derivative
        // When detected, move the altitude set point to the actual position
        // This will avoid any problem related to joystick delays
        // or smaller input signals
        if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) {
            engageStopZ = false;
            pos_control.init_z_controller();
        }

检测速度的零导数。如果检测到（即速度从正变为负或从负变为正），则将 engageStopZ 设置为 false 并重新初始化Z轴控制器，将目标深度设置为当前深度。

        pos_control.update_z_controller();
    }

无论是否检测到零导数，都会更新Z轴控制器，以维持或改变潜水器的深度。