ArduSub程序学习（10）--位置控制和导航

标签：10 target -- auto accel pos ArduSub wp vel

参考图：

1.追溯起源

1.1设置home点

Ardusub.cpp中的update_home_from_EKF();函数

//更新无人机 Home 点（起始点）的位置的函数
void Sub::update_home_from_EKF()
{
    // exit immediately if home already set
    //检查 Home 点是否已经设置
    if (ahrs.home_is_set()) {
        return;//如果 Home 点已经设置，函数立即返回，不做任何操作。
    }

    // special logic if home is set in-flight
    //检查飞控的马达是否已经解锁
    if (motors.armed()) {
        set_home_to_current_location_inflight();
    } else {
        // move home to current ekf location (this will set home_state to HOME_SET)
        //如果未解锁马达，将 Home 点设置为当前 EKF 估计的位置。
        if (!set_home_to_current_location(false)) {
            // ignore this failure
        }
    }
}

主要功能：该函数的主要功能是根据当前飞控状态（是否飞行）和 EKF 提供的位置数据动态设置或更新 Home 点。
飞行中 vs 地面：在飞行中，Home 点设置有特殊的逻辑；而在地面时，Home 点直接使用 EKF 估计的位置。
错误处理：如果设置 Home 点失败，不会抛出错误或中断操作，而是简单地忽略这一错误。

1.2加速度信息转换为对应的姿态角度

accel_to_lean_angles 是一个用于将加速度转换为俯仰角（pitch）和横滚角（roll）的函数。

位于libraries/AC_AttitudeControl/AC_PosControl.cpp

//加速度信息（沿 X 和 Y 轴的加速度）转换为对应的姿态角度，以便姿态控制器可以调整无人机的俯仰和横滚，保持稳定飞行。
void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
    // rotate accelerations into body forward-right frame
    const float accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
    const float accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw();

    // update angle targets that will be passed to stabilize controller
    pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
    float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
    roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
}

功能：将无人机的前向和右向加速度转换为相应的俯仰和横滚角度，便于姿态控制器调整姿态。
坐标转换：首先将加速度从全球坐标系转换到机体坐标系，然后根据转换后的加速度计算姿态角。
单位转换：输出的目标角度是厘度，适合飞控系统的控制算法使用。

1.3PID

位于 libraries/AC_PID/AC_PID_2D.cpp

Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit)
{
    // don't process inf or NaN
    if (target.is_nan() || target.is_inf() ||
        measurement.is_nan() || measurement.is_inf()) {
        return Vector2f{};
    }

    _target = target;

    // reset input filter to value received
    if (_reset_filter) {
        _reset_filter = false;
        _error = _target - measurement;
        _derivative.zero();
    } else {
        Vector2f error_last{_error};
        _error += ((_target - measurement) - _error) * get_filt_E_alpha();

        // calculate and filter derivative
        if (_dt > 0.0f) {
            const Vector2f derivative{(_error - error_last) / _dt};
            _derivative += (derivative - _derivative) * get_filt_D_alpha();
        }
    }

    // update I term
    update_i(limit);

    _pid_info_x.target = _target.x;
    _pid_info_x.actual = measurement.x;
    _pid_info_x.error = _error.x;
    _pid_info_x.P = _error.x * _kp;
    _pid_info_x.I = _integrator.x;
    _pid_info_x.D = _derivative.x * _kd;
    _pid_info_x.FF = _target.x * _kff;

    _pid_info_y.target = _target.y;
    _pid_info_y.actual = measurement.y;
    _pid_info_y.error = _error.y;
    _pid_info_y.P = _error.y * _kp;
    _pid_info_y.I = _integrator.y;
    _pid_info_y.D = _derivative.y * _kd;
    _pid_info_y.FF = _target.y * _kff;
//二维 PID 控制器
    return _error * _kp + _integrator + _derivative * _kd + _target * _kff;
}

控制逻辑：PID 控制器利用误差的比例、积分和微分值来计算合适的输出，调整系统的状态以尽量减少误差。
X 和 Y 轴独立控制：每个轴都有各自的 PID 计算过程，但总体的控制目标是协同实现对整个系统的位置控制。

2.自动模式水平位置控制实现逻辑

2.1判断飞行模式

ardusub.cpp中的 update_flight_mode();函数

void Sub::update_flight_mode()
{
    switch (control_mode) {
    case ACRO:
        acro_run();
        break;

    case STABILIZE:
        stabilize_run();
        break;

    case ALT_HOLD:
        althold_run();
        break;

    case AUTO:
        auto_run();
        break;

    case CIRCLE:
        circle_run();
        break;

    case GUIDED:
        guided_run();
        break;

    case SURFACE:
        surface_run();
        break;

#if POSHOLD_ENABLED == ENABLED
    case POSHOLD:
        poshold_run();
        break;
#endif

    case MANUAL:
        manual_run();
        break;

    case MOTOR_DETECT:
        motordetect_run();
        break;

    default:
        break;
    }
}

2.2auto_run();

判断为自动模式AUTO后运行auto_run();函数。

//auto_run 函数根据当前自动模式 (auto_mode) 选择合适的自动控制器函数来运行。
void Sub::auto_run()
{
    //更新当前任务状态，可能包括检查任务进度、更新导航状态等。
    mission.update();

    // call the correct auto controller
    switch (auto_mode) {
    
    case Auto_WP:
    case Auto_CircleMoveToEdge:
        auto_wp_run();//这个函数一般用于航点导航控制，让无人机或机器人飞向预定的航点或边缘位置。
        break;

    case Auto_Circle:
        auto_circle_run();//让无人机做圆周运动。
        break;

    case Auto_NavGuided:
#if NAV_GUIDED == ENABLED
        auto_nav_guided_run();//用于自主导航（Guided Mode），但只有在 NAV_GUIDED 编译选项启用时才会执行。
#endif
        break;

    case Auto_Loiter:
        auto_loiter_run();//此模式用于悬停，保持当前位置。
        break;

    case Auto_TerrainRecover:
        auto_terrain_recover_run();//用于地形恢复，可能用于在复杂地形或意外情况下的自动调整
        break;
    }
}

// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Sub::auto_wp_start(const Vector3f& destination)
{
    auto_mode = Auto_WP;//将 auto_mode 设置为 Auto_WP，表示进入航点模式。

    // initialise wpnav (no need to check return status because terrain data is not used)
    //调用 wp_nav.set_wp_destination(destination, false)，设置航点导航的目标点 destination，第二个参数 false 表示不使用地形数据。
    wp_nav.set_wp_destination(destination, false);

    // initialise yaw
    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
    if (auto_yaw_mode != AUTO_YAW_ROI) {
        set_auto_yaw_mode(get_default_auto_yaw_mode(false));
    }
}

auto_run()：根据当前模式 auto_mode 选择适当的自动控制器，确保无人机或机器人在不同模式下执行相应的行为。
auto_wp_start()：初始化航点控制器，设置目标位置，并根据需要设置航向模式。

2.3 航点导航控制auto_wp_run()

auto_wp_run 函数用于无人机或机器人在航点模式（Auto_WP）下的导航控制。这是飞控系统中的一个核心函数，负责根据航点导航任务调整姿态、速度和位置，确保飞行器安全稳定地向目标航点移动。

void Sub::auto_wp_run()
{
    // if not armed set throttle to zero and exit immediately
    //如果没有武装设置油门为零并立即退出
    if (!motors.armed()) {
        // To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
        //    (of course it would be better if people just used take-off)
        // call attitude controller
        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        attitude_control.set_throttle_out(0,true,g.throttle_filt);
        attitude_control.relax_attitude_controllers();
        return;
    }

    // process pilot's yaw input
    //检查是否存在飞行员输入失效保护
    float target_yaw_rate = 0;
    if (!failsafe.pilot_input) {
        // get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz()) * 100;
        if (!is_zero(target_yaw_rate)) {
            set_auto_yaw_mode(AUTO_YAW_HOLD);
        }
    }

    // set motors to full range
    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    // run waypoint controller
    // TODO logic for terrain tracking target going below fence limit
    // TODO implement waypoint radius individually for each waypoint based on cmd.p2
    // TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter
    failsafe_terrain_set_status(wp_nav.update_wpnav());//运行航点控制器

    ///
    // update xy outputs //

    float lateral_out, forward_out;
    //调用 translate_wpnav_rp 将航点导航的输出转换为侧向（lateral）和前向（forward）输出。
    translate_wpnav_rp(lateral_out, forward_out);

    // Send to forward/lateral outputs
    //通过 motors.set_lateral 和 motors.set_forward 将这些控制输出发送给电机，用于调整无人机的水平移动。
    motors.set_lateral(lateral_out);
    motors.set_forward(forward_out);

    // WP_Nav has set the vertical position control targets
    // run the vertical position controller and set output throttle
    //垂直位置控制器已经由航点导航设置了目标，调用 pos_control.update_z_controller() 更新垂直控制，调整油门输出以控制高度。
    pos_control.update_z_controller();

    
    // update attitude output //

    // get pilot desired lean angles
    float target_roll, target_pitch;
    //获取飞行员期望的横滚（roll）和俯仰（pitch）角度，并将其限制在允许的最大倾角范围内。
    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);

    // call attitude controller
    //调用姿态控制器
    if (auto_yaw_mode == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
    } else {
        // roll, pitch from waypoint controller, yaw heading from auto_heading()
        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true);
    }
}

功能：auto_wp_run 负责航点模式下的姿态和位置控制，处理从飞行员输入到自动控制的各种情况。
分段控制：包括电机状态设置、飞行员输入处理、航点导航执行、水平和垂直控制输出以及最终姿态调整。

2.4 航点导航更新函数

AC_WPNav::update_wpnav 函数是一个航点导航更新函数，用于在航点导航过程中更新目标位置、速度以及控制输出。这个函数的主要功能是更新当前的导航状态，并通过调用相关控制器的方法来实现无人机或机器人的移动控制。

这个导航控制器的功能:
(1)将所有航迹点规划成一条具体路径曲线或直线，从这条曲线上选择一个点作为当前时刻的飞机位置期望值，并将飞机期望位置值沿着这条路径问前推进:
(2)有了期望位置后就可以调用位置控制器;

bool AC_WPNav::update_wpnav()
{
    //函数返回一个布尔值，默认值为 true，表示更新过程正常完成。如果发生错误或无法按轨迹前进（例如地形数据缺失），该值会变为 false。
    bool ret = true;

    //检查当前航点速度 _wp_speed_cms 是否与上次记录的速度 _last_wp_speed_cms 相等。
    if (!is_equal(_wp_speed_cms.get(), _last_wp_speed_cms)) {
        set_speed_xy(_wp_speed_cms);//如果不相等，调用 set_speed_xy(_wp_speed_cms) 设置新的航点速度
        _last_wp_speed_cms = _wp_speed_cms;//同时更新 _last_wp_speed_cms。
    }

    // advance the target if necessary
    //调用 advance_wp_target_along_track(_pos_control.get_dt()) 函数根据当前的时间步长（_pos_control.get_dt()）在航点轨迹上来推进当前的目标点。
    if (!advance_wp_target_along_track(_pos_control.get_dt())) {
        // To-Do: handle inability to advance along track (probably because of missing terrain data)
        ret = false;//如果无法推进目标（例如地形数据缺失或其他导航问题），返回 false
    }

    //更新水平位置控制器，这个控制器负责计算无人机在 X、Y 平面的运动输出（例如侧向和前向推力），确保无人机按照期望的轨迹移动。
    _pos_control.update_xy_controller();

    //记录更新时间
    _wp_last_update = AP_HAL::millis();
    //函数返回 ret，表示导航更新是否成功。如果任何步骤失败（例如无法推进目标），ret 将返回 false，表示需要采取进一步的处理措施。
    return ret;
}

关键步骤：

速度检查与更新：确保导航速度是最新的并应用到控制器。
目标推进：沿轨迹推进当前目标点，处理潜在的推进失败情况。
控制器更新：更新水平位置控制器，计算无人机在 X、Y 平面的运动输出。
时间记录：更新最后一次导航更新的时间，为下次控制逻辑提供参考。

2.5advance_wp_target_along_track()沿着预定的轨迹推进目标位置

AC_WPNav::advance_wp_target_along_track 函数负责在航点导航过程中沿着预定的轨迹推进目标位置。这一过程涉及到计算地形调整、更新目标位置、速度和加速度，并最终将这些信息传递给位置控制器。

bool AC_WPNav::advance_wp_target_along_track(float dt)
{
    // calculate terrain adjustments
    float terr_offset = 0.0f;
    //是否启用地形调整，获取地形高度调整。如果获取失败，函数返回 false，表示无法推进目标。
    if (_terrain_alt && !get_terrain_offset(terr_offset)) {
        return false;
    }
    //使用 pos_offset_z_scaler 方法计算高度缩放因子，考虑到地形偏移和地形边界。
    const float offset_z_scaler = _pos_control.pos_offset_z_scaler(terr_offset, get_terrain_margin() * 100.0);

    // input shape the terrain offset
    //调用 _pos_control.update_pos_offset_z(terr_offset) 更新位置控制器中的高度偏移。
    _pos_control.update_pos_offset_z(terr_offset);

    // get current position and adjust altitude to origin and destination's frame (i.e. _frame)
    //从导航系统获取当前位置信息，并根据地形高度调整将其转换到合适的坐标系中。
    const Vector3f &curr_pos = _inav.get_position() - Vector3f{0, 0, terr_offset};

    // Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft
    //从位置控制器获取当前目标速度，并考虑到高度偏移。
    Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
    curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();

    //计算时间缩放因子 track_scaler_dt，用于限制目标点的推进速度。根据当前速度、目标速度和误差来调整时间缩放因子，以避免目标移动过快。
    float track_scaler_dt = 1.0f;
    // check target velocity is non-zero
    if (is_positive(curr_target_vel.length())) {
        Vector3f track_direction = curr_target_vel.normalized();
        const float track_error = _pos_control.get_pos_error_cm().dot(track_direction);
        const float track_velocity = _inav.get_velocity().dot(track_direction);
        // set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation.
        track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f);
    }

    //如果设置了航点的期望速度，调用 update_vel_accel 和 shape_vel_accel 方法，更新地形速度和加速度。计算速度时间缩放因子 vel_time_scalar，确保目标速度不会超过设定的期望速度。
    float vel_time_scalar = 1.0;
    if (is_positive(_wp_desired_speed_xy_cms)) {
        update_vel_accel(_terrain_vel, _terrain_accel, dt, false);
        shape_vel_accel( _wp_desired_speed_xy_cms * offset_z_scaler, 0.0,
            _terrain_vel, _terrain_accel,
            -_wp_accel_cmss, _wp_accel_cmss, _pos_control.get_shaping_jerk_xy_cmsss(), dt, true);
        vel_time_scalar = _terrain_vel / _wp_desired_speed_xy_cms;
    }

    //根据最大加速度和最大变化率更新 S 曲线时间缩放因子 _track_scalar_dt，确保目标沿着 S 曲线移动时不会发生剧烈的速度变化。
    // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
    float track_scaler_tc = 1.0f;
    if (!is_zero(_wp_jerk)) {
        track_scaler_tc = 0.01f * _wp_accel_cmss/_wp_jerk;
    }
    _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc);

    // target position, velocity and acceleration from straight line or spline calculators
    Vector3f target_pos, target_vel, target_accel;

    bool s_finished;
    if (!_this_leg_is_spline) {
        // update target position, velocity and acceleration
        target_pos = _origin;
        s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel);
    } else {
        // splinetarget_vel
        target_vel = curr_target_vel;
        _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel);
        s_finished = _spline_this_leg.reached_destination();
    }

    //更新目标位置、速度和加速度
    target_vel *= vel_time_scalar;
    target_accel *= sq(vel_time_scalar);

    // convert final_target.z to altitude above the ekf origin
    //将时间缩放因子应用到目标速度和加速度上。
    target_pos.z += _pos_control.get_pos_offset_z_cm();
    target_vel.z += _pos_control.get_vel_offset_z_cms();
    target_accel.z += _pos_control.get_accel_offset_z_cmss();

    // pass new target to the position controller
    //将最终目标位置、速度和加速度的 Z 轴值调整为相对于 EKF 原点的高度。
    _pos_control.set_pos_vel_accel(target_pos.topostype(), target_vel, target_accel);

    // check if we've reached the waypoint
    //调用 _pos_control.set_pos_vel_accel 将新的目标位置、速度和加速度传递给位置控制器。
    if (!_flags.reached_destination) {
        if (s_finished) {
            // "fast" waypoints are complete once the intermediate point reaches the destination
            if (_flags.fast_waypoint) {
                _flags.reached_destination = true;
            } else {
                // regular waypoints also require the copter to be within the waypoint radius
                const Vector3f dist_to_dest = curr_pos - _destination;
                if (dist_to_dest.length_squared() <= sq(_wp_radius_cm)) {
                    _flags.reached_destination = true;
                }
            }
        }
    }

    // successfully advanced along track
    //检查是否到达航点
    return true;
}

/// recalculate path with update speed and/or acceleration limits
void AC_WPNav::update_track_with_speed_accel_limits()
{
    // update this leg
    if (_this_leg_is_spline) {
        _spline_this_leg.set_speed_accel(_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(),
                                         _wp_accel_cmss, _wp_accel_z_cmss);
    } else {
        _scurve_this_leg.set_speed_max(_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms());
    }

    // update next leg
    if (_next_leg_is_spline) {
        _spline_next_leg.set_speed_accel(_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(),
                                         _wp_accel_cmss, _wp_accel_z_cmss);
    } else {
        _scurve_next_leg.set_speed_max(_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms());
    }
}

/// get_wp_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav::get_wp_distance_to_destination() const
{
    // get current location
    const Vector3f &curr = _inav.get_position();
    return norm(_destination.x-curr.x,_destination.y-curr.y);
}

/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_wp_bearing_to_destination() const
{
    return get_bearing_cd(_inav.get_position(), _destination);
}

/// update_wpnav - run the wp controller - should be called at 100hz or higher
bool AC_WPNav::update_wpnav()
{
    //函数返回一个布尔值，默认值为 true，表示更新过程正常完成。如果发生错误或无法按轨迹前进（例如地形数据缺失），该值会变为 false。
    bool ret = true;

    //检查当前航点速度 _wp_speed_cms 是否与上次记录的速度 _last_wp_speed_cms 相等。
    if (!is_equal(_wp_speed_cms.get(), _last_wp_speed_cms)) {
        set_speed_xy(_wp_speed_cms);//如果不相等，调用 set_speed_xy(_wp_speed_cms) 设置新的航点速度
        _last_wp_speed_cms = _wp_speed_cms;//同时更新 _last_wp_speed_cms。
    }

    // advance the target if necessary
    //调用 advance_wp_target_along_track(_pos_control.get_dt()) 函数根据当前的时间步长（_pos_control.get_dt()）在航点轨迹上来推进当前的目标点。
    if (!advance_wp_target_along_track(_pos_control.get_dt())) {
        // To-Do: handle inability to advance along track (probably because of missing terrain data)
        ret = false;//如果无法推进目标（例如地形数据缺失或其他导航问题），返回 false
    }

    //更新水平位置控制器，这个控制器负责计算无人机在 X、Y 平面的运动输出（例如侧向和前向推力），确保无人机按照期望的轨迹移动。
    _pos_control.update_xy_controller();

    //记录更新时间
    _wp_last_update = AP_HAL::millis();
    //函数返回 ret，表示导航更新是否成功。如果任何步骤失败（例如无法推进目标），ret 将返回 false，表示需要采取进一步的处理措施。
    return ret;
}

关键步骤：

地形调整：处理地形信息以调整无人机的高度。
速度和加速度计算：根据目标速度和地形影响更新速度与加速度。
S 曲线和样条推进：根据航点类型选择适当的推进方式。
到达判断：检查是否到达目标航点，并根据设定的半径进行判断。

2.6update_xy_controller()更新水平位置控制器

位置控制器的结构是：p位置 ~> pid 速度 ~> 加速度 ~>飞机倾角的这样一种串联结构，控制器实例调用本身的 update_all 就可以计算出控制量：

void AC_PosControl::update_xy_controller()
{
    // check for ekf xy position reset
    //检查并处理扩展卡尔曼滤波器（EKF）的 XY 位置是否需要重置。
    handle_ekf_xy_reset();

    // Check for position control time out
    //检查 XY 控制是否处于活动状态。如果未活动，初始化控制器，并检查时间是否正常。如果存在时间问题，发送警告消息。
    if ( !is_active_xy() ) {
        init_xy_controller();
        if (has_good_timing()) {
            // call internal error because initialisation has not been done
            gcs().send_text(MAV_SEVERITY_WARNING, "Bad loop slippage detected.");
        }
    }
    //记录最后一次更新的时间戳，以便后续的时间计算。
    _last_update_xy_us = AP_HAL::micros64();
    //获取 EKF 的速度限制和增益缩放因子，这些将用于后续的速度和加速度计算。
    float ekfGndSpdLimit, ekfNavVelGainScaler;
    AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);

    // Position Controller

    //获取当前位置信息，并更新目标速度，以实现向目标位置移动。
    const Vector3f &curr_pos = _inav.get_position();
    Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos, _limit.pos_xy);

    // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
    //添加速度前馈
    vel_target *= ekfNavVelGainScaler;
    _vel_target.x = vel_target.x;
    _vel_target.y = vel_target.y;
    _vel_target.x += _vel_desired.x;
    _vel_target.y += _vel_desired.y;

    // Velocity Controller

    // check if vehicle velocity is being overridden
    // todo: remove this and use input shaping
    //检查是否存在速度覆盖，如果没有，则更新车辆的当前水平速度，并计算加速度目标。
    if (_flags.vehicle_horiz_vel_override) {
        _flags.vehicle_horiz_vel_override = false;
    } else {
        _vehicle_horiz_vel.x = _inav.get_velocity().x;
        _vehicle_horiz_vel.y = _inav.get_velocity().y;
    }
    Vector2f accel_target = _pid_vel_xy.update_all(Vector2f{_vel_target.x, _vel_target.y}, _vehicle_horiz_vel, Vector2f(_limit_vector.x, _limit_vector.y));
    // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
    //将计算得到的加速度目标与所需加速度进行合并。
    accel_target *= ekfNavVelGainScaler;

    // pass the correction acceleration to the target acceleration output
    _accel_target.x = accel_target.x;
    _accel_target.y = accel_target.y;

    // Add feed forward into the target acceleration output
    _accel_target.x += _accel_desired.x;
    _accel_target.y += _accel_desired.y;

    // Acceleration Controller

    // limit acceleration using maximum lean angles
    //限制加速度
    _limit_vector.x = 0.0f;
    _limit_vector.y = 0.0f;
    float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
    float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
    if (_accel_target.limit_length_xy(accel_max)) {
        _limit_vector.x = _accel_target.x;
        _limit_vector.y = _accel_target.y;
    }

    // update angle targets that will be passed to stabilize controller
    //更新目标角度，根据加速度目标计算倾斜角度（滚转和俯仰）并计算航向。
    accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
    calculate_yaw_and_rate_yaw();
}

p位置控制器的输出是期望速度值

pid速度控制器的输出是期望加速度值

加速度控制器的输出是姿态内环的期望姿态角（倾角）

2.7update_all

位于libraries/AC_PID/AC_PID.cpp，这段代码实现了 PID 控制算法的核心逻辑，能够根据目标和测量的输入动态调整控制信号。它处理了误差计算、滤波、输出限制等多个关键因素，以确保控制器的稳定性和有效性。

~累了~明天补充一下

标签：10,target,--,auto,accel,pos,ArduSub,wp,vel
From： https://blog.csdn.net/weixin_49839886/article/details/142530666