垂起固定翼控制器

垂起固定翼使用“多旋翼起飞—高度建立—转换—固定翼任务”流程。vector_vtol.py 管理任务阶段，VTOL_class.uav_vtol 封装 MAVROS 通信与形态状态，VectorControlVtol 完成固定翼阶段的速度—姿态转换。

状态机

fly_status = 0
  多旋翼位置控制，目标高度 100 m
        │ 高度 > 80 m
        ▼
fly_status = 1
  请求 drone → plane，持续给前方位置目标
        │ ExtendedState=plane 或等待超过约 8 s
        ▼
fly_status = 2
  执行上层 vel / att 指令

80 m 和 100 m 是当前节点中的默认值，分别由 formation_threshold 与 formation_threshold + 20 形成。若修改，应同时考虑转换所需空速、爬升空间和仿真模型能力。

VTOL 形态映射

FlyStateCB() 将 MAVROS ExtendedState.vtol_state 映射为：

状态码 内部字符串
4 plane
3 drone
2 plane2drone
1 drone2plane

FlyModeTrans("plane") 在多旋翼态调用 VTOL 转换服务请求状态 4；从固定翼切回多旋翼时请求状态 3。不满足当前形态与目标形态组合时，函数直接拒绝转换。

多旋翼阶段 MotionControl()

在 fly_mode == "drone" 时，控制器发布 PositionTarget：

  • pos：位置控制，局部坐标系；
  • vel：速度控制，机体系相对坐标帧；
  • acc：加速度/力设定值。

启动阶段使用 pos，使飞机先垂直爬升。此时上层轨迹命令即使已经发布，也要等到固定翼任务阶段才会进入固定翼控制链。

固定翼阶段 MotionControl()

在 fly_mode == "plane" 时支持：

  • pos：发布位置设定值；
  • vel：源码把输入解释为机体系角速度并发布 AttitudeTarget；
  • att：发布欧拉角转换后的姿态四元数与纵向控制量。

vector_vtol.py 在任务阶段使用 att。

Vec2Ori()

算法与固定翼版本结构相同：

  1. 期望水平速度方向生成偏航角；
  2. 速度模长 PID 生成轴向加速度需求；
  3. 当前与期望水平速度夹角经 PID 生成期望航向变化率；
  4. 根据协调转弯关系 roll=atan(v·yaw_rate/g) 生成滚转角；
  5. 滚转角限制在 [-π/4, π/4]。

VTOL 版本在水平速度极小时增加了保护，将航向差置零。

tecsControl()

VTOL 固定翼阶段根据总速度、垂向速度、加速度模长与期望速度构造能量变化和能量分配量。当前实际输出为：

force = PID_E(dot_E_sp, dot_E) + dot_E_sp + 0.5
pitch = PID_pitch(desire_v_z, 0)

俯仰限制为 [-π/6, π/5]，纵向控制量下限为 0.15。源码中还计算了 B_err_pid 对应的俯仰候选量，但随后被 pitch_pid 输出覆盖，因此当前调参应关注最终赋值。

话题与服务检查

rostopic echo -n 1 /mavros/extended_state
rostopic echo -n 1 /mavros/state
rostopic hz /control_signal/vector
rosservice info /mavros/cmd/vtol_transition

多机时给每个名称增加 /uavN 前缀。

运行边界

  • 在固定翼形态前，不应使用固定翼低速条件下的速度—姿态转换。
  • 转换成功应以 ExtendedState 为准；固定等待时间只应作为仿真兜底条件。
  • tecsControl() 中总速度、期望速度为分母，零速度指令需要在上层避免。
  • uav_amount 会影响话题命名空间；单机 launch 必须传入 1。

完整签名见VTOL_class.py API和vector_vtol.py API。