博客

Ardupilot代码学习笔记

  • Post author:
  • Post category:其他


本文为学习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

    库。无论如何，这个库的一些常用方法包括：



如果调用了 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（总节能系统）控制车辆的动能（即速度）与其势能（即高度）之间的交换。它的输入是目标速度和高度，它试图通过计算目标油门和俯仰值来达到这些目标，然后将这些值传递给较低级别​​的俯仰和油门控制器。


版权声明：本文为PENG__WANG原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。