apm——2015.3.28.pdf

Ardupilot 石迪 简介 硬件配置 软件系统 简介 简介 四旋翼飞行器地面站 mission planner 简介 飞行器和地面站关系 1 实时监控飞行状态 2 更新飞控固件 调整控制参数 3 保存 下载飞行日志 硬件配置 硬件配置 机架遥控器 硬件配置 航电设备动力系统 软件系统 主循环 总调度 功能型程序 工具型程序 底层驱动 软件系统 主循环 软件系统 主循环 motors gyroaccelcompass barosonarGPS AHRS attitude heading reference system 航姿参考系统 位置姿态解算 姿态角 位置 姿态控制 位置 高度 控制 云台降落伞起落架喷雾器 光流 遥 控 器 输 入 软件系统 主循环 软件系统 功能型程序 软件系统 功能型程序 在主程序之下 是一些功能型程序 主要为飞行模式和飞行任务而服务 Control acro pde Control althold pde Control auto pde Control loiter pde Control stabilize pde Failsafe pde Fence pde 软件系统 功能型程序 程序执行流程 定点飞行模式 loiter mode为例 loop 初始 化条 件 N Y 解锁 初始化 target N Y 失控 保护 N Y 清除控 制输入 从遥控器获取 target Roll pitch yaw Climb rate 着陆 状态 准备复飞 初始化 Y N Loiter 自 动悬停控 制 软件系统 功能型程序 飞行任务执行流程 解锁 获取 Target roll pitch yaw cl imb rate 当climb rate 0 时 起飞 根据之前得到的 roll pitch yaw climb rate信息执行位置 姿 态 高度控制 当前状态偏离target时 loiter控制器自动控制 运行到target 着陆 软件系统 功能型程序 初始化程序 执行 条件 检查 初始 化 loiter init 通过 将当前位置 x y 设置 为目标位置 设置Z方向速度加 速度限制 以及z 方向悬停位置 不通过退出 G P S 光 流 忽 略 处理当前Z方向位置 以及 Z方向速度对位置的补偿 软件系统 功能型程序 static void loiter run 解锁 着陆遥控自控 失控保护 软件系统 功能型程序 loiter 未解锁状态 init loiter target 初始化悬停位置以及前馈补偿值 init xy controller 控制器初始化 将当前姿态设定为目标姿态 获取重力在x y上的投影 并作为积分初值 软件系统 功能型程序 loiter 未解锁状态 relax bf rate controller 将陀螺仪在bodyframe下三轴上的投影作为目标加速 度 软件系统 功能型程序 loiter 未解锁状态 set throttle ou t 0 false 切断电机 输出 motors set throttle t hrottle out 将PMW 信号输出 给电机 set throttle int16 t throttle in rc throttle servo o ut throttle in range 0 1000 软件系统 功能型程序 loiter 遥控状态 采用simple mode模式处理控制输入信号 简单模式相当于一个无头模式 每个 飞行模式 飞机将解锁起飞前的机头指向恒定作为遥控器前行摇杆的指向 将控制输入从机头指 向方位投影到地面坐 标系 将地面系上的控制输 入投影到机体系 软件系统 功能型程序 loiter 遥控状态 wp nav set pilot desired acceleration g rc 1 control in g rc 2 control in get pilot desired yaw rate g rc 4 control in 软件系统 功能型程序 loiter 遥控状态 get pilot desired climb rate 软件系统 功能型程序 loiter 着陆状态 loiter soften for landing ap land complete 将当前位置设置 为目标位置 冻结位置误差的 前馈补偿 保持油门输出 为复 飞做准备 软件系统 功能型程序 loiter 着陆状态 X y方向位置控制 姿态控制 高度控制 软件系统 功能型程序 loiter 自控状态 update loiter 计算积分步长 calc loiter desired velocity dt ekfGndSpdL imit pos control update xy controller true ekfNavVelGainScaler 计算目标速度 并进行x y 方向控制 update loiter 进行x y方向位置控制 angle ef roll pitch rate ef yaw 进行姿态控制 软件系统 功能型程序 loiter 自控状态 高度控制 软件系统 功能型程序 loiter 自控状态 如果有sonar做修正的话 执行 target climb rate get throttle surface tracking target climb rate pos control get alt target G Dt 软件系统 功能型程序 loiter 自控状态 set alt target from climb rate 对上升速率climb rate cms做积分 用 以更新z方向的目标高度 pos target z Z方向目标速度 vel desired z设定为 climb rate cms 软件系统 功能型程序 loiter 自控状态 update z controller 执行高度控制 pos to rate z 得到z方向速率 rate to accel z accel to throttle desired acc el attitude control set throttle out int16 t p i d throttle hover true 得到z方向加速度 软件系统 工具型程序 软件系统 工具型程序 具体处理某一传感器信息或进行某种 计算 AP PID AP GPS AP BARO AP AHRS AP INERTIALNAV AP INERTIALSENSOR AP NAVEKF AP MATH 软件系统 工具型程序 传感器相关 AP COMPASS 从传感器 读取数据 累加到数 组中 累加结果 读取量测 数据 平均补偿 硬 件 数据 处理 200HZ 40HZ 补偿 磁偏角 电机 软件系统 工具型程序 计算相关 AP AHRS AHRS俗称航姿参考系统 AHRS由加速度计 磁场计 陀螺仪构成 AHRS定姿来自 于地球的重力场和地球的磁场的测量 他的静态终精度取决于对磁场的测量精度 和对重力的测量精度 而陀螺仪决定了他的动态性能 AP AHRS主要作用有 1 得到相对于地面 NED方向 的加速度 2 得到当前的位置 经度 纬度 高度 3 得到相对于地面 NED方向 的速度 4 维护 DCM 方向余弦矩阵 5 维护 roll pitch yaw 的 cos si