Ardusub源码解析学习（六）—— althold model

---

althold model：深度保持模式，需要配合深度计使用。stabilize mode 在系列的第一篇博文里面已经论述过了，因此这部分博文有些相同的细节将不会展开讨论。

---

注意此处讲解版本为master，关于稳定版的stable计划以Sub4.0为主进行，后续有时间更新。

进入ardupilot\Ardusub\路径下，找到 control_althold.cpp 文件，内部头文件为 Sub.h，内部实现有3个函数：althold_init()、althold_run()和control_depth()，接下来依次进行讲解。

althold_init()

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

    // initialize vertical speeds and leash lengths
    // sets the maximum speed up and down returned by position controller
    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
    pos_control.set_max_accel_z(g.pilot_accel_z);

    // initialise position and desired velocity
    pos_control.set_alt_target(inertial_nav.get_altitude());
    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());

    last_pilot_heading = ahrs.yaw_sensor;

    return true;
}

control_check_barometer()这个函数的作用就是检查是否有深度计存在，如下所示，如果飞控板不处于仿真状态，那么将会检查深度计是否存在，如果不存在则会通过 gcs().send_text()这个函数向地面站发送“没有连接深度计”的警告信息。如果存在，则判断传感器的健康状态，如果不健康，则通过相同方法发送“深度计出错”的警告信息。否则，确认深度计存在并且正确。

bool Sub::control_check_barometer()
{
    
    
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
    if (!ap.depth_sensor_present) {
    
     // can't hold depth without a depth sensor
        gcs().send_text(MAV_SEVERITY_WARNING, "Depth sensor is not connected.");
        return false;
    } else if (failsafe.sensor_health) {
    
    
        gcs().send_text(MAV_SEVERITY_WARNING, "Depth sensor error.");
        return false;
    }
#endif
    return true;
}

然后是 pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up) 这段程序，首先看括号内的参数部分：

// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
uint16_t Sub::get_pilot_speed_dn()
{
    
    
    if (g.pilot_speed_dn == 0) {
    
    
        return abs(g.pilot_speed_up);
    }
    return abs(g.pilot_speed_dn);
}

get_pilot_speed_dn()实现于Ardusub.cpp中，内部函数首先判断 g.pilot_speed_dn 是否等于0，是的话那么就将 g.pilot_speed_up 取绝对值返回，不是的话则直接将 g.pilot_speed_dn 取绝对值并且返回。关于两个值，可以查看同路径下的 Parameters.cpp 文件。

    // @Param: PILOT_SPEED_UP
    // @DisplayName: Pilot maximum vertical ascending speed
    // @Description: The maximum vertical ascending velocity the pilot may request in cm/s
    // @Units: cm/s
    // @Range: 50 500
    // @Increment: 10
    // @User: Standard
    GSCALAR(pilot_speed_up,     "PILOT_SPEED_UP",   PILOT_VELZ_MAX),	// PILOT_VELZ_MAX = 500, 单位cm/s

    // @Param: PILOT_SPEED_DN
    // @DisplayName: Pilot maximum vertical descending speed
    // @Description: The maximum vertical descending velocity the pilot may request in cm/s
    // @Units: cm/s
    // @Range: 50 500
    // @Increment: 10
    // @User: Standard
    GSCALAR(pilot_speed_dn,     "PILOT_SPEED_DN",   0),

    // @Param: PILOT_ACCEL_Z
    // @DisplayName: Pilot vertical acceleration
    // @Description: The vertical acceleration used when pilot is controlling the altitude
    // @Units: cm/s/s
    // @Range: 50 500
    // @Increment: 10
    // @User: Standard
    GSCALAR(pilot_accel_z,  "PILOT_ACCEL_Z",    PILOT_ACCEL_Z_DEFAULT),	// PILOT_VELZ_MAX = 100, 单位cm/s/s

回到源程序中的set_max_speed_z()。该函数接收了-get_pilot_speed_dn() 和 g.pilot_speed_up，可知分别为-500和+500。那么进入这个函数之后，首先确保speed_down始终为负数，然后比较_speed_down_cms（默认250.0f）、_speed_up_cms（默认100.0f）和speed_down、speed_up是否相等，不相等的话再次确认两个参数的正负关系，然后将值赋给_speed_down_cms和_speed_up_cms，并且将_flags中的对应位置1。然后才能使用calc_leash_length_z()在z轴速度或加速度发生变化时根据 update_z_controller 提供的最大速度，加速度计算出垂直方向的刹车长度，计算完成之后将_flags.recalc_leash_z重新置位为false。

/// set_max_speed_z - set the maximum climb and descent rates
/// To-Do: call this in the main code as part of flight mode initialisation
void AC_PosControl::set_max_speed_z(float speed_down, float speed_up)
{
    
    
    // ensure speed_down is always negative
    speed_down = -fabsf(speed_down);

    // exit immediately if no change in speed up or down
    if (is_equal(_speed_down_cms, speed_down) && is_equal(_speed_up_cms, speed_up)) {
    
    
        return;
    }

    // sanity check speeds and update
    if (is_positive(speed_up) && is_negative(speed_down)) {
    
    
        _speed_down_cms = speed_down;
        _speed_up_cms = speed_up;
        _flags.recalc_leash_z = true;
        calc_leash_length_z();
    }
}

_flags为AC_PosControl类中protected部分的结构体：

    // general purpose flags
    struct poscontrol_flags {
    
    
            uint16_t recalc_leash_z     : 1;    // 1 if we should recalculate the z axis leash length
            uint16_t recalc_leash_xy    : 1;    // 1 if we should recalculate the xy axis leash length
            uint16_t reset_desired_vel_to_pos   : 1;    // 1 if we should reset the rate_to_accel_xy step
            uint16_t reset_accel_to_lean_xy     : 1;    // 1 if we should reset the accel to lean angle step
            uint16_t reset_rate_to_accel_z      : 1;    // 1 if we should reset the rate_to_accel_z step
            uint16_t freeze_ff_z        : 1;    // 1 used to freeze velocity to accel feed forward for one iteration
            uint16_t use_desvel_ff_z    : 1;    // 1 to use z-axis desired velocity as feed forward into velocity step
            uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep
    } _flags;

pos_control.set_max_accel_z(g.pilot_accel_z)函数实现方式与上面类似，这边便不再赘述。

以下部分程序与系列第一篇博文讲述内容相似。set_alt_target()设置从惯性导航获取的最新的高度信息设置为当前期望值，同理下方的z轴当前期望爬升速率（cm/s），last_pilot_heading 获取最后的机头朝向(yaw角)。

    pos_control.set_alt_target(inertial_nav.get_altitude());
    pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());

    last_pilot_heading = ahrs.yaw_sensor;

althold_run()

// 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_z(-get_pilot_speed_dn(), g.pilot_speed_up);
    pos_control.set_max_accel_z(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_alt_hold_controllers(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());
}

对比Ardusub源码解析学习（一）——Ardusub主程序中所讲的stabilize model，可以看出总体框架是相似的，只不过是在stabilize的基础上添加了一些程序，这里挑部分讲讲。

注意到，相当于在stabilize的motors.set_desired_spool_state()和get_pilot_desired_lean_angles()添加了下述程序段。

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

这段程序用来判断是否存在GPS位置系统，并启用无需位置系统的姿态和深度控制。并将mavlink包中的四元数转换为以度数表示的欧拉角形式进行RPY角的姿态控制。

后面部分同stabilize模式部分类似，即开启姿态控制器并且设置前后向通道的电机控制。只不过在最后面去掉了油门控制并添加了control_depth()这个函数，接下来我们来详细解析一下其中的程序。

control_depth()

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_alt_hold_controllers();
        engageStopZ = true;
        lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z());
    } else {
    
     // hold z

        if (ap.at_bottom) {
    
    
            pos_control.relax_alt_hold_controllers(); // clear velocity and position targets
            pos_control.set_alt_target(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.relax_alt_hold_controllers();
        }

        pos_control.update_z_controller();
    }
}

深度控制流程如下：

• 首先保持实际位置直到检测到零导数，并获取最后的用户速度方向以检查零导数点，具体来说就是将engageStopZ 标志位置为true，把lastVelocityZWasNegative 置为false；
• 然后判断用于控制沉浮的油门输入通道的输入量是否达到了5%；
• 如果超过了5%，那么判断为进行z方向的深度控制，通过attitude_control.set_throttle_out()函数获取通道油门输入并进行设置，然后用pos_control.relax_alt_hold_controllers()把z轴方向上的期望和目标参数重置为当前值，然后置位对应的标志位；
• 如果没有超过5%，那么判断当前输入无效，维持当前深度状态：
  • 首先判断水下车辆是否已经触底，如果已经触底，清除并重置z轴上的期望值，并且重新设置目标姿态位置为距离底部10cm处；
  • 如果没有触底，检测零导数，检测到时，将z轴方向上的期望和目标参数重置为当前值，这样可以避免与操纵杆延迟或较小的输入信号有关的任何问题；
• 最后，更新并运行z轴方向的位置控制器。

如下为relax_alt_hold_controllers()内部程序：

/// relax_alt_hold_controllers - set all desired and targets to measured
void AC_PosControl_Sub::relax_alt_hold_controllers()
{
    
    
    _pos_target.z = _inav.get_altitude();
    _vel_desired.z = 0.0f;
    _flags.use_desvel_ff_z = false;
    _vel_target.z = _inav.get_velocity_z();
    _vel_last.z = _inav.get_velocity_z();
    _accel_desired.z = 0.0f;
    _accel_last_z_cms = 0.0f;
    _flags.reset_rate_to_accel_z = true;
    _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
    _pid_accel_z.reset_filter();
}

z轴位置控制器程序：

/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
    
    
    // check time since last cast
    const uint64_t now_us = AP_HAL::micros64();
    if (now_us - _last_update_z_us > POSCONTROL_ACTIVE_TIMEOUT_US) {
    
    
        _flags.reset_rate_to_accel_z = true;
        _pid_accel_z.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f);
        _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
        _pid_accel_z.reset_filter();
    }
    _last_update_z_us = now_us;

    // check for ekf altitude reset
    check_for_ekf_z_reset();

    // check if leash lengths need to be recalculated
    calc_leash_length_z();

    // call z-axis position controller
    run_z_controller();
}

后续有时间再更新点东西
第一次更新：2020/10/11