为飞行器添加自定义姿态控制器

自定义控制器库允许您在 ArduPilot 中以系统的方式实现并轻松运行控制器。Original-primary-mains 表示现有控制器，custom-secondary 表示新控制器。该库旨在不干扰主控制器的其他部分或载具级代码。控制器的输出会以与主控制器相同的方式发送到控制分配库，即混合器。自定义控制器库具有以下功能：

特点

  • 通过 RC 开关在主控制器和定制控制器之间进行飞行切换，选项 109。

  • 用于选择使用自定义控制器输出的轴的位掩码

  • 在控制器之间切换时，采用滤波器、积分器复位机制 - 从定制控制器切换到主控制器时，采用无缓冲传输方式

  • 地面和飞行中阀芯状态分离，以避免在使用定制控制器进行上膛和起飞时出现积聚现象

  • 前端与后端分离，只需极少的开销即可添加新控制器

  • 在硬件上编译出自定义控制器相关代码的标志，-enable-custom-controller

  • 正确执行参数表，允许添加新的自定义控制器参数表而不会损坏

  • 只需一个参数即可在不同的自定义控制器之间切换，需要重新启动

  • 多重检查可避免意外运行未配置或未配置的自定义控制器与 RC 开关

  • 自定义控制器参数以 CC_ 在全球监控系统中。

参数

前端库有以下参数

  • CC_TYPE：选择要使用的自定义控制器后端，需要重启。- 设置为 0 将关闭该功能，GCS 将不显示与自定义控制器相关的参数

  • CC_AXIS_MASK：选择使用自定义控制器输出的轴 - 这是一个位掩码类型的参数。设置 7 可使用所有输出

警告

自定义控制器要求用户设置构建环境 (编写代码），并在本地克隆了 ArduPilot GitHub repo (下载代码/使用 Git).示例控制器仅用于实验目的，只进行了简单测试。建议用户在实际载具上测试时小心谨慎。

与主控制器互动

自定义控制器更新函数在主速率控制器运行后、电机输出库调用前调用。这种安排允许覆盖电机库混合器输入，即 在......, _pitch_in和 我不知道 值，而不会引起电机库内部的功能变化。这与主控制器将其输出发送到控制分配库（称为混合器）的方式相同。这将延迟降低到最低水平。

自定义控制器库使用与主控制器相同的目标态度。自定义控制器库中的大部分代码 AC_AttitudeControl 库与输入形状有关，例如如何根据飞行模式或高级位置控制器输出解释飞行员指令。例如，在稳定（STABILIZE）模式下，飞行员的遥控指令会根据最大倾斜角和偏航率参数进行缩放，这些值会传递给 输入_euler_angle_roll_pitch_euler_rate_yaw（偏航角度_roll_pitch_euler_rate_yaw）。 功能。先导指令被送入一阶输入整形算法，以消除因 RC 链路引起的抖动，并生成加速度受限的姿态目标。随后，姿态控制仪 姿态控制仪_跑姿 调用时，将根据加速度限值计算姿态误差。 目标 值，并生成目标速率。即使通过设置 汇率 为 0 时，目标姿态仍会显示一个 目标 变量。

内的每个输入整形函数 AC_AttitudeControl 电话 姿态控制仪_跑姿 在末尾运行姿态控制仪，除非在 ACRO 模式下飞行时将 acro 选项设置为仅速率。即使在这种情况下 目标 正确更新，并且可以通过 率_bf_目标 功能。

默认情况下，输入整形算法处于开启状态。这将产生运动学上一致的姿态目标值和速率前馈值。请查看自定义 PID 后端，了解如何将速率前馈添加到姿态控制输出中。如果启用了 ATC_RATE_FF_ENAB，建议使用速率前馈值，否则飞行员可能会感到遥控指令与飞行器响应之间存在明显滞后。

运行自定义控制器后，混合器输入将通过 设置滚动, 设置间距, 设置偏航 职能

无袋传输

从定制控制器切换到主控制器时，必须重置主控制器目标、误差和 d 期滤波器，并正确设置各轴积分器。否则，控制器误差信号或电机输出将出现突然跳变，从而导致运动生涩。为了实现控制器之间的平稳过渡，在切换出自定义控制器时会调用主控制器复位功能，以复位主控制器的所有三个轴。复位在 重置主控制器 功能。

姿态目标和速率目标也与当前姿态和陀螺速率相等，以使误差信号从零开始增长。 放松态度控制器 功能。为了避免对控制器的脉冲输入，当通过以下设置禁用前馈功能时，目标重置将不执行 汇率 参数为 0。

空白 AC_CustomControl::reset_main_att_controller(空白)
{
    // 如果启用前馈，则重置姿态和速率目标
    如果 (_att_control->；get_bf_feedforward()) {
        _att_control->；放松态度控制器();
    }

    _att_control->；get_rate_roll_pid().设置积分器(0.0);
    _att_control->；get_rate_pitch_pid().设置积分器(0.0);
    _att_control->；get_rate_yaw_pid().设置积分器(0.0);
}

后台类型

目前，有 2 个自定义控制器后端可用。它们是

空控制器 CC_TYPE = 1

空控制器不进行任何计算。创建它的目的是为了方便复制和实现新控制器。从空控制器切换时，主控制器不会重置。

PID 控制器 CC_TYPE = 2

PID 控制器后台的控制器结构与主控制器相同。它没有任何保护机制，如加速度限制或速率限制。默认增益的比例为 0.9 倍，以便将自定义控制器的响应与主控制器区分开来。由于该控制器没有加速度限制，特别是没有平方根控制器，因此在使用它飞行时，下达温和的指令会更安全。虽然该控制器的结构与主控制器相同，但为了更容易检测到不正确复位的影响，我们并没有刻意实现适当的复位功能。

如何使用

SITL 默认启用了自定义控制器。您可以使用 PID 后台对其进行测试。

步骤 #1： 编译并运行默认的 SITL 模型。在 GCS 中选择自定义控制器类型、分配轴掩码并设置激活自定义控制器的 RC 开关。重新启动(飞行)控制器。例如，在 MAVProxy 中使用 RC 通道 6 作为自定义控制器的启用/禁用开关：

停止 设置 CC_TYPE 2
停止 设置 CC_AXIS_MASK 7
停止 设置 RC6_OPTION 109
重新启动

步骤 #2： 运行以下命令显示后台参数。这些参数将位于 CC2_ 用于 PID 后台。

停止 设置 CC2*

步骤 #3： 上膛起飞。悬停时，将 RC6 切换为高电平。在 MAVProxy 中，您可以使用

rc 6 2000

步骤 #4： 系统将提示您 定制 控制器 是 关于 消息，表明自定义控制器正在运行。

步骤 #5： 将 RC6 设置为低电平，以便切换回主控制器。系统将提示您 定制 控制器 是 关闭 关于全球监控系统的信息。

实际飞行测试

建议您始终在主控制器运行时进行上膛、起飞、着陆和撤膛。应在飞行器稳定悬停时切换到自定义控制器。这将减少过滤器重置不当的影响。只有在地面空转正常的情况下，才能使用自定义控制器进行上膛和起飞。

要在硬件上进行测试，请使用 --启用自定义控制器 旗子

./waf 配置 --官方 橙色立方体 旋翼飞行器 --以便-风俗-控制器

发布飞行日志

自定义控制器的切换记录在日志树中的 "CC "下。您可以在 "CC.Act "下查看自定义控制器的激活时间和持续时间。

如何添加新的自定义控制器

您可以通过本视频中演示的以下步骤添加自定义控制器后台。假设我们要添加第 3 个自定义控制器。

步骤 #1： 生成一份 AC_CustomControl_Empty.cpp 和 AC_CustomControl_Empty.h 内 AC_CustomControl 文件夹。文件夹树看起来是这样的

AC_CustomControl.cpp
AC_CustomControl.h
AC_CustomControl_Backend.h
AC_CustomControl_Empty.cpp
AC_CustomControl_Empty.h
AC_CustomControl_Empty - 复制.cpp
AC_CustomControl_Empty - 复制.h
.
.
.

为简单起见，省略了 PID 和 README 文件。

步骤 #2： 改变 空 - 复制 后缀，我们称之为 XYZ如下所示

AC_CustomControl.cpp
AC_CustomControl.h
AC_CustomControl_Backend.h
AC_CustomControl_Empty.cpp
AC_CustomControl_Empty.h
AC_CustomControl_XYZ.cpp
AC_CustomControl_XYZ.h
.
.
.

步骤 #3： 将每个类名、函数定义等从 AC_CustomControl_Empty 至 AC_CustomControl_XYZ 内侧 AC_CustomControl_XYZ.cpp 和 AC_CustomControl_XYZ.h 文件。头文件的格式如下

#pragma once

#include "AC_CustomControl_Backend.h"；

类 AC_CustomControl_XYZ : 公 AC_CustomControl_Backend {
公:
    AC_CustomControl_XYZ(AC_CustomControl 及样品；前端, AP_AHRS_View*&； 小时, AC_AttitudeControl_Multi*&； atti_control, AP_MotorsMulticopter*&； 电机, 浮动 dt);


    向量3f 更新(空白) 否决;
    空白 重置(空白) 否决;

    // 用户可设置的参数
    天电 缢 结构 AP_Param::群组信息 var_info[];

受保护的:
    // 在此声明参数
    AP_Float 参数1;
    AP_Float 参数2;
    AP_Float 参数3;
};

步骤 #4： 将自定义控制变量的最大数量增加一个，并更新自定义控制类型枚举

从

#define CUSTOMCONTROL_MAX_TYPES 2

至

#define CUSTOMCONTROL_MAX_TYPES 3

枚举 类 自定义控件类型 : uint8_t {
    CONT_NONE            = 0,
    CONT_EMPTY           = 1,
    CONT_PID             = 2,
    CONT_XYZ             = 3,
};            // 应使用的控制器

步骤 #5： 在 AC_CustomControl.cpp 文件。将其置于其他后台包含文件之下。

#include "AC_CustomControl_Backend.h"；
#include "AC_CustomControl_Empty.h"；
#include "AC_CustomControl_PID.h"；
#include "AC_CustomControl_XYZ.h"；

第 6 步 在 AC_CustomControl.cpp 文件递增 _backend_var_info 数组索引、后端参数前缀和参数表索引各增加一个。将其置于其他后端参数之下。

.
.
.
    // 空控制器的参数
    ap_subgroupvarptr(后端, "1_"；, 6, AC_CustomControl, _backend_var_info[0]),

    // PID 控制器的参数
    ap_subgroupvarptr(后端, "2_"；, 7, AC_CustomControl, _backend_var_info[1]),

    // XYZ 控制器的参数
    ap_subgroupvarptr(后端, "3_"；, 8, AC_CustomControl, _backend_var_info[2]),

    AP_GROUPEND
};

步骤 #7： 允许在 AC_CustomControl.cpp 文件。

.
.
.
个案 自定义控件类型::CONT_PID:
    后端 = 新 AC_CustomControl_PID(*此, _ahrs, _atti_control, _发动机, 日);
    _backend_var_info[get_type()] = AC_CustomControl_PID::var_info;
    断裂;
个案 自定义控件类型::CONT_XYZ:
    后端 = 新 AC_CustomControl_XYZ(*此, _ahrs, _atti_control, _发动机, 日);
    _backend_var_info[get_type()] = AC_CustomControl_XYZ::var_info;
    断裂;
默认:
    返回;
}

步骤 #8： 这是编译和运行自定义控制器的最低要求。您可以在 AC_CustomControl_XYZ 文件，而无需更改任何其他内容。

步骤 #9： 您可以按照以下说明添加新参数 为库添加参数 <https://ardupilot.org/dev/docs/code-overview-adding-a-new-parameter.html#adding-a-parameter-to-a-library>； 维基页面。

步骤 #10： 在后端构造函数中初始化类对象。例如，在 PID 后端

// 将与控制器有关的变量放在这里

// 角度 P 控制器对象
AC_P                _p_angle_roll2;
AC_P                _p_角度_间距2;
AC_P                _p_angle_yaw2;

// 率 PID 控制器对象
AC_PID _pid_atti_rate_roll;
AC_PID _pid_atti_rate_pitch;
AC_PID _pid_atti_rate_yaw;

上述 P 或 PID 类在后端构造函数中初始化、

AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl 及样品；前端, AP_AHRS_View*&； 小时, AC_AttitudeControl_Multi*&； atti_control, AP_MotorsMulticopter*&； 电机, 浮动 dt) :
    AC_CustomControl_Backend(前端, 小时, atti_control, 电机, dt),
    _p_angle_roll2(ac_attitude_control_angle_p * 0.90f),
    _p_角度_间距2(ac_attitude_control_angle_p * 0.90f),
    _p_angle_yaw2(ac_attitude_control_angle_p * 0.90f),
    _pid_atti_rate_roll(AC_ATC_Multi_rate_RP_P * 0.90f, AC_ATC_Multi_rate_RP_I * 0.90f, AC_ATC_Multi_rate_RP_D * 0.90f, 0.0f, AC_ATC_Multi_rate_RP_IMAX * 0.90f, ac_atc_multi_rate_rp_filt_hz * 0.90f, 0.0f, ac_atc_multi_rate_rp_filt_hz * 0.90f, dt),
    _pid_atti_rate_pitch(AC_ATC_Multi_rate_RP_P * 0.90f, AC_ATC_Multi_rate_RP_I * 0.90f, AC_ATC_Multi_rate_RP_D * 0.90f, 0.0f, AC_ATC_Multi_rate_RP_IMAX * 0.90f, ac_atc_multi_rate_rp_filt_hz * 0.90f, 0.0f, ac_atc_multi_rate_rp_filt_hz * 0.90f, dt),
    _pid_atti_rate_yaw(ac_atc_multi_rate_yaw_p * 0.90f, AC_ATC_Multi_rate_YAW_I * 0.90f, AC_ATC_Multi_rate_YAW_D * 0.90f, 0.0f, AC_ATC_Multi_rate_YAW_IMAX * 0.90f, ac_atc_multi_rate_rp_filt_hz * 0.90f, ac_atc_multi_rate_yaw_filt_hz * 0.90f, 0.0f, dt)
{
    AP_Param::设置对象默认值(此, var_info);
}

步骤 #11： 在 AC_CustomControl_XYZ.cpp 文件。该函数返回一个由滚动、前平移/俯仰、偏航和混合器输入分别组成的三维向量。

步骤 #12： 在 AC_CustomControl_XYZ.cpp 文件。用户有责任添加适当的控制器重置功能。这在很大程度上取决于控制器，不应该在 SITL 中测试之前从其他后端复制粘贴。

步骤 #13： 您可以通过以下方式访问目标姿态和目标速率值 atti_control->get_attitude_target_quat() 和 atti_control->rate_bf_targets() 功能。您还可以使用 _ahrs->get_gyro_latest() 功能。

将生成的代码集成到 ArduPilot 中

生成的源代码需要复制到 ArduPilot 中，并在新创建的自定义控制器后台中调用。下面的视频演示了实现这一目标的步骤。

步骤 #1： 将压缩文件内容复制到 ardupilot/libraries 内的新文件夹中。按照示例调用 AC_Simulink。 步骤 #2： 如果启用了自定义控制器，则告诉 waf 建立此文件夹。 步骤 #3： 创建 Simulink 代码生成类的对象。 步骤 #4： 在后端构造器中调用该对象的初始化函数。 步骤 #5： 在后台更新函数中调用该对象的步进函数。按照与 ert_main.cpp 中类似的模式排列输入和输出参数。 第 6 步 完成这些步骤后，我们就可以编译 ArduPilot 了。

只要输入和输出类型以及将它们传递给阶跃函数的方式保持不变，每次生成代码时都无需重复此处和上一节中给出的步骤。例如，即使我们将控制器类型从 P+PID 更改为 PID+PID，也只需点击生成代码按钮，然后将压缩文件内容复制并粘贴到 AC_Simulink 文件夹即可。

本 PR 提供了一个实施示例 https://github.com/ArduPilot/ardupilot/pull/21700.