Ardusubソースコードの分析と学習（6）-代替モデル
altholdモデル：深度保持モード。深度ゲージで使用する必要があります。安定化モードについては、シリーズの最初のブログ投稿ですでに説明されているため、ブログ投稿のこの部分での同じ詳細の一部については説明しません。
ここで説明するバージョンはマスターであり、安定バージョンの安定プランはSub4.0に基づいており、後で更新する時間があります。
ardupilot \ Ardusub \パスを入力し、control_althold.cppファイルを見つけます。内部ヘッダーファイルはSub.hで、内部実装にはalthold_init（）、althold_run（）、control_depth（）の3つの関数があり、順番に説明します。
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を直接絶対値として取得して返します。2つの値については、同じパスで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が等しいかどうかを比較し、等しくない場合は、2つのパラメーター間の正と負の関係を再確認します。 _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クラスの保護された部分の構造です。
// 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は、最後の方位（ヨー角）を取得します。
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ソースコードの分析と学習を比較すると（1）-Ardusubメインプログラムで説明されている安定化モデルでは、全体的なフレームワークは似ていますが、安定化に基づいて追加されたプログラムがいくつかあります。その一部を次に示します。
安定化された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角度の姿勢を制御します。
後者の部分は安定化モードの部分に似ています。つまり、姿勢コントローラーをオンにして、順方向チャネルと逆方向チャネルのモーター制御を設定します。最後にスロットルコントロールを外し、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