PX4-Autopilot代码解析（12）-飞行模式管理器

最新推荐文章于 2025-11-25 10:20:59 发布

原创

最新推荐文章于 2025-11-25 10:20:59 发布 · 832 阅读

22 ·

CC 4.0 BY-SA版权

文章标签：

#飞控 #PX4 #系统架构 #无人机

飞行模式管理器是 PX4 的核心模块，负责协调飞行任务并生成用于位置控制的轨迹设定点。它作为指挥官模块和导航器模块的高级导航命令与低级位置控制器之间的主要接口。该类根据FlightModeManager飞行器状态选择和管理适当的飞行任务，然后发布trajectory_setpoint_s消息供位置控制模块使用。

架构概述

FlightModeManager系统架构

该类FlightModeManager通过基于工作队列的执行模型来协调飞行任务。主要执行发生在方法中，该方法由50 Hz 的更新Run()触发。vehicle_local_position

操作周期

飞行模式管理器以连续循环的方式运行：

监控车辆状态更新（位置、状态、控制模式）
根据当前飞行器状态确定应激活哪个飞行任务
如果需要，启动适当的飞行任务
处理任何车辆命令
调用活动任务来生成轨迹设定点
发布位置控制器生成的设定点

每次迭代都会在vehicle_local_position通过接收更新时发生_vehicle_local_position_sub，根据 20ms 间隔设置限制为 50 Hz。

FlightModeManager::Run() 方法执行流程

飞行任务层级

飞行任务以分层继承结构进行组织，允许专门的行为，同时共享通用功能。

飞行任务选择

飞行模式管理器根据当前飞行器状态选择合适的飞行任务，主要由指挥官模块提供的导航状态决定。

任务选择逻辑如下：

start_flight_task() 任务选择逻辑

任务激活和停用

在飞行任务之间切换时，会发生以下序列：

当前任务（如果有）被停用并销毁
新任务已实例化
该activate()方法使用最后一个轨迹设定点进行调用
任务初始化通过以下方式验证updateInitialize()

如果任何步骤失败，系统将退回到故障安全任务或禁用所有任务。

任务切换实现

实际switchTask()方法通过保留先前的轨迹设定点并确保适当的清理来处理任务转换：

FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
{
    // switch to the running task, nothing to do
    if (new_task_index == _current_task.index) {
        return FlightTaskError::NoError;
    }

    // Save current setpoints for the next FlightTask
    trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;

    if (isAnyTaskActive()) {
        last_setpoint = _curr