Ardupilot代码学习笔记

本文为学习ardupilot官网文档做的笔记。

ardupilot总体架构

ArduCopter架构

手动模式架构（Stabilize、Acro、Drift）

自动模式架构（RTL、Guided、Auto）

Copter姿态控制流程

AP控制器将角度误差（目标角度和实际角度之间的差异）转换为所需的转速，然后PID控制器将转速误差转换为高级电机命令。

手动模式流程

调用顶层 flight-mode.cpp 的“update_flight_mode()”函数。该函数检查飞行器的飞行模式（即“control_mode”变量），然后调用相应的<flight mode>_run() 函数（即 stable_run 用于稳定模式， rtl_run 用于RTL 模式等）。<flight mode>_run() 函数可以在mode_<flight mode>.cpp 文件中找到（即 mode_stabilize.cpp、 mode_rtl.cpp等）。
<flight mode>_run函数负责将用户输入（在 g.rc_1.control_in、g.rc_2.control_in 等中找到）转换为适合该飞行模式的倾斜角、旋转速率、爬升速率等。例如，AltHold将用户的滚转和俯仰输入转换为倾斜角（以度为单位），偏航输入转换为旋转速率（以度/秒为单位），油门输入转换为爬升速率（以 cm/s 为单位）。
<flight mode>_run 函数必须做的最后一件事是将这些所需的角度、速率等传递到 Attitude Control 和/或 Position Control 库（它们都保存在 AC_AttitudeControl 文件夹中）。
AC_AttitudeControl库提供了 5种可能的方式来控制车辆的姿态，下面介绍最常见的 3 种。

angle_ef_roll_pitch_rate_ef_yaw() ：这接受滚转和俯仰的NED角度，以及偏航的NED速率。例如，提供此功能 roll = -1000, pitch = -1500, yaw = 500 表示车辆向左倾斜 10 度，向前倾斜 15 度并以 5 度/秒的速度向右旋转。

angle_ef_roll_pitch_yaw() ：这接受用于滚转、俯仰和偏航的NED角度。与上面类似，除了提供 500 的偏航意味着将车辆旋转到北方以东 5 度。

rate_bf_roll_pitch_yaw() ：这接受滚转、俯仰和偏航的机体系速率（以度/秒为单位）。例如，提供这个函数 roll = -1000, pitch = -1500, yaw = 500 将导致车辆以 10 度/秒的速度向左滚动，以 15 度/秒的速度向前俯仰并以 5 度/秒的速度绕车辆的 z 轴旋转。

在对这些函数进行任何调用后，将调用 AC_AttitudeControl::rate_controller_run() 。这会将上面列出的方法的输出转换为滚转、俯仰和偏航输入，这些输入通过它的set_roll、set_pitch、set_yaw 和 set_throttle 方法发送到 AP_Motors库。

AC_PosControl库允许对车辆进行 3D 位置控制。通常只使用更简单的 Z 轴（即高度控制）方法，因为更复杂的 3D 位置飞行模式（即 Loiter）使用 AC_WPNav 库。无论如何，这个库的一些常用方法包括：
- set_alt_target_from_climb_rate() ：接受以 cm/s 为单位的爬升率并更新绝对高度目标
- set_pos_target() : 这接受一个 3D 位置向量，它是一个以厘米为单位的离家的偏移量

如果调用了 AC_PosControl 中的任何方法，则飞行模式代码也必须调用 AC_PosControl::update_z_controller() 方法。这将运行 z 轴位置控制 PID 循环并将低级油门级别发送到 AP_Motors 库。如果调用任何 xy 轴方法，则必须调用 AC_PosControl::update_xy_controller() 。

AP_Motors 库包含“电机混控”代码。该代码负责将从 AC_AttitudeControl 和 AC_PosControl 库接收到的横滚、俯仰、偏航和油门值转换为绝对电机输出（即 PWM 值）。
set_roll(), set_pitch(), set_yaw() : 接受 -4500 ~ 4500 范围内的滚动、俯仰和偏航值。这些不是所需的角度甚至速率，而只是一个值。例如 set_roll(-4500) 意味着尽可能快地向左滚动。
set_throttle() ：接受 0 ~ 1000 范围内的绝对油门值。0 = 电机关闭，1000 = 全油门。
每种框架类型（quad、Y6、传统直升机）都有不同的类，但在每个框架中都有一个“ output_armed ”函数，负责将这些横滚、俯仰、偏航和油门值转换为 pwm 输出。这种转换通常包括实现一个“稳定性补丁”，当输入请求超出框架的物理限制时（即，最大油门和最大滚动在四边形中是不可能的，因为某些电机必须更少比其他人造成滚动）。在“output_armed”函数的底部有一个对 hal.rcout->write() 的调用，它将所需的 pwm 值传递给 AP_HAL 层。
AP_HAL库（硬件抽象层）为所有板提供一致的接口。特别是 hal.rc_out_write() 函数将使从 AP_Motors 类接收到的指定 PWM 出现在电路板的适当 pwm 引脚输出上。

Copter电机库

Motors库的类层次结构图

电机库的输入

姿态控制器向电机库提供高级滚转、俯仰、偏航和油门命令，范围为-1到1（滚转、俯仰和偏航）与0到1（油门）。姿态控制器发出这些命令的确切位置可以在AC_AttitudeControl_Multi的rate_controller_run()函数中找到。

void AC_AttitudeControl_Multi::rate_controller_run()
{Vector3f gyro_latest = _ahrs.get_gyro_latest();_motors.set_roll(rate_target_to_motor_roll(gyro_latest.x, _rate_target_ang_vel.x));_motors.set_pitch(rate_target_to_motor_pitch(gyro_latest.y, _rate_target_ang_vel.y));_motors.set_yaw(rate_target_to_motor_yaw(gyro_latest.z, _rate_target_ang_vel.z));

所用方法定义见AP_Motors_Class.h

// set_roll, set_pitch, set_yaw, set_throttle
void set_roll(float roll_in) { _roll_in = roll_in; };        // range -1 ~ +1
void set_pitch(float pitch_in) { _pitch_in = pitch_in; };    // range -1 ~ +1
void set_yaw(float yaw_in) { _yaw_in = yaw_in; };            // range -1 ~ +1
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0~1

电机库的作用

将姿态控制器的高电平输入转换为单独的电机和伺服输出
设置限制标志以避免姿态和油门控制器中的“积分饱和”。尽管车辆可以在不设置这些标志的情况下飞行，但它们对于防止倾斜角控制中的过冲以及自动着陆检测非常重要。限制保存在AP_Motors_Class::limit变量中。

// structure for holding motor limit flags
struct AP_Motors_limit {uint8_t roll_pitch      : 1; // we have reached roll or pitch limituint8_t yaw             : 1; // we have reached yaw limituint8_t throttle_lower  : 1; // we have reached throttle's lower limituint8_t throttle_upper  : 1; // we have reached throttle's upper limit
} limit;

Copter位置控制和导航

类层次结构和描述

在最高级别，AC_WPNav、AC_Circle 和 AC_Loiter 库是对等的，都使用 AC_PosControl 库

AC_WPNav
由三个独立的控制器组成（将来可能会分离到单独的库中）：
Waypoint（航点控制器）尝试将车辆直线飞行到目标航点。此接口接受指定为纬度、经度和高度或作为与 EKF 原点的偏移量的 3D 目标目的地
Spline（样条线控制器）将车辆以平滑的曲线路径飞行到 3D 目标航路点，最终速度将允许其平稳地继续朝下一个航路点前进。它的接口与上面的 Waypoint 控制器非常相似
Brake（刹车控制器）试图让车辆尽快停下来
Waypoint、Spline 和 Brake 控制器不直接使用 PID。相反，它们更新目标位置或速度，然后将其传递给位置控制器

AC_PosControl：

水平（X 和 Y 轴）控制和垂直（Z 轴）控制的单独接口。这些接口是分开的，因为某些飞行模式（如 AltHold 模式）只需要 Z 轴控制器
使用分层 PID 控制器
- XY 轴使用位置 P控制器将位置误差转换为目标速度。速度 PID 将速度误差转换为所需的加速度，然后将其转换为所需的倾斜角，然后将其发送到姿态控制库中。

Z 轴使用位置 P 控制器将位置误差转换为目标垂直速度（也称为爬升率）。速度 P 控制器将速度误差转换为所需的加速度。加速度 PID 将加速度误差转换为所需的油门，然后将其发送到姿态控制库（主要只是将其传递到低级电机库）
AC_PosControl 还包括一个 3D 速度控制器和一个 3D 位置+速度控制器

L1制导律

L1 控制器的update_waypoint 方法的最终输出是所需的横向加速度（在下面红色中显示为“latAccDem”），它将使车辆回到起点和终点之间的线。

使用的公式也如下所示。阻尼来自NAVL1_DAMPING参数。period来自NAVL1_PERIOD参数。

ArduPlane架构

ahrs_update调用 EKF 以使用最新的传感器数据并产生姿态和位置估计。
read_radio读取飞行员的输入并计算适当的姿态或位置目标
navigate调用 L1 和 TECS 控制器（见下文）为横滚、俯仰和油门控制器转换位置目标
update_flight_mode将 L1 控制器的滚动和俯仰目标复制到 nav_roll_cd 和 nav_pitch_cd 全局变量
stabilize执行较低级别的横滚、俯仰和油门控制器
set_servos将横滚、俯仰和油门控制器的输出发送到适当的伺服输出

Plane控制器

Plane有两个高级控制器和至少3个低级控制器。

L1控制器将起点和终点（分别表示为纬度、经度）转化为横向加速度，使车辆沿起点到终点的路径水平行驶。
TECS（总节能系统）控制车辆的动能（即速度）与其势能（即高度）之间的交换。它的输入是目标速度和高度，它试图通过计算目标油门和俯仰值来达到这些目标，然后将这些值传递给较低级别的俯仰和油门控制器。

Ardupilot代码学习笔记相关推荐

GEE（Google Earth Engine）代码学习笔记一快速入门
GEE 代码学习笔记一 (GEE 基于JavaScript语言和python语言,我记录的是JavaScript语言) 1.GEE 快速入门 quick start. 2.基本语句 - 简单输出 pr ...
frustum pointnets训练代码学习笔记——kitti_object.py
frustum pointnets训练代码学习笔记--kitti_object.py 本文记录了博主学习frustum pointnets过程中遇到的2D和3D数据库显示程序.为了画出输出结果,博主希 ...
GEE （Google Earth Engine）最基础代码学习笔记三
GEE (Google Earth Engine)代码学习笔记三本次学习核心为:将JavaScript objects and primitives放入Earth Engine 容器传到服务器,并处 ...
DSB2017项目grt123代码学习笔记一：项目基本情况
DSB2017项目grt123代码学习笔记一:项目基本情况 Kaggle上Data Science Bowl 2017年肺结节检测比赛第一名grt123团队的算法. github地址:https:// ...
GEE(Google Earth Engine) 最基础代码学习笔记二 —— JavaScript 语言
GEE(Google Earth Engine) 学习笔记二 Javascript 语言 1. 注释 print('Hello World!'); 如果要注释,则在代码前面加//,比如: // pri ...
石墨文档代码学习笔记分享
代码学习: 前后端对json的简单处理 Java和数据库锁的问题简单介绍 Java后端二维码加密 xml解析发送简单邮件等常用功能. Java后端日志logback 文件的上传和下载 freem ...
ArduPilot代码学习--模式切换
引言:ArduPilot代码兼容无人机,无人车,UUV,帆船等多种vehicle,本文以Copter为例,说明代码中是如何完成模式切换的. 各个模式的init 首先定位在系统初始化中,即system. ...
程世东老师TensorFlow实战——个性化推荐，代码学习笔记之③推荐过程
个性化推荐第三部分:推荐过程(完结撒花) 代码来自于知乎:https://zhuanlan.zhihu.com/p/32078473 /代码地址https://github.com/chengston ...
第一行代码学习笔记第十章——探究服务
知识点目录 10.1 服务是什么 10.2 Android多线程编程 * 10.2.1 线程的基本用法 * 10.2.2 在子线程中更新UI * 10.2.3 解析异步消息处理机制 * 10.2.4 ...

Ardupilot代码学习笔记

Ardupilot代码学习笔记相关推荐

最新文章

热门文章