记录无人机的航点任务

航点的是怎么影响无人机的姿态的？

打个断点停在上一个航点结束 下一个开始


计算以当前点为原点的下一个目标点的向量值

向量的三个分量：

• x：北向距离（米）

• y：东向距离（米）

• z：垂直距离（米，向下为正）

这个方向向量通常用于航点导航中：

1. 计算航向角：

   float target_bearing = atan2(destination_vector.y, destination_vector.x);

2. 计算距离：

   float leg_length = destination_vector.length();

3. 计算路径跟踪误差：

   // 计算当前位置到目标航线的偏移 Vector3f position_error = current_pos - _destination_ned_m; float crosstrack_error = position_error.x * sin(leg_bearing) - position_error.y * cos(leg_bearing);

4. 计算速度前馈：

   // 将方向向量归一化后作为期望速度方向 Vector3f target_velocity = destination_vector.normalized() * wp_speed;

航点影响_ang_vel_target_rads

void ModeAuto::nav_attitude_time_run()

具体来说：

1. 航点 → 位置控制器：AC_WPNav 将航点位置传递给 AC_PosControl。

2. 位置控制器 → 目标姿态角：AC_PosControl::NE_update_controller() 计算出 _roll_target_rad 和 _pitch_target_rad。

3. 目标姿态角 → 姿态控制器：ModeAuto 或其他飞行模式调用 attitude_control->input_euler_angle_roll_pitch_yaw_rad()，传入目标横滚/俯仰/偏航角。

4. 姿态控制器 → _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)