二次开发流程

建议先在现有示例上替换算法，再逐步修改控制器。每次只改变一层，可以更快区分是环境、接口、算法还是控制律问题。

新建单机算法

1. 创建节点脚本

以 circle_track.py 为模板：

#!/usr/bin/env python3
import numpy as np
import rospy

from uav_lib.spawn_uav import uav

rospy.init_node("my_guidance")
vehicle = uav(type="plane", uav_amount=1, uav_index=0)
rate = rospy.Rate(50)

while not rospy.is_shutdown():
    position = np.asarray(vehicle.local_pose, dtype=float)
    velocity = np.asarray(vehicle.local_vel, dtype=float)

    command = my_algorithm(position, velocity)
    vehicle.control_send(command, "vel")
    rate.sleep()

command 必须至少包含三个分量，单位为 m/s。

2. 添加 launch

复制同机型的现有 launch，替换算法节点脚本名。保留对应控制节点：

  • 固定翼：vector_fw.py；
  • 垂起固定翼：vector_vtol.py；
  • 多旋翼：rotor.py。

3. 编译与运行

cd ~/swiftwing_ws
catkin_make
source devel/setup.bash

先启动 PX4 SITL，再启动新算法 launch。

新建轨迹类

推荐接口：

class MyGuidance:
    def __init__(self, ...):
        ...

    def direction(self, position):
        """返回归一化三维导航方向。"""
        ...

算法层负责处理退化点、归一化和速度大小。控制节点只接收最终期望速度，不会替上层验证轨迹几何是否有效。

直接发送姿态

vehicle.control_send([roll, pitch, yaw, force], "att")

使用姿态入口前应确认：

  • 欧拉角顺序和单位；
  • 俯仰符号；
  • force 的安全范围；
  • 发布频率与 OFFBOARD 超时；
  • 固定翼当前空速和高度。

姿态入口绕过 Vec2Ori() 与能量式控制，适合验证自定义内环/外环衔接，不适合作为不了解控制链时的第一入口。

修改固定翼控制律

建议按以下顺序：

  1. 记录现有 velocity_sp、local_vel、global_acc、姿态设定值和实际姿态；
  2. 离线验证新公式的零速度、直线、定常转弯和爬升工况；
  3. 保留滚转、俯仰、纵向控制量限幅；
  4. 在单机、无风、低轨迹曲率场景验证；
  5. 再测试大误差、三维轨迹和多机调用。

新建编队算法

可以保留 ROS 集成脚本，只替换 swarm_lf 中的纯算法类。推荐接口至少包含：

StatusUpdate(...)  更新自身与邻居状态
Acal(...)          输出二维期望加速度
AngleCal(...)      转为轴向加速度和滚转角

如果算法输出速度而非加速度，也可以直接发布 /uavN/control_signal/vector，但需要重新设计固定翼转弯与队形误差之间的关系。

验证清单

接口

  • MAVROS connected=True；
  • 状态话题频率稳定；
  • 控制指令包含有限数值；
  • 单机/多机命名空间一致；
  • 模型名和 uav_id 对应。

算法

  • 初始点在路径上、路径外、路径退化点均有明确输出；
  • 所有归一化检查零模长；
  • acos/asin 输入钳位到有效区间；
  • 加速度、滚转、俯仰和爬升率有限幅；
  • 控制周期使用真实 dt，并处理异常大/小时间步。

仿真

  • 记录 Gazebo real-time factor；
  • 先单机后多机；
  • 先固定初始条件后随机化；
  • 保存参数、源码版本、launch 命令和日志；
  • 不把仿真成功直接等同于实机可用。

函数签名、调用和状态写入可在API 参考总览中逐文件查询。