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

 电机库的作用

1. 将姿态控制器的高电平输入转换为单独的电机和伺服输出
2. 设置限制标志以避免姿态和油门控制器中的“积分饱和”。尽管车辆可以在不设置这些标志的情况下飞行，但它们对于防止倾斜角控制中的过冲以及自动着陆检测非常重要。限制保存在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 limit
    uint8_t yaw             : 1; // we have reached yaw limit
    uint8_t throttle_lower  : 1; // we have reached throttle's lower limit
    uint8_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（总节能系统）控制车辆的动能（即速度）与其势能（即高度）之间的交换。它的输入是目标速度和高度，它试图通过计算目标油门和俯仰值来达到这些目标，然后将这些值传递给较低级别​​的俯仰和油门控制器。