Ardusub源码解析学习（六）—— althold model
Ardusub源码解析学习（六）—— althold model
- althold_init()
- althold_run()
- control_depth()
althold model：深度保持模式，需要配合深度计使用。stabilize mode 在系列的第一篇博文里面已经论述过了，因此这部分博文有些相同的细节将不会展开讨论。
进入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();
}