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

althold_init()althold_run()control_depth()

---

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