(二)Ardupilot软件分析及代码架构

本文详细介绍了无人机飞行控制系统的工作原理,包括坐标系转换、姿态数据获取、传感器校正以及PID控制。核心库如AP_AHRS采用DCM或EKF估算飞行器姿态,而AP_HAL提供硬件抽象层。传感器库如AP_InertialSensor和AP_Compass负责读取陀螺仪和地磁数据。此外,还提到了飞行模式、导航库和电机控制等相关组件,揭示了无人机稳定飞行背后的复杂技术细节。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

先要搞明白ardupilot是怎么实现飞行控制的?然后再看文件

1.建立两个基本坐标系:地理坐标系和载体坐标系,保证两个基本坐标系的正确转化
   一般使用旋转矩阵实现坐标系转换:
   四元数运算(q0123);欧拉角(XYZ轴-roll/pitch/yaw) ;方向余弦矩阵(9个系数)

2.主要姿态数据:重力,地磁,陀螺仪,加速度计,电子罗盘,
                       重力和地磁属于地理坐标系/陀螺仪和加速度计和罗盘属于载体坐标系;
                      这些数据由UAV上面的传感器测量出来,exp:加速度计->测量线速度
                                                                                         陀螺仪->测量角速度

3.怎么保证坐标系的正确转化?
偏航yaw由电子罗盘,地磁对比修正误差补偿得到;
pitch俯仰和roll横滚的修正由加速度计,重力对比修正误差得到;
保证两个坐标系正确转化后,利用陀螺仪积分运算得到载体坐标系的姿态数据;
经过PID控制给出一系列控制量,完成基于载体坐标系的稳定控制,
再反应到地理坐标系稳定控制,达到可观察的定高/偏航/翻滚/俯仰等动作;

4.导航姿态理论上可只用陀螺仪完成,但是由于陀螺仪积分过程中产生误差累积,以及
  白噪声,温度偏差,需要进行修正补偿。
  1.需要用加速度计在水平面对重力进行对比和补偿,
  2.需要用电子罗盘在水平面的地磁方向修正陀螺仪的水平误差。
  加速度计:测量地球上的重力加速度,如果uav沿z轴旋转,则无法感知;
  电子罗盘:测量地球磁场方向,如果uav沿y轴旋转,则无法感知;

5.航姿系统的缺陷所在:磁场和重力场越正交效果越好,如果方向相同,例如南北极,
  则AHRS(加速度计,磁场计,陀螺仪)无法正常工作,高纬度航线角越来越大;

基本结构层次图:

再看主要文件以及其功能介绍

(1)飞行模式

(2)核心库
AP_AHRS:采用DCM(方向余弦矩阵方法)或EKF(扩展卡尔曼滤波方法)预估飞行器姿态
AP_Common:所有执行文件(sketch格式,arduino IDE的文件)和其他库都需要的基础核心库。
AP_Math:包含了许多数学函数,特别对于矢量运算。
AC_PID:PID控制器库。
AP_InertialNav:扩展带有gps和气压计数据的惯性导航库。
AC_AttitudeControl:姿态控制相关库。
AP_WPNav:航点相关的导航库。
AP_Motors:多旋翼和传统直升机混合的电机库。
RC_Channel:更多的关于从APM_RC的PWM输入/输出数据转换到内部通用单位的库,比如角度。
AP_HAL,AP_HAL_AVR,AP_HAL_PX4:硬件抽象层库,提供给其他高级控制代码一致的接口,而不必担心底层不同的硬件。AP_HAL_PX4:GPIO、I2C、UART、RCinput/output、scheduler、semaphores、storage。
(3)传感器相关库
AP_InertialSensor:读取陀螺仪和加速度计数据,并向主程序执行标准程序和提供标准单位数据(deg/s,m/s)。
AP_RangerFinder:声呐和红外测距传感器的交互库
AP_Baro:气压计相关库
AP_GPS:GPS相关库
AP_Compass:三轴罗盘相关库
AP_OpticalFlow:光流传感器相关库
(4)其他库
AP_Mount,AP_Camera, AP_Relay:相机安装控制库,相机快门控制库
AP_Mission: 从eeprom(电可擦只读存储器)存储/读取飞行指令相关库
AP_Buffer:惯性导航时所用到的一个简单的堆栈(FIFO,先进先出)缓冲区
AP_AccelCal、AP_Declination、AP_RCMapper、AP_RPM、AP_RSSI
AP_ADC:Analog to Digital
APM_Control: pitch/roll/yaw controller
DataFlash:flash memory
GCS_Console/GCS_MAVLink:地面站通信、飞行日志
 

2.代码架构图:

这里写图片描述

参考:https://blog.youkuaiyun.com/qq_21842557/article/details/50815000

<think>好的,用户之前询问了ArduPilot的主循环代码,现在他们想了解主循环之外的低优先级线程。首先,我需要回顾之前的对话,确保回答的连贯性。用户已经知道主循环的结构,包括无限循环、回调函数、调度延迟和看门狗机制。现在他们想了解其他低优先级的线程,这可能涉及到ArduPilot的多任务处理机制。 首先,我应该确定ArduPilot的线程模型。ArduPilot主要运行在实时操作系统(如ChibiOS)或裸机上,但在某些平台上可能使用多线程。不过,主循环通常是协作式多任务,而低优先级线程可能是通过任务调度器实现的。需要区分不同的运行环境,比如是否有RTOS支持。 接下来,用户提到的“其他低优先级线程”可能指的是主循环之外的后台任务,比如日志记录、通信处理等。这些任务通常不需要高实时性,但需要定期执行。在ArduPilot中,可能通过Scheduler来管理这些任务,每个任务有自己的执行频率和优先级。 需要注意,ArduPilot的设计可能因平台而异。有些平台使用多线程,而有些则使用单线程配合协作式调度。需要说明这一点,避免用户混淆。例如,在ChibiOS上,可能有多个实时线程,而在Linux上可能使用不同的机制。 另外,主循环中的g_callbacks->loop()可能负责处理高优先级任务,比如传感器数据的读取和处理,而低优先级任务可能被分配到其他线程或通过调度器在空闲时间执行。需要解释调度器如何平衡这些任务的执行时间,尤其是在主循环中主动延迟50微秒的部分,这是为了给低优先级任务让出CPU时间。 用户可能想知道这些低优先级线程的具体例子,比如日志记录、参数保存、遥测数据传输等。需要列举一些常见的低优先级任务,并说明它们如何被调度执行。 还要提到看门狗的作用,确保系统不会因为某个任务卡死而导致整个系统崩溃。主循环中的watchdog_pat()是喂狗操作,但其他线程可能也有自己的看门狗机制,或者由主循环统一管理。 最后,需要总结ArduPilot如何处理多任务,强调实时性和可靠性的设计,以及不同优先级任务之间的协作。确保用户理解主循环和低优先级线程如何协同工作,保证系统的稳定运行。</think>这是一个关于ArduPilot实时任务调度的关键问题。其线程架构设计遵循典型的**优先级分层+协作式调度**原则,以下是详细解析: --- ### **1. 主循环(高优先级核心任务)** ```cpp while (true) { g_callbacks->loop(); // 执行飞行控制核心逻辑 schedulerInstance.watchdog_pat(); } ``` - **实时性等级**:最高优先级(通常以1000Hz+频率运行) - **关键任务**: - IMU传感器数据采集(惯性测量单元) - 姿态解算(四元数/欧拉角计算) - PID控制器运算 - 舵机/PWM信号输出 - **特性**:必须保证**硬实时性**(执行时间严格可控) --- ### **2. 低优先级线程(协作式任务调度)** 通过`AP_Scheduler`系统管理,采用**时间片轮转+优先级插队**机制: #### **调度器配置示例** ```cpp AP_Scheduler::Task scheduler_tasks[] = { { .function = update_logging, .interval_ms = 10 }, // 日志记录 { .function = check_serial, .interval_ms = 20 }, // 串口通信 { .function = gps_update, .interval_ms = 100 }, // GPS数据处理 { .function = param_save, .interval_ms = 5000 } // 参数保存 }; ``` - **执行策略**: - 主循环每次迭代调用`scheduler.run()` - 检查每个任务的`interval_ms`是否到期 - **动态时间预算分配**(避免阻塞主循环) #### **典型低优先级任务** | 任务类型 | 执行频率 | 最大允许延迟 | 功能描述 | |-------------------|------------|--------------|------------------------------| | 数据日志记录 | 10-100Hz | 50ms | 存储飞行数据到SD卡 | | 遥测传输 | 20-50Hz | 100ms | 与地面站通信(MAVLink协议) | | 参数管理 | 0.2-1Hz | 1s | 保存/加载飞行参数 | | 状态LED控制 | 5-10Hz | 200ms | 系统状态指示灯控制 | | 电池监测 | 1-5Hz | 500ms | 电压/电流检测与低电量预警 | --- ### **3. 关键调度技术** #### **a) 时间片预留机制** ```cpp // 在每次主循环开始前预留时间窗口 uint16_t time_available = scheduler.time_available_usec(); if (time_available > 0) { scheduler.run(time_available); // 严格限制低优先级任务执行时间 } ``` - **数学保证**:设主循环周期为$T_{main}$,则低优先级任务最大可用时间为: $$ T_{low} = T_{main} - T_{core} - T_{margin} $$ 其中$T_{core}$为核心任务耗时,$T_{margin}$为安全裕量 #### **b) 优先级反转防御** ```cpp // 在关键代码段禁止任务切换 WITH_SEMAPHORE(_data_sem) { // 操作共享数据 } ``` - 使用信号量保护共享资源 - 通过`HAL_NVIC_SystemReset()`实现看门狗级错误恢复 --- ### **4. 实时性能指标** 以常见飞控硬件(如Pixhawk 4)为例: | 指标 | 主循环任务 | 低优先级任务 | |---------------------|------------|--------------------| | 最小周期 | 250μs | 1ms | | 时间抖动(Jitter) | <5μs | <50μs | | CPU占用率 | 60-70% | 10-20% | | 最坏执行时间(WCET)| 严格校准 | 统计分析法保证 | --- ### **5. 设计哲学** 1. **实时性分层**:飞行控制必须满足$\tau_{response} < 1ms$的硬实时要求 2. **资源隔离**:通过内存池(Memory Pool)为关键任务预留资源 3. **故障遏制**:看门狗分级机制(独立硬件WDT + 软件任务级WDT) 这种架构使得ArduPilot能在单核Cortex-M7处理器上同时处理200+个不同优先级的任务,同时保证飞行控制的绝对实时性。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值