固定翼控制器¶

固定翼控制链由 vector_fw.py、FW_class.uav 和 VectorControlPlane 组成。上层发布速度或姿态命令后，控制节点以约 100 Hz 更新姿态设定值，并持续检查解锁和 OFFBOARD 状态。

初始化与通信¶

FW_class.uav.__init__() 依次完成：

根据 uav_index 选择根命名空间或 /uavN；
初始化位置、速度、加速度、姿态和控制命令缓存；
创建 MAVROS 订阅者；
等待起飞、解锁和模式服务；
创建姿态、位置和局部速度设定值发布者。

控制节点收到 /control_signal/vector 后把 control_mode 设为 vel；收到 /control_signal/att 后设为 att。后收到的指令决定当前控制模式。

自动起飞与 OFFBOARD¶

未解锁时，takeoff() 根据当前 GPS 位置计算向东约 100 m 的目标点，并调用 MAVROS 起飞服务。随后 arming(True) 请求解锁。主循环在非 OFFBOARD 状态下持续调用 set_offboard()。

OFFBOARD 通常要求设定值连续发布。调试时应同时检查：

rostopic hz /control_signal/vector
rostopic echo -n 1 /mavros/state
rostopic hz /mavros/setpoint_raw/attitude

速度向量到姿态：`Vec2Ori()`¶

输入为期望速度 v_d=[vx, vy, vz]，当前速度为 v。源码输出：

desire_h_d = v_d,z
desire_psi = atan2(v_d,y, v_d,x)
a = 0.9 (||v_d|| - ||v||)

其中 a 是用于纵向能量控制的速度模长误差项。水平面内通过当前速度与期望速度的叉积确定转弯方向。

method=1：航向误差 PID¶

psi_diff = signed acos((v_d,xy · v_xy) / (||v_d,xy|| ||v_xy||))
psi_rate_cmd = PID(psi_diff)
roll_cmd = atan(||v_xy|| psi_rate_cmd / g)
roll_cmd ∈ [-π/4, π/4]

控制器返回时再次对滚转角取负，vector_fw.py 直接使用返回值。

method=0：横向速度误差¶

源码从期望水平速度中减去沿当前速度方向的投影，以剩余横向分量估计法向加速度，再通过 atan(a_t/g) 得到滚转角。当前固定翼节点默认使用 method=1。

能量式纵向控制：`tecsControl()`¶

该函数不是 PX4 TECS 的完整复刻，而是源码中的 TECS 类能量控制实现。它根据水平速度、垂向速度、水平加速度和轴向加速度需求构造两个组合量：

path_angle    = atan(v_up / v_horizontal)
path_angle_sp = atan(desire_climb_rate / v_horizontal)

dot_B    = path_angle - a_horizontal/g
dot_E    = a_horizontal/g + path_angle
dot_B_sp = path_angle_sp - a_sp/g
dot_E_sp = a_sp/g + 2 path_angle_sp

dot_E 对应总能量变化趋势，dot_B 对应动能与势能分配趋势。当前实现用 E_err_pid 与前馈项生成 force，俯仰角主要由期望爬升率及 pitch_h_pid 生成：

force = PID_E(dot_E_sp, dot_E) + dot_E_sp + 0.5
pitch = 0.15 desire_climb_rate + PID_h(desire_climb_rate, v_up)

desire_climb_rate 限制在 [-5, 5] m/s；
pitch 限制在 [-π/6, π/6]；
force 小于 0.15 或为 NaN 时置为 0.15。

源码虽然计算了 B_err_pid 输出，但当前返回的俯仰角没有使用该输出。调参时应以实际执行路径为准。

姿态设定值：`MotionControl()`¶

`control_type`	MAVROS 消息	有效字段
`pos`	`PoseStamped`	x、y、z
`angular`	`AttitudeTarget`	机体系角速度、thrust
`att`	`AttitudeTarget`	四元数姿态、thrust
`vel`	`PositionTarget`	局部速度、yaw

vector_fw.py 的默认链路最终使用 att。Eular2Quater() 把滚转、俯仰、偏航转换为四元数，角速度字段由 type_mask 忽略。

数值边界¶

Vec2Ori(method=1) 在水平速度或期望水平速度为零时会出现除零风险；进入控制前应设置最小巡航速度并检查有效状态。
acos() 的输入没有显式钳位到 [-1,1]；浮点误差可能产生 NaN。
tecsControl() 计算 v_up/v_horizontal，水平速度过小时不适合使用固定翼控制律。
PID 参数在类构造函数和 vector_fw.py 中均有设置，节点中的后续赋值会覆盖部分默认值。

完整签名和状态写入见FW_class.py API。