PX4-Autopilot 中 FixedWingLongitudinalSetpoint 消息详解：固定翼纵向（高度/速度/姿态）控制指令的 UORB 接口

导读

本文围绕 PX4-Autopilot 中固定翼纵向控制的核心 UORB 消息 FixedWingLongitudinalSetpoint（话题 fixed_wing_longitudinal_setpoint）展开，讲解其字段定义、NAN 语义、发布方（fw_mode_manager）与消费方（fw_lateral_longitudinal_control）的完整调用链，以及 TECS 总能量控制与直接俯仰/油门控制的切换逻辑。读完本文，你将掌握该消息在固定翼自动飞行与手动飞行各模式下的取值约定，并能在调试纵向控制、二次开发新飞行模式时正确读写这一消息。

消息概览与在飞控链路中的位置

FixedWingLongitudinalSetpoint 是固定翼（含 VTOL 固定翼模式）纵向通道的控制指令消息。根据 msg/versioned/FixedWingLongitudinalSetpoint.msg 中的注释，它由 fw_lateral_longitudinal_control 模块使用，负责把"要飞多高、爬升/下降多快、飞多快、或直接给定俯仰/油门"等纵向期望传递给底层姿态与推力控制器。

整条数据流为：

1. 发布方 fw_mode_manager（FixedWingModeManager.hpp 中 _longitudinal_ctrl_sp_pub）根据当前飞行模式（idle、自动定高、自动下降、起飞、着陆、手动等）计算纵向期望，发布该消息；
2. 消费方 fw_lateral_longitudinal_control（FwLateralLongitudinalControl.hpp 中 _fw_longitudinal_ctrl_sub）订阅该消息，结合 longitudinal_control_configuration 配置消息驱动 TECS 输出俯仰/油门设定值，或直接透传 pitch_direct / throttle_direct。

该消息同时被记录进飞行日志（见 src/modules/logger/logged_topics.cpp）并通过 uXRCE-DDS 桥接到 ROS 2（见 src/modules/uxrce_dds_client/dds_topics.yaml），便于地面站与外部程序观测。

字段定义与语义（完整字段表）

消息定义位于 msg/versioned/FixedWingLongitudinalSetpoint.msg，共 5 个数据字段加 1 个常量。官方文档（docs/en/msg_docs/FixedWingLongitudinalSetpoint.md）给出的字段说明如下：

字段名	类型	单位 [坐标系]	范围/枚举	说明
timestamp	uint64	us	—	系统启动以来的时间
altitude	float32	m	—	AMSL 高度设定值；若为 NAN 或 height_rate 有效（finite），则不直接控制高度
height_rate	float32	m/s [ENU]	—	标量高度变化率设定值；若不直接控制则为 NAN
equivalent_airspeed	float32	m/s	[0 : inf]	标量等效空速设定值；NAN 表示使用系统默认值
pitch_direct	float32	rad [FRD]	[-pi : pi]	不控制时为 NAN；有效时覆盖 TECS 总能量控制器
throttle_direct	float32	norm	[0 : 1]	不控制时为 NAN；有效时覆盖 TECS 总能量控制器

常量

名称	类型	值
MESSAGE_VERSION	uint32	0

NAN 语义：消息的"软开关"设计

这是理解该消息的关键：几乎所有纵向期望字段都以 NAN 作为"不参与控制"的标记，而不是使用独立布尔标志。

• 若 pitch_direct 与 throttle_direct 不同时为有限值，则控制器依赖 altitude / height_rate 与 equivalent_airspeed 来控制垂直运动（走 TECS 路径）；
• 若 altitude 与 height_rate 同时为 NAN，控制器保持当前高度；
• 若 equivalent_airspeed 为 NAN，则使用系统默认空速设定值。

这一约定在源码中有一致的落地：

• FwLateralLongitudinalControl.hpp 定义了空指令默认值：{.timestamp = 0, .altitude = NAN, .height_rate = NAN, .equivalent_airspeed = NAN, .pitch_direct = NAN, .throttle_direct = NAN}；
• FixedWingModeManager.cpp 也定义了同样的 empty_longitudinal_control_setpoint，供各模式在"只改部分字段"时作为基座复用。

消费方实现：TECS 与直接控制的切换

在 fw_lateral_longitudinal_control 的主循环中（FwLateralLongitudinalControl.cpp）：

if (_fw_longitudinal_ctrl_sub.updated()) {
    _fw_longitudinal_ctrl_sub.copy(&_long_control_sp);
}

// 空速设定值（含最小空速与风补偿的自适应）
const float airspeed_sp_eas = adapt_airspeed_setpoint(control_interval, _long_control_sp.equivalent_airspeed,
                  _min_airspeed_from_guidance, _lateral_control_state.wind_speed.length());

// 高度/高度率互斥：height_rate 有效时 altitude 置为 NAN
const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude;

current_flight_phase = tecs_update_pitch_throttle(
                   control_interval, altitude_sp, airspeed_sp_eas,
                   _long_configuration.pitch_min, _long_configuration.pitch_max,
                   _long_configuration.throttle_min, _long_configuration.throttle_max,
                   _long_configuration.sink_rate_target, _long_configuration.climb_rate_target,
                   _long_configuration.disable_underspeed_protection,
                   _long_control_sp.height_rate, now);

// 直接俯仰/油门：优先于 TECS 输出（TECS 内已扣除 trim，此处加回）
pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct)
           ? _long_control_sp.pitch_direct
           : _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());

throttle_sp = PX4_ISFINITE(_long_control_sp.throttle_direct) ? _long_control_sp.throttle_direct :
              _tecs.get_throttle_setpoint();

可见两条纵向控制通路：

• TECS 通路：altitude / height_rate + equivalent_airspeed 输入 tecs_update_pitch_throttle，由总能量控制器（TECS）同时解算俯仰与油门，并在解算时受 longitudinal_control_configuration 中的 pitch_min/pitch_max、throttle_min/throttle_max、sink_rate_target、climb_rate_target、disable_underspeed_protection 等配置约束；
• 直接通路：只要 pitch_direct / throttle_direct 为有限值，就直接覆盖 TECS 输出，用于需要绕开能量控制器的特殊工况（如失速改出、特定测试、外部接管）。

发布方实现：各飞行模式的典型取值

fw_mode_manager 在 FixedWingModeManager.cpp 中为不同模式构造并发布该消息，以下是可验证的代表性模式：

1. Idle（待机/未起飞）

control_idle()：基于空指令只填充 timestamp，并将 pitch_direct = 0、throttle_direct = 0 发布，同时把油门上限/下限配置为 0，确保地面待机时无推力输出：

fixed_wing_longitudinal_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint};
long_contrl_sp.timestamp = now;
long_contrl_sp.pitch_direct = 0.f;
long_contrl_sp.throttle_direct = 0.0f;
_longitudinal_ctrl_sp_pub.publish(long_contrl_sp);

2. 自动定高盘旋（auto fixed bank alt hold）

control_auto_fixed_bank_alt_hold()：height_rate = 0、其余为 NAN，即"维持当前高度"，空速交给系统默认，由 TECS 控制：

const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
    .timestamp = now,
    .altitude = NAN,
    .height_rate = 0.f,
    .equivalent_airspeed = NAN,
    .pitch_direct = NAN,
    .throttle_direct = NAN
};

若垂直位置或垂直速度估计无效（!_local_pos.z_valid || !_local_pos.v_z_valid），则把油门上限强制降为 FW_THR_MIN，防止"越飞越高"（climb-away）。

3. 自动下降（auto descend）

control_auto_descend()：硬编码下降率 0.5 m/s，即 height_rate = -0.5f，作为失速/异常恢复时的折衷策略。

4. 其他模式

源码中 equivalent_airspeed 的赋值位置（FixedWingModeManager.cpp）还揭示了各模式对空速的期望：

• 自动巡航、RTL 等模式使用 target_airspeed（由导航层给定的目标空速）；
• 起飞阶段使用 takeoff_airspeed（对应参数如 FW_AIRSPD_MIN 相关配置）；
• 着陆阶段使用 airspeed_land（着陆空速）；
• 手动模式使用 get_manual_airspeed_setpoint()（由遥控器/参数映射得到的手动空速期望）。

消息与其他话题、外部系统的关系

• 日志记录：fixed_wing_longitudinal_setpoint 被列入飞行日志的已记录话题（见 src/modules/logger/logged_topics.cpp），可通过飞行日志回放分析纵向指令是否正确产生；
• ROS 2 / DDS 桥接：该话题在 src/modules/uxrce_dds_client/dds_topics.yaml 中被声明为桥接话题，外部 ROS 2 节点可订阅同名话题观测 PX4 的纵向期望值；
• 消息版本管理：消息同时存在于 msg/versioned/FixedWingLongitudinalSetpoint.msg 与 msg/CMakeLists.txt 的生成列表中，构建时由 Tools/msg/px_generate_uorb_topic_files.py 生成 uORB/topics/fixed_wing_longitudinal_setpoint.h 等头文件，供各模块 #include 使用。

调试与使用建议

1. 判读纵向控制来源：在日志中同时查看 fixed_wing_longitudinal_setpoint 与 tecs_status、vehicle_attitude_setpoint。若 pitch_direct/throttle_direct 为 NAN，说明纵向由 TECS 控制；反之说明被直接指令接管。
2. 注意互斥规则：height_rate 有效时 altitude 会被消费方强制忽略（PX4_ISFINITE(height_rate) ? NAN : altitude），因此发布方不必同时置空两个字段，消费方已兜底。
3. 空速单位：equivalent_airspeed 使用等效空速（EAS）而非指示空速（IAS），与 airspeed_validated 话题保持一致；直接比较时需确认单位统一。
4. 时间戳一致性：所有字段的 timestamp 均取 hrt_absolute_time()（微秒），消费方仅依赖消息到达顺序，不额外做时间对齐，因此发布方务必每次填充最新时间戳。

总结

FixedWingLongitudinalSetpoint 是 PX4 固定翼纵向控制链路上承飞行模式决策、下启 TECS 姿态/推力执行的"指令总线"。其 NAN 语义设计使各模式可以按需只填充部分字段，配合 pitch_direct/throttle_direct 的直接覆盖通道，兼顾了常规能量控制与特殊工况旁路两种需求。理解该消息的字段约定与发布/消费实现（msg/versioned/FixedWingLongitudinalSetpoint.msg、FixedWingModeManager.cpp、FwLateralLongitudinalControl.cpp），是进行固定翼纵向控制调参与二次开发的基础。