航点的是怎么影响无人机的姿态的？
打个断点停在上一个航点结束 下一个开始
计算以当前点为原点的下一个目标点的向量值
向量的三个分量：
-
x：北向距离（米） -
y：东向距离（米） -
z：垂直距离（米，向下为正）
这个方向向量通常用于航点导航中：
-
计算航向角：
float target_bearing = atan2(destination_vector.y, destination_vector.x);
-
计算距离：
float leg_length = destination_vector.length();
-
计算路径跟踪误差：
// 计算当前位置到目标航线的偏移 Vector3f position_error = current_pos - _destination_ned_m; float crosstrack_error = position_error.x * sin(leg_bearing) - position_error.y * cos(leg_bearing);
-
计算速度前馈：
// 将方向向量归一化后作为期望速度方向 Vector3f target_velocity = destination_vector.normalized() * wp_speed;
航点影响_ang_vel_target_rads
void ModeAuto::nav_attitude_time_run()
具体来说：
-
航点 → 位置控制器 ：
AC_WPNav将航点位置传递给AC_PosControl。 -
位置控制器 → 目标姿态角 ：
AC_PosControl::NE_update_controller()计算出_roll_target_rad和_pitch_target_rad。 -
目标姿态角 → 姿态控制器 ：
ModeAuto或其他飞行模式调用attitude_control->input_euler_angle_roll_pitch_yaw_rad()，传入目标横滚/俯仰/偏航角。 -
姿态控制器 →
_ang_vel_target_rads：在input_euler_angle_roll_pitch_yaw_rad()中，attitude_command_model()计算角度误差对应的角速度，最终通过euler_derivative_to_body()转换为机体角速度，存入_ang_vel_target_rads。
_euler_angle_target_rad 来自与_ang_vel_target_rads
看下roll
(gdb) p ((AP_MotorsMatrix*)copter.motors)._roll_in
$16 = 0.383552969
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
_roll_in =>
_motors.set_roll(get_rate_roll_pid().update_all(ang_vel_body.x, gyro_rads.x, dt, _motors.limit.roll, _pd_scale.x, _i_scale.x) + _actuator_sysid.x);
PID:
get_rate_roll_pid().update_all(ang_vel_body.x, gyro_rads.x, dt, _motors.limit.roll, _pd_scale.x, _i_scale.x)
| 变量 | 含义 |
|---|---|
_target |
滤波后的目标值 |
_error |
滤波后的误差值（target - measurement） |
_derivative |
滤波后的误差导数（D 项） |
_integrator |
积分项累积值（I 项） |
_target_derivative |
目标值导数（用于 D_FF 前馈） |