ArduPilot性能优化与部署 - 第2节

实时操作系统(RTOS)基础：ChibiOS在ArduPilot中的应用

说实话，很多做飞控开发的朋友一开始都栽在同一个坑里——把飞控代码当单片机裸机程序写。你想想看，一个四旋翼要在空中做姿态解算、传感器融合、控制输出、GPS处理、遥控器信号解析……这些任务如果挤在一个大循环里排队，稍微有个传感器卡顿，飞机就翻了。

这就是为什么ArduPilot选择了ChibiOS。我参与ArduPilot开发那会儿，正好赶上从NutTX向ChibiOS迁移的阶段。当时团队里争论得很厉害，有人觉得换RTOS成本太高。但后来大家发现，ChibiOS的实时性确实更适合飞控这种硬实时场景。

ChibiOS为什么适合飞控？

我最早接触ChibiOS是在一个工业无人机项目上。当时客户要求飞控必须在1kHz的控制频率下稳定运行，同时还要处理4路串口数据、2路I2C传感器和一路摄像头触发信号。裸机方案根本扛不住，中断嵌套能把人逼疯。

ChibiOS有几个特性特别适合飞控场景：

• 抢占式调度——高优先级任务可以打断低优先级任务，保证控制环路不被阻塞
• 极低的中断延迟——从中断触发到任务唤醒，通常只需要几微秒
• 静态内存分配——没有malloc/free，避免内存碎片
• 丰富的同步原语——信号量、互斥锁、事件标志、消息队列一应俱全
• 可裁剪内核——不需要的功能可以编译时去掉，节省Flash和RAM

任务调度与优先级

RTOS的核心就是任务调度。说白了，就是决定"谁先跑、谁后跑、谁跑多久"。ChibiOS用的是优先级抢占式调度，加上时间片轮转。

优先级抢占的意思是：如果一个高优先级任务就绪了，它会立刻抢走CPU，不管低优先级任务跑没跑完。时间片轮转的意思是：同优先级的任务，大家轮流跑，每人分一个时间片。

在ArduPilot中，任务被分成了几个优先级等级：

优先级等级	典型任务	周期	说明
最高	IO任务、RC输入	1ms	必须实时响应，不能有任何延迟
高	姿态控制、传感器读取	1-2.5ms	控制环路的核心，延迟会导致震荡
中	GPS处理、导航	10-50ms	可以容忍少量抖动
低	日志写入、参数存储	100ms+	后台任务，CPU空闲时才跑
空闲	空闲钩子、低功耗	-	没有任务时执行

你可能会问：为什么IO任务优先级最高？我记得有一次调试一个飞控，发现遥控器信号偶尔会丢失几百毫秒。查了半天，原来是IO任务被其他任务阻塞了。从那以后我就明白了——遥控器信号是飞手和飞机之间的生命线，优先级必须最高。

任务创建与配置

在ChibiOS中，任务通过chThdCreateStatic()创建。ArduPilot封装了一层，你可以在AP_Scheduler中注册任务：

// ArduPilot中的任务注册示例
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    // 任务名          函数              周期(us)  优先级
    { &copter.rc_loop,          &Copter::rc_loop,          1000,  1 },
    { &copter.fast_loop,        &Copter::fast_loop,        2500,  2 },
    { &copter.gps_update,      &Copter::gps_update,      10000,  3 },
    { &copter.update_logging,  &Copter::update_logging,  50000,  4 },
};

这里有个细节：周期是微秒为单位，优先级数字越小优先级越高。我刚开始用的时候搞反了，把日志任务优先级设成了1，结果控制任务跑不出来，飞机直接炸了。嗯，这个教训挺深刻的。

线程间通信

任务之间需要交换数据，但直接访问全局变量是不安全的。ChibiOS提供了几种线程间通信机制，ArduPilot里用得最多的是队列、信号量和互斥锁。

消息队列

队列用于任务之间的数据传递。一个任务往队列里放数据，另一个任务从队列里取数据。队列是FIFO的，先进先出。

在ArduPilot中，传感器数据经常通过队列传递。比如IMU数据从读取任务传到姿态解算任务：

// 创建队列
static msg_queue_t imu_queue;
static uint8_t imu_queue_buffer[10 * sizeof(Vector3f)];
chMQObjectInit(&imu_queue, imu_queue_buffer, sizeof(Vector3f), 10);

// 发送任务
Vector3f accel_data = read_accel();
chMQSend(&imu_queue, (msg_t)&accel_data, TIME_IMMEDIATE);

// 接收任务
Vector3f received;
chMQReceive(&imu_queue, (msg_t)&received, TIME_INFINITE);

我个人习惯用队列来解耦生产者和消费者。这样即使生产者跑得比消费者快，数据也不会丢失——队列满了就阻塞生产者，或者丢弃旧数据。

信号量

信号量用于任务同步。最常见的场景是：一个任务等待某个事件发生，另一个任务通知它"事件来了"。

在ArduPilot中，信号量常用于中断和任务之间的同步。比如一个外部中断触发了，中断服务程序里释放信号量，等待该信号量的任务被唤醒：

// 创建二进制信号量
static binary_semaphore_t ext_int_sem;
chBSemObjectInit(&ext_int_sem, false);

// 中断服务程序
void ext_interrupt_handler(void) {
    chSysLockFromISR();
    chBSemSignalI(&ext_int_sem);  // 释放信号量
    chSysUnlockFromISR();
}

// 等待任务
void wait_for_interrupt_task(void) {
    chBSemWait(&ext_int_sem);  // 阻塞等待
    // 事件发生，开始处理
}

互斥锁

互斥锁用于保护共享资源。当多个任务需要访问同一个变量或外设时，互斥锁确保同一时间只有一个任务能访问。

ArduPilot中，I2C总线的访问就用了互斥锁。因为多个传感器可能挂在同一条I2C总线上，如果两个任务同时发起I2C通信，数据就乱套了：

// 创建互斥锁
static mutex_t i2c_mutex;
chMtxObjectInit(&i2c_mutex);

// 任务A：读取气压计
chMtxLock(&i2c_mutex);
read_barometer();  // 占用I2C总线
chMtxUnlock(&i2c_mutex);

// 任务B：读取磁力计
chMtxLock(&i2c_mutex);
read_compass();    // 等待任务A释放
chMtxUnlock(&i2c_mutex);

中断管理

中断是实时系统的灵魂。飞控里几乎所有传感器数据都是通过中断通知CPU的——IMU数据准备好、GPS数据到达、遥控器信号变化……

ChibiOS的中断管理有几个原则：

• 中断服务程序要短——只做最必要的事，比如读取数据、释放信号量
• 中断里不能调用阻塞函数——比如chThdSleep()、chMQReceive()
• 中断里必须用FromISR版本的API——比如chSysLockFromISR()、chBSemSignalI()
• 中断优先级要合理设置——飞控里通常把定时器中断设最高，外设中断次之

在ArduPilot中，中断处理通常采用"上半部+下半部"的模式。上半部在中断上下文执行，只做快速处理；下半部在任务上下文执行，做耗时操作：

// 上半部：中断服务程序
void timer_interrupt_handler(void) {
    chSysLockFromISR();
    // 读取定时器值
    uint32_t timestamp = get_timer_value();
    // 释放信号量，唤醒下半部任务
    chBSemSignalI(&timer_sem);
    chSysUnlockFromISR();
}

// 下半部：任务
void timer_processing_task(void) {
    while(true) {
        chBSemWait(&timer_sem);
        // 在这里做耗时处理
        process_timer_data();
    }
}

为什么要分上半部和下半部？因为中断服务程序里不能做太多事情，否则会影响其他中断的响应。把耗时操作放到任务里，可以让中断尽快返回。

我记得有一次调试一个四旋翼，发现飞机在悬停时会周期性地抖动。用逻辑分析仪抓了中断响应时间，发现定时器中断偶尔会被其他中断阻塞长达200微秒。后来把定时器中断优先级提到最高，问题就解决了。嗯，中断优先级这个事，真的不能马虎。