PX4 uORB 消息 VehicleAttitudeSetpoint 全解析：字段语义、发布链路与多旋翼/固定翼姿态控制实践

嵌入式物联网机器人自动驾驶智能硬件【免费下载链接】PX4-AutopilotPX4 Autopilot Software项目地址https://gitcode.com/gh_mirrors/px/PX4-Autopilot点击查看免费下载导读VehicleAttitudeSetpoint是 PX4 飞控内部姿态控制环路的目标输入核心 uORB 消息它把姿态期望四元数、机体坐标系下的推力指令与用户偏航速率指令打包在一起在姿态控制器多旋翼mc_att_control、固定翼fw_att_control与上游导航/位置控制器之间流转。本文以 docs/en/msg_docs/VehicleAttitudeSetpoint.md 为骨架结合仓库源码与 MAVLink 接收链路完整讲解该消息的字段语义、三个主题vehicle_attitude_setpoint、mc_virtual_attitude_setpoint、fw_virtual_attitude_setpoint的发布/订阅关系、跨机型多旋翼、固定翼、VTOL的推力约定以及 Offboard 外环控制如何通过该消息注入期望。读完本文你将掌握这条消息在 PX4 控制架构中的精确位置、每个字段的真实取值规则并能正确读懂或编写基于该主题的控制器与日志分析。消息定义与字段详解该消息由 msg/versioned/VehicleAttitudeSetpoint.msg 定义文档页 docs/en/msg_docs/VehicleAttitudeSetpoint.md 由 PX4 的 msg 文档生成工具自动生成。消息当前版本号为 1MESSAGE_VERSION 1原始定义如下uint32 MESSAGE_VERSION 1 uint64 timestamp # time since system start (microseconds) float32 yaw_sp_move_rate # rad/s (commanded by user) # For quaternion-based attitude control float32[4] q_d # Desired quaternion for quaternion control # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. float32[3] thrust_body # Normalized thrust command in body FRD frame [-1,1] # TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint各字段语义对照字段类型单位/取值说明timestampuint64微秒系统启动以来的时间戳由发布者写入如hrt_absolute_time()yaw_sp_move_ratefloat32rad/s用户摇杆或外环指令期望的偏航速率用于偏航目标跟踪q_dfloat32[4]四元数 (w, x, y, z)期望姿态四元数供基于四元数的姿态控制使用thrust_bodyfloat32[3]归一化[-1, 1]机体坐标系FRD 机体轴系X 前、Y 右、Z 下下的归一化推力指令关于thrust_body的分量约定消息注释给出了两条关键规则且这两条规则在源码中得到了严格印证多旋翼thrust_body[0]、thrust_body[1]通常为 0thrust_body[2]为负的油门需求机体 Z 轴向下产生升力需负向推力。固定翼thrust_body[0]机体 X 轴是油门需求thrust_body[1]、thrust_body[2]通常为 0。补充同一目录下还存在 docs/en/msg_docs/VehicleAttitudeSetpointV0.md源文件 msg/px4_msgs_old/msg/VehicleAttitudeSetpointV0.msg它是版本 0 的历史定义除相同字段外还包含reset_integral导航逻辑切换时重置 roll/pitch/yaw 积分与fw_control_yaw_wheel跑道自动起飞时用转向轮控制航向两个布尔字段。当前主版本 v1 已移除这两个字段说明相关职责已迁移至其他消息或模块。三个 TOPICS 及其在控制架构中的角色消息定义末尾声明了三个主题名TOPICS它们共享同一套字段结构分别承载不同机型/控制链路的姿态设定值TOPICS典型发布者典型订阅者vehicle_attitude_setpoint位置控制模块、MAVLink 接收端Offboard、姿态控制模块本身姿态控制器、VTOL 控制器、日志与 MAVLink 流mc_virtual_attitude_setpoint多旋翼虚拟姿态控制VTOL 固定翼阶段的多旋翼子控制器VTOL 姿态控制中的多旋翼部分、位置控制fw_virtual_attitude_setpoint固定翼虚拟姿态控制VTOL 多旋翼阶段的固定翼子控制器VTOL 姿态控制中的固定翼部分在源码层面多旋翼姿态控制器 src/modules/mc_att_control/mc_att_control_main.cpp 的构造函数中发布主题会根据是否 VTOL 模式切换MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), ...即当作为独立多旋翼运行时发布vehicle_attitude_setpoint当处于 VTOL 的虚拟多旋翼角色时发布mc_virtual_attitude_setpoint。固定翼姿态控制器 src/modules/fw_att_control/FixedwingAttitudeControl.cpp 同理FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), ...VTOL 姿态控制模块 src/modules/vtol_att_control/vtol_att_control_main.h 同时订阅fw_virtual_attitude_setpoint与mc_virtual_attitude_setpoint见其成员_fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}与_mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)}并通过get_fw_virtual_att_sp()、get_mc_virtual_att_sp()等访问器在两套虚拟控制器之间协调实现多旋翼与固定翼控制律在过渡阶段的平滑切换。阅读提示VTOL 姿态控制的完整协调逻辑位于 src/modules/vtol_att_control/vtol_type.cpp 与 tiltrotor.cpp本文不再展开仅确认主题归属关系。生产者谁能写入 vehicle_attitude_setpoint该主题的生产者主要有三类分别对应不同的控制源。1. 位置/导航控制器内环目标生成多旋翼位置控制模块 src/modules/mc_pos_control/MulticopterPositionControl.cpp 在完成位置/速度环计算后会调用ControlMath::thrustToAttitude()把期望推力矢量换算成姿态设定。核心实现位于 src/modules/mc_pos_control/PositionControl/ControlMath.cppvoid thrustToAttitude(const Vector3f thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s att_sp) { bodyzToAttitude(-thr_sp, yaw_sp, att_sp); att_sp.thrust_body[2] -thr_sp.length(); }其配套函数bodyzToAttitude()从期望推力矢量作为机体 Z 轴方向与期望偏航角出发构造期望姿态旋转矩阵并转成四元数Dcmf R_sp; // fill rotation matrix for (int i 0; i 3; i) { R_sp(i, 0) body_x(i); R_sp(i, 1) body_y(i); R_sp(i, 2) body_z(i); } const Quatf q_sp{R_sp}; q_sp.copyTo(att_sp.q_d);注意thrust_body[2] -thr_sp.length()的负号多旋翼期望推力在机体 Z 轴向下方向负号用于把向上的升力需求表达为机体轴系下的负向推力与消息注释中的thrust_body[2]是负的油门需求完全一致。2. 姿态控制器自身手飞模式在多旋翼手飞Stabilized/Altitude 等模式下姿态设定由姿态控制器直接生成。MulticopterAttitudeControl::generate_attitude_setpoint()src/modules/mc_att_control/mc_att_control_main.cpp把摇杆 roll/pitch 映射为期望倾斜角矢量v限制在MPC_MAN_TILT_MAX内把偏航摇杆经Sticks::expoDeadzone与低通滤波后交给_stick_yaw.generateYawSetpoint()最终合成期望四元数Quatf q_sp q_sp_yaw * q_sp_rp; q_sp.copyTo(attitude_setpoint.q_d); attitude_setpoint.thrust_body[2] -throttle_curve(_manual_control_setpoint.throttle); attitude_setpoint.timestamp hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint);yaw_sp_move_rate由_stick_yaw.generateYawSetpoint(...)计算填充rad/s源自用户摇杆指令油门通过throttle_curve()受MPC_THR_CURVE、MPC_THR_HOVER、MPC_THR_MAX等参数控制映射为归一化推力并取负写入thrust_body[2]VTOL 模式下会额外调用AttitudeControlMath::correctTiltSetpointForYawError()修正大偏航误差下的倾斜设定保证用户意图与机头朝向一致。固定翼手飞模式下src/modules/fw_att_control/FixedwingAttitudeControl.cpp 则以机体欧拉角构造期望四元数并直接把油门写入thrust_body[0]q.copyTo(_att_sp.q_d); _att_sp.thrust_body[0] (_manual_control_setpoint.throttle 1.f) * .5f; _att_sp.timestamp hrt_absolute_time(); _attitude_sp_pub.publish(_att_sp);(throttle 1) * 0.5把摇杆区间[-1, 1]映射到归一化油门[0, 1]正是固定翼thrust_body[0]为油门需求的约定。3. MAVLink Offboard 接收端外部期望注入当 GCS 或机载计算机以 Offboard 模式下发SET_ATTITUDE_TARGET时src/modules/mavlink/mavlink_receiver.cpp 会构造vehicle_attitude_setpoint_sif (attitude has_thrust) { vehicle_attitude_setpoint_s attitude_setpoint{}; const matrix::Quatf q{attitude_target.q}; q.copyTo(attitude_setpoint.q_d); // TODO: review use case attitude_setpoint.yaw_sp_move_rate (type_mask ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? (float)NAN : attitude_target.body_yaw_rate; if (!thrust_body thrust) { fill_thrust(attitude_setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust); } ... }这里yaw_sp_move_rate直接取自 MAVLink 消息的body_yaw_rate字段若BODY_YAW_RATE_IGNORE标志置位则写为 NAN表示忽略。而 MAVLink 推力矢量如何落入机体轴系由fill_thrust()src/modules/mavlink/mavlink_receiver.cpp 的MavlinkReceiver::fill_thrust按机型分派switch (_mavlink.get_system_type()) { case MAV_TYPE_FIXED_WING: case MAV_TYPE_GROUND_ROVER: thrust_body_array[0] thrust; break; case MAV_TYPE_QUADROTOR: case MAV_TYPE_HEXAROTOR: case MAV_TYPE_OCTOROTOR: case MAV_TYPE_TRICOPTER: case MAV_TYPE_HELICOPTER: case MAV_TYPE_COAXIAL: thrust_body_array[2] -thrust; break; case MAV_TYPE_SUBMARINE: thrust_body_array[0] thrust; break; ...VTOL 机型则进一步依据vehicle_status.vehicle_typeVEHICLE_TYPE_FIXED_WING写 X 轴、VEHICLE_TYPE_ROTARY_WING写 Z 轴取负动态选择填充位置。这一实现完整印证了消息注释中多旋翼写 Z 轴取负、固定翼写 X 轴的约定并覆盖了无人船、潜艇等更多载体类型。消费者姿态控制器如何消费该消息多旋翼姿态控制MulticopterAttitudeControl通过_vehicle_attitude_setpoint_sub或 VTOL 下的mc_virtual_attitude_setpoint订阅设定值在Run()中检查新消息并交给内部_attitude_control计算角速率期望见 src/modules/mc_att_control/mc_att_control_main.cpp 中的_vehicle_attitude_setpoint_sub.updated()处理分支。姿态控制使用q_d计算姿态误差使用thrust_body归一化推力并通过MC_ROLL_P、MC_PITCH_P、MC_YAW_P、MC_YAW_WEIGHT、MC_ROLLRATE_MAX等参数在parameters_updated()中配置见 src/modules/mc_att_control/mc_att_control.hpp调节响应特性。固定翼姿态控制FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()src/modules/fw_att_control/FixedwingAttitudeControl.cpp读取设定值并转发为角速率期望vehicle_attitude_setpoint_s att_sp{}; if (_att_sp_sub.update(att_sp)) { const Quatf q_d(att_sp.q_d); q_d.copyTo(_att_sp.q_d); _rates_sp.thrust_body[0] att_sp.thrust_body[0]; _rates_sp.thrust_body[1] att_sp.thrust_body[1]; _rates_sp.thrust_body[2] att_sp.thrust_body[2]; ... }可见thrust_body三个分量被原样透传至_rates_sp固定翼姿态控制只消费q_d与thrust_body[0]。其他消费方日志该主题被记录进飞行日志见 src/modules/logger/logged_topics.cpp可在 Flight Review 等工具中回放姿态设定与实际姿态的偏差用于调参与故障分析。MAVLink 状态流AUTOPILOT_STATE_FOR_GIMBAL_DEVICEsrc/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp等流会引用该设定值向云台等设备广播当前姿态期望ATTITUDE_TARGET流src/modules/mavlink/streams/ATTITUDE_TARGET.hpp则把设定值打包回传给地面站。失败检测src/modules/commander/failure_detector/FailureDetector.hpp 引用该主题用于评估设定值一致性。在日志与调试中读取该主题如需在飞行日志中验证姿态设定链路可按以下步骤操作使用 QGroundControl 或ulog工具下载 .ulg 日志在 Flight Review或等效分析工具中选择vehicle_attitude_setpoint主题对照vehicle_attitude主题查看设定四元数q_d[4]与实际四元数q[4]的偏差评估姿态跟踪质量查看thrust_body[2]多旋翼是否始终为负、是否贴近MPC_THR_HOVER等期望值判断油门映射是否异常手飞时若偏航响应迟缓可结合yaw_sp_move_rate与MC_YAW_P、MC_YAW_WEIGHT参数综合分析。提示该主题的 V0 版本字段reset_integral、fw_control_yaw_wheel在历史日志PX4 v1.13 之前版本中仍可能出现分析老日志时请对应 docs/en/msg_docs/VehicleAttitudeSetpointV0.md 的字段表。小结VehicleAttitudeSetpoint是 PX4 姿态控制链路的枢纽消息理解它等于理解了导航层如何把目标姿态交给姿态层这一核心数据流字段设计四元数q_d表达姿态期望、thrust_body以机体 FRD 轴系承载归一化推力多旋翼 Z 轴取负、固定翼 X 轴为正、yaw_sp_move_rate携带用户偏航速率主题拆分vehicle_attitude_setpoint常规、mc_virtual_attitude_setpointVTOL 多旋翼虚拟控制器、fw_virtual_attitude_setpointVTOL 固定翼虚拟控制器通过同一结构支撑不同控制链路的解耦与 VTOL 过渡协调生产链路位置控制ControlMath::thrustToAttitude、手飞姿态生成generate_attitude_setpoint、MAVLink Offboardfill_thrust按机型分派三路写入消费链路多旋翼/固定翼姿态控制器、VTOL 协调器、日志与 MAVLink 流共同订阅。配合 docs/en/msg_docs/VehicleAttitudeSetpoint.md、msg/versioned/VehicleAttitudeSetpoint.msg 与上文列出的各实现文件你可以完整追踪一条设定值从产生、传输到被控制器消费的全过程为二次开发、调参与日志分析打下坚实基础。赞分享嵌入式物联网机器人自动驾驶智能硬件【免费下载链接】PX4-AutopilotPX4 Autopilot Software项目地址https://gitcode.com/gh_mirrors/px/PX4-Autopilot点击查看免费下载相关推荐PX4 固定翼“8 字盘旋”状态消息 FigureEightStatus 深度解析字段含义、发布链路与源码实现PX4 固定翼“8 字盘旋”状态消息 FigureEightStatus 深度解析字段含义、发布链路与源码实现 本文以 PX4 Autopilot 仓库中的嵌入式物联网机器人自动驾驶智能硬件PX4 TecsStatus uORB 消息完全指南固定翼 TECS 能量控制的状态观测字段与源码链路解析PX4 TecsStatus uORB 消息完全指南固定翼 TECS 能量控制的状态观测字段与源码链路解析 导读 TecsStatus 是 PX4 自动驾驶仪嵌入式物联网机器人自动驾驶智能硬件PX4-Autopilot 固定翼横向设定点消息 FixedWingLateralSetpoint字段语义、NAN 优先级与控制链路解析PX4 Autopilot 固定翼横向设定点消息 FixedWingLateralSetpoint字段语义、NAN 优先级与控制链路解析 本篇技术指南以 PX嵌入式物联网机器人自动驾驶智能硬件上一篇颠覆传统You-Dont-Need-JavaScript如何用纯CSS替代JavaScript实现惊艳动画下一篇Bili.Copilot分区浏览指南高效探索B站内容的终极方法创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考