参考链接：直升机位置控制和导航 — 开发文档 (ardupilot.org)
参考图：
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 frameconst 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 controllerpitch_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 NaNif (target.is_nan() || target.is_inf() ||measurement.is_nan() || measurement.is_inf()) {return Vector2f{};}_target = target;// reset input filter to value receivedif (_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 derivativeif (_dt > 0.0f) {const Vector2f derivative{(_error - error_last) / _dt};_derivative += (derivative - _derivative) * get_filt_D_alpha();}}// update I termupdate_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 == ENABLEDcase POSHOLD:poshold_run();break;
#endifcase 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 controllerswitch (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 == ENABLEDauto_nav_guided_run();//用于自主导航（Guided Mode），但只有在 NAV_GUIDED 编译选项启用时才会执行。
#endifbreak;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 ROIif (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 disarmedmotors.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 ratetarget_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 rangemotors.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 loiterfailsafe_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 anglesfloat 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 pilotattitude_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 adjustmentsfloat 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-zeroif (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 jerkfloat 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 calculatorsVector3f target_pos, target_vel, target_accel;bool s_finished;if (!_this_leg_is_spline) {// update target position, velocity and accelerationtarget_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_veltarget_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 destinationif (_flags.fast_waypoint) {_flags.reached_destination = true;} else {// regular waypoints also require the copter to be within the waypoint radiusconst 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 legif (_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 legif (_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 locationconst 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 donegcs().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 控制算法的核心逻辑，能够根据目标和测量的输入动态调整控制信号。它处理了误差计算、滤波、输出限制等多个关键因素，以确保控制器的稳定性和有效性。
//pid核心算法
float AC_PID::update_all(float target, float measurement, bool limit)
{// don't process inf or NaN//检查确保 target 和 measurement 的值是有限的if (!isfinite(target) || !isfinite(measurement)) {return 0.0f;}// reset input filter to value received//如果 _reset_filter 标志为真，则重置 PID 控制器的状态：if (_flags._reset_filter) {_flags._reset_filter = false;_target = target;//设置当前目标为新的 target。_error = _target - measurement;//计算误差 (_target - measurement) 并更新 _error_derivative = 0.0f;//将导数 _derivative 重置为 0} else {//如果没有重置，计算新的目标值、误差和导数：float error_last = _error;_target += get_filt_T_alpha() * (target - _target);//更新目标值 _target，使用滤波器常数 get_filt_T_alpha()_error += get_filt_E_alpha() * ((_target - measurement) - _error);//更新误差 _error// calculate and filter derivative//如果 _dt（时间增量）大于 0，则计算误差的导数并应用滤波器常数 get_filt_D_alpha()。if (_dt > 0.0f) {float derivative = (_error - error_last) / _dt;_derivative += get_filt_D_alpha() * (derivative - _derivative);}}// update I term//更新积分项update_i(limit);//计算比例（P）输出和导数（D）输出float P_out = (_error * _kp);float D_out = (_derivative * _kd);// calculate slew limit modifier for P+D//使用斜率限制器对 P 和 D 输出进行修正，以确保输出变化速率不超过设定值。_pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt);_pid_info.slew_rate = _slew_limiter.get_slew_rate();P_out *= _pid_info.Dmod;D_out *= _pid_info.Dmod;//将当前的目标、测量值、误差和 P、D 输出保存到 _pid_info 结构中，这可能用于调试或监控。_pid_info.target = _target;_pid_info.actual = measurement;_pid_info.error = _error;_pid_info.P = P_out;_pid_info.D = D_out;//返回最终的 PID 输出，包含比例输出、积分输出和导数输出的和。这个值将被用作控制器的输出return P_out + _integrator + D_out;
}
更新积分项
void AC_PID::update_i(bool limit)
{if (!is_zero(_ki) && is_positive(_dt)) {// Ensure that integrator can only be reduced if the output is saturatedif (!limit || ((is_positive(_integrator) && is_negative(_error)) || (is_negative(_integrator) && is_positive(_error)))) {//积分项的更新公式_integrator=_integrator+error×K i×dt_integrator += ((float)_error * _ki) * _dt;_integrator = constrain_float(_integrator, -_kimax, _kimax);}} else {_integrator = 0.0f;}_pid_info.I = _integrator;_pid_info.limit = limit;
}