一、系统启动流程全景图

PX4的启动过程可以清晰地分为三个层次：

Bootloader阶段 → NuttX RTOS启动 → PX4中间件与应用启动

1.1 Bootloader阶段

当飞行控制器上电时，首先执行的是固化在芯片内部的Bootloader程序。

核心功能：

• 基础硬件初始化（时钟、内存、存储）

• 加载并验证主固件

• 跳转到固件入口

关键代码：

// platforms/nuttx/NuttX/nuttx/arch/arm/src/stm32_h7/stm32_h7b3xx_boot.c
void __start(void)
{
    /* 初始化堆栈指针 */
    __asm__ __volatile__("\tldr sp, =_estack\n");
    
    /* 清零BSS段 */
    memset(__bss_start, 0, __bss_end - __bss_start);
    
    /* 复制数据段到RAM */
    memcpy(__data_start, __data_load, __data_end - __data_start);
    
    /* 板级初始化 */
    stm32_boardinitialize();
    
    /* 跳转到主固件 */
    nx_start();
}

1.2 NuttX RTOS启动

Bootloader跳转后，NuttX实时操作系统开始初始化。

初始化流程：

// nuttx/sched/init/nx_start.c
void nx_start(void)
{
    /* 1. 初始化系统数据结构 */
    sched_initialize();
    
    /* 2. 创建空闲任务 */
    idle_task_create();
    
    /* 3. 初始化设备驱动 */
    drivers_initialize();  // 这里调用board_app_initialize()
    
    /* 4. 创建init任务 */
    task_create("init", PRIO_INIT, STACK_INIT, init_task, NULL);
}

关键转折点 - board_app_initialize()：

// platforms/nuttx/src/px4/stm/px4fmu_common/stm32_boardinit.c
int board_app_initialize(void)
{
    /* 初始化硬件外设 */
    stm32_serial_initialize();  // UART控制台
    stm32_spi_initialize();     // SPI总线
    stm32_i2c_initialize();     // I2C总线
    stm32_pwm_initialize();     // PWM输出
    stm32_adc_initialize();     // ADC采集
    
    /* 挂载文件系统 */
    mount("/dev/ram0", "/etc", "romfs", 0, NULL);
    mount("/dev/mmcsd0", "/fs/microsd", "vfat", 0, NULL);
    
    /* 创建PX4主任务 - 这才是真正的入口点！ */
    px4_task_spawn_cmd("init",
                       SCHED_DEFAULT,
                       SCHED_PRIORITY_DEFAULT,
                       CONFIG_PTHREAD_STACK_DEFAULT,
                       px4_main,
                       (char * const *)NULL);
    
    return OK;
}

二、PX4中间件启动

2.1 px4_main() - PX4世界的大门

// src/platforms/common/px4_common_init.cpp
int px4_main(int argc, char *argv[])
{
    /* 1. 初始化PX4通用层 */
    px4::init(argc, argv, "px4");
    
    /* 2. 启动工作队列线程池 */
    work_queue_manager_start();
    
    /* 3. 执行启动脚本rcS - 这才是真正的核心！ */
    int ret = rc_script_run("/etc/init.d/rcS");
    
    /* 4. 进入主循环 */
    while (true) {
        px4_sleep(1);  // 1秒休眠，等待其他任务运行
    }
    
    return 0;
}

2.2 rcS脚本深度解析

rcS脚本位于ROMFS文件系统中，是PX4模块启动的"总指挥"。

# ROMFS/px4fmu_common/init.d/rcS
#!/bin/sh

# 1. 参数系统初始化
param select /fs/mtd_params
param load

# 2. 传感器驱动启动
if [ $AUTOCNF == yes ]; then
    # IMU驱动
    mpu6000 start
    bmp280 start
    hmc5883 start
    
    # GPS驱动
    gps start
    
    # 其他外设
    tone_alarm start
    rgbled start
fi

# 3. 姿态估计启动
ekf2 start

# 4. 控制器启动
mc_att_control start   # 多旋翼姿态控制
mc_pos_control start   # 多旋翼位置控制

# 5. 通信模块启动
mavlink start -d /dev/ttyS1  # USB
mavlink start -d /dev/ttyS2  # TELEM1

# 6. 核心任务启动
sensors start   # 传感器数据发布
commander start # 状态机与安全监控
logger start    # 数据日志记录
navigator start # 任务导航

三、后台运作机制

3.1 系统架构图

Commander (状态机)
uORB消息总线 (所有模块通过它通信)
传感器驱动	EKF2	控制器	MAVLink
(MPU6000)	(姿态估计)	(控制律)	(通信协议)
工作队列(Work Queue) - 处理耗时/阻塞任务
NuttX RTOS内核 - 任务调度、中断管理

3.2 uORB通信机制

uORB是PX4模块间通信的核心，采用发布-订阅模式。

消息定义示例：

# msg/vehicle_attitude.msg
uint64 timestamp        # 时间戳
float32[4] q            # 四元数
float32[4] delta_q_reset # 增量四元数
uint8 reset_counter     # 重置计数器

模块通信示例：

// 发布者：EKF2模块
void EKF2::publish_attitude()
{
    vehicle_attitude_s att;
    att.timestamp = hrt_absolute_time();
    matrix::Quatf q{_ekf.getQuaternion()};
    q.copyTo(att.q);
    
    _att_pub.publish(att);  // 发布到uORB总线
}

// 订阅者：姿态控制器
void MulticopterAttitudeControl::subscribe()
{
    _att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
    // 设置轮询fd
    _poll_fds[0].fd = _att_sub;
    _poll_fds[0].events = POLLIN;
}

void MulticopterAttitudeControl::run()
{
    while (!should_exit()) {
        // 等待新消息
        int ret = px4_poll(_poll_fds, 1, 1000);
        
        if (ret > 0 && (_poll_fds[0].revents & POLLIN)) {
            // 获取最新姿态
            orb_copy(ORB_ID(vehicle_attitude), 
                     _att_sub, &_att);
            
            // 执行控制律计算
            control_attitude();
        }
    }
}

3.3 工作队列(Work Queue)机制

工作队列用于处理不适合在主循环中执行的耗时任务。

// src/lib/work_queue/work_queue.h
struct work_s {
    struct work_s *next;     // 链表指针
    void (*worker)(void *arg); // 工作函数
    void *arg;                // 参数
    uint32_t qid;             // 队列ID
    uint32_t delay;           // 延迟时间
};

// 工作队列初始化
void work_queue_manager_start()
{
    /* 创建高优先级工作队列 */
    work_queue_create(&g_hpwork, "hpwork", 
                      PRIORITY_HPWORK, STACK_HPWORK);
    
    /* 创建低优先级工作队列 */
    work_queue_create(&g_lpwork, "lpwork", 
                      PRIORITY_LPWORK, STACK_LPWORK);
}

典型应用场景：

// 传感器驱动中的使用
class MPU6000 : public SPI
{
private:
    struct work_s _work;  // 工作项
    
    static void work_callback(void *arg)
    {
        MPU6000 *dev = (MPU6000 *)arg;
        dev->read_measurements();  // 读取传感器数据
        dev->publish();             // 发布到uORB
        
        /* 重新调度 */
        work_queue(HPWORK, &dev->_work, 
                  (worker_t)&MPU6000::work_callback,
                  dev, USEC2TICK(1000));  // 1kHz
    }
};

int MPU6000::start()
{
    /* 初始调度 */
    work_queue(HPWORK, &_work, 
              (worker_t)&MPU6000::work_callback,
              this, USEC2TICK(1000));
    return PX4_OK;
}

3.4 定时器机制

PX4提供了高精度定时器用于周期性任务。

// platforms/common/include/px4_platform_common/timer.h
typedef void (*px4_timer_callback_t)(void *arg);

int px4_timer_register(px4_timer_callback_t callback,
                       void *arg, unsigned int usec,
                       bool oneshot);

int px4_timer_unregister(px4_timer_callback_t callback,
                         void *arg);

实际应用：

// 用于PWM输出
static void pwm_timer_callback(void *arg)
{
    PWMOutput *pwm = (PWMOutput *)arg;
    
    /* 更新PWM占空比 */
    pwm->update_duty_cycle();
    
    /* 如果需要复杂处理，提交到工作队列 */
    work_queue(LPWORK, &pwm->_work, 
               pwm_work_callback, pwm, 0);
}

四、核心模块交互流程

4.1 传感器数据流

[硬件中断] → [IMU驱动] → [传感器模块] → [EKF2] → [控制器]
    ↓           ↓            ↓            ↓         ↓
  触发读取    读取原始    校准、融合    状态估计    控制计算
             数据

具体实现：

// 1. IMU驱动 - 中断处理
int mpu6000::interrupt_handler(int irq, void *context)
{
    /* 触发数据读取 */
    work_queue(HPWORK, &_work, 
               (worker_t)&mpu6000::read_data, this, 0);
    return 0;
}

// 2. 传感器模块订阅并处理
void Sensors::run()
{
    // 订阅原始IMU数据
    int accel_sub = orb_subscribe(ORB_ID(sensor_accel));
    
    while (true) {
        orb_copy(ORB_ID(sensor_accel), accel_sub, &accel);
        
        // 校准
        apply_calibration(accel);
        
        // 发布校准后的IMU数据
        _vehicle_imu_pub.publish(imu);
    }
}

// 3. EKF2订阅并融合
void Ekf2::run()
{
    int imu_sub = orb_subscribe(ORB_ID(vehicle_imu));
    
    while (true) {
        orb_copy(ORB_ID(vehicle_imu), imu_sub, &imu);
        
        // EKF更新
        _ekf.update(imu);
        
        // 发布估计结果
        publish_attitude();
        publish_local_position();
    }
}

4.2 控制流

[Commander] → [位置控制器] → [姿态控制器] → [混控器] → [PWM输出]
    ↓              ↓               ↓            ↓           ↓
 模式切换      位置设定值      姿态设定值     电机混控    执行器输出

控制循环示例：

// 姿态控制器主循环
void MulticopterAttitudeControl::Run()
{
    /* 等待新数据 */
    _att_sub.wait_for_update();
    
    /* 获取当前姿态 */
    orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
    
    /* 获取姿态设定值 */
    orb_copy(ORB_ID(vehicle_attitude_setpoint), 
             _att_sp_sub, &_att_sp);
    
    /* 计算角速率设定值 */
    matrix::Vector3f rates_sp = 
        attitude_controller(_att.q, _att_sp.q);
    
    /* 发布角速率设定值 */
    _rates_sp_pub.publish(rates_sp);
}

// 混控器处理
void Mixer::Run()
{
    /* 订阅控制指令 */
    orb_copy(ORB_ID(actuator_controls_0), 
             _ctrl_sub, &_ctrl);
    
    /* 混控计算：控制值 -> 电机PWM */
    float outputs[ACTUATOR_COUNT];
    _mixer->mix(_ctrl.control, outputs);
    
    /* 写入PWM设备 */
    ioctl(_pwm_fd, PWM_SET_OUTPUTS, 
          (unsigned long)outputs);
}

五、Commander状态机详解

Commander是PX4的最高级状态机，负责安全管理。

5.1 状态定义

// src/modules/commander/commander.h
enum main_state_t {
    MAIN_STATE_MANUAL,       // 手动模式
    MAIN_STATE_ALTCTL,       // 高度控制
    MAIN_STATE_POSCTL,       // 位置控制
    MAIN_STATE_AUTO_MISSION, // 自动任务
    MAIN_STATE_AUTO_LOITER,  // 自动悬停
    MAIN_STATE_AUTO_RTL,     // 自动返航
    MAIN_STATE_ACRO,         // 特技模式
    MAIN_STATE_OFFBOARD,     // 外部控制
    MAIN_STATE_STAB,         // 增稳模式
    MAIN_STATE_MAX
};

enum arming_state_t {
    ARMING_STATE_INIT,       // 初始化中
    ARMING_STATE_STANDBY,    // 待命(已上锁)
    ARMING_STATE_ARMED,      // 已解锁
    ARMING_STATE_ARMED_ERROR,// 解锁但有错误
    ARMING_STATE_STANDBY_ERROR,// 待命但有错误
    ARMING_STATE_REBOOT,     // 重启中
    ARMING_STATE_MAX
};

5.2 Commander主循环

// src/modules/commander/commander.cpp
void Commander::run()
{
    /* 订阅所需数据 */
    int battery_sub = orb_subscribe(ORB_ID(battery_status));
    int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
    int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
    
    while (!should_exit()) {
        /* 1. 更新状态 */
        update_arming_state();  // 检查解锁条件
        update_main_state();    // 根据RC/任务更新主模式
        
        /* 2. 安全检查 */
        check_battery_failsafe();   // 电池低电压保护
        check_gps_failsafe();       // GPS丢失保护
        check_rc_failsafe();        // 遥控器信号丢失保护
        check_geofence_failsafe();  // 电子围栏保护
        
        /* 3. 发布状态信息 */
        publish_vehicle_status();
        publish_vehicle_command_ack();
        
        /* 4. 处理命令 */
        handle_vehicle_commands();
        
        /* 5. 250ms循环 */
        usleep(250000);
    }
}

5.3 解锁条件检查

bool Commander::check_arming_allowed()
{
    /* 必须满足的条件 */
    if (!_system_calibrated) {
        return false;  // 未校准
    }
    
    if (_battery->voltage < BATTERY_LOW_THRESH) {
        return false;  // 电池电压过低
    }
    
    if (_sensor_failure) {
        return false;  // 传感器故障
    }
    
    /* 可选条件（根据参数） */
    if (_param_arm_without_gps.get() == 0 && !_gps_fix) {
        return false;  // 需要GPS但无定位
    }
    
    if (!_prearm_check_passed()) {
        return false;  // 预检未通过
    }
    
    return true;
}

六、总结与关键点

6.1 启动流程要点

1. Bootloader → NuttX → PX4 三层递进

2. board_app_initialize() 是硬件与PX4的桥梁

3. rcS脚本是应用启动的总指挥

4. 模块按依赖关系顺序启动：驱动→估计器→控制器→通信→Commander

6.2 后台运作要点

1. uORB 是模块间通信的"神经网络"

2. 工作队列 处理耗时任务，保证实时性

3. 定时器 驱动周期性任务

4. Commander 是系统的"大脑"，负责安全和状态管理

6.3 代码文件索引

功能	关键文件	说明
启动入口	stm32_boardinit.c	board_app_initialize()
PX4主入口	px4_common_init.cpp	px4_main()
启动脚本	ROMFS/init.d/rcS	模块启动顺序
uORB	src/modules/uORB/	消息总线
工作队列	src/lib/work_queue/	后台任务处理
Commander	src/modules/commander/	状态机与安全
EKF2	src/modules/ekf2/	姿态估计
控制器	src/modules/mc_att_control/	控制律

理解PX4的启动和运作机制，不仅是阅读代码，更要理解各模块如何通过uORB和工作队列协同工作。希望本文能帮助您更好地掌握PX4系统的精髓。