纪录无人机PID参数配置-CSDN博客

PSC_D_POS_P

0.50 4.00

Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller. Previously _POSZ_P.

PSC_D_VEL_P

1.0 10.0

Velocity (vertical) controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller. Previously _VELZ_P.

PSC_D_ACC_P

0.05

0.010 0.250

Acceleration (vertical) controller P gain. Converts the difference between desired vertical acceleration and actual acceleration into a motor output. If upgrading from 4.6 this is _ACCZ_P * 0.1.

pid的算法函数在

// Runs the vertical (U-axis) position controller.

// Computes output acceleration based on position and velocity errors using PID correction.

// Feedforward velocity and acceleration are combined with corrections to produce a smooth vertical command.

// Desired position, velocity, and acceleration must be set before calling.

void AC_PosControl::D_update_controller()

{

// check for ekf z-axis position reset

D_handle_ekf_reset();

// Check for z_controller time out

if (!D_is_active()) {

D_init_controller();

if (has_good_timing()) {

// call internal error because initialisation has not been done

INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);

}

_last_update_d_ticks = AP::scheduler().ticks32();

参数名称	所属控制器（类成员变量）	核心作用
`PSC_D_POS_P`	位置环 (P Controller) 对应代码中的 `_p_pos_d_m`	将位置误差转换为期望速度，代码为 `_p_pos_d_m.update_all()`
`PSC_D_VEL_P`	速度环 (PID Controller) 对应代码中的 `_pid_vel_d_m`	将速度误差转换为期望加速度，代码为 `_pid_vel_d_m.update_all()`
`PSC_D_ACC_P`	加速度环 (PID Controller) 对应代码中的 `_pid_accel_d_m`	将加速度误差转换为电机推力（油门），代码为 `_pid_accel_d_m.update_all()`

欧拉角与4元组的变换

// 欧拉角：横滚=40°, 俯仰=50°, 偏航=30°
_euler_angle_target_rad.x = radians(40); // 横滚
_euler_angle_target_rad.y = radians(50); // 俯仰
_euler_angle_target_rad.z = radians(30); // 偏航

不能直接用"绕每个轴转多少"来构造四元数，因为四元数表示的是一次复合旋转，而不是三次独立旋转的简单叠加。

正确的做法是：按顺序累乘三次小旋转。

// 初始：单位四元数（无旋转）
Quaternion attitude_target;

// 第一步：绕Z轴转30°（偏航）
Quaternion q_yaw;
q_yaw.from_axis_angle(Vector3f(0, 0, 1), radians(30));
attitude_target = attitude_target * q_yaw;

// 第二步：绕Y轴转50°（俯仰）
Quaternion q_pitch;
q_pitch.from_axis_angle(Vector3f(0, 1, 0), radians(50));
attitude_target = attitude_target * q_pitch;

// 第三步：绕X轴转40°（横滚）
Quaternion q_roll;
q_roll.from_axis_angle(Vector3f(1, 0, 0), radians(40));
attitude_target = attitude_target * q_roll;

/////欧拉角的万向锁问题

当 pitch = 90° 时：

绕原Z轴的偏航 等同于 绕原X轴的横滚（万向锁）
不同的 (roll, yaw) 组合可能映射到同一个物理姿态

例如

(roll=30°, pitch=90°, yaw=0°) 
= (roll=0°, pitch=90°, yaw=30°)

//////////// log格式

struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
time_us : AP_HAL::micros64(),
throttle_in : attitude_control->get_throttle_in(),
angle_boost : attitude_control->angle_boost(),
throttle_out : motors->get_throttle(),
throttle_hover : motors->get_throttle_hover(),
desired_alt : des_alt_m,
inav_alt : float(pos_control->get_pos_estimate_U_m()),
baro_alt : baro_alt_m,
desired_rangefinder_alt : desired_rangefinder_alt_m,
#if AP_RANGEFINDER_ENABLED
rangefinder_alt : surface_tracking.get_dist_for_logging(),
#else
rangefinder_alt : AP_Logger::quiet_nanf(),
#endif
terr_alt : terr_alt,
target_climb_rate_ms : target_climb_rate_ms,
climb_rate_ms : pos_control->get_vel_estimate_U_ms()
};
logger.WriteBlock(&pkt, sizeof(pkt));