多旋翼控制器¶

多旋翼链路由 rotor.py 与 Rotor_class.uav 组成，接收 /control_signal/vector，自动起飞后把期望速度转换为 MAVROS PositionTarget。

启动流程¶

1. Rotor_class.uav 创建状态订阅、设定值发布和 MAVROS 服务代理。
2. 未解锁时，rotor.py 调用 takeoff(h=30)。
3. 高度超过 25 m 后，请求进入 OFFBOARD。
4. 收到速度命令时，持续调用 MotionControl("vel", velocity_sp)。

起飞高度¶

takeoff() 使用 pygeodesy.geoids.GeoidPGM 读取：

/usr/share/GeographicLib/geoids/egm96-5.pgm

函数用 EGM96 大地水准面偏差修正 GPS 高度，再向 MAVROS 起飞服务发送目标高度。如果文件不存在，应重新运行 MAVROS GeographicLib 数据安装脚本。

坐标与航向¶

local_pose_cb() 根据四元数构造旋转矩阵，并保存 R_local2global。速度和加速度命令在发布前经过该矩阵转换。控制器还根据当前全局水平速度计算：

yaw_cmd = atan2(v_y, v_x) - π/2

当三维速度模长不超过 0.05 m/s 时，偏航指令设为 0。

MotionControl() 支持类型¶

类型	有效字段	说明
pos	position + yaw	局部位置控制
vel	velocity + yaw	当前主节点使用的速度控制
acc	acceleration_or_force + yaw	加速度/力控制入口

不同类型通过 PositionTarget.type_mask 忽略无关字段。

开发注意¶

• 当前主节点只订阅速度控制话题，没有启用姿态控制订阅。
• takeoff() 依赖 GPS、地球重力模型文件和 MAVROS 起飞服务；任一项缺失都会阻止自动流程。
• 改变初始高度时，应同步修改起飞高度和进入 OFFBOARD 的高度门限。
• coordinate_frame 与旋转矩阵共同决定指令方向，修改其中任一项后都应做单轴方向测试。

完整签名见Rotor_class.py API。