ardupilot开发 --- CustomControl 篇

1. 一些概念

• AC_CustomControl允许您以系统的方式在ArduPilot中实现并轻松运行自己的控制算法。
• 仅局限与角速度控制环；
• 设置一个RC通道作为控制器切换，如RC6_OPTION = 109。
• CC_AXIS_MASK 设置roll、pitch、yaw哪些轴参与CustomControl
• 控制器切换时，滤波器、积分器会进行复位
• 从自定义控制器切换到主控制器时的无障碍传输
• 地面和飞行中的线轴状态分离，以避免在使用自定义控制器武装和起飞期间积累
• 前端-后端分离，允许以非常小的开销添加新控制器
• 编译自定义控制器在编译固件时要加入参数：–enable-custom-controller
• 允许添加新的自定义控制器参数
• 自定义控制器参数在GCS中以 “CC_” 为前缀。
• custom controller 在SITL中可用，可在SITL中先测试；
• …

2. 参数

• 前端参数
  • CC_TYPE
    自定义控制器后端类型，0时关闭功能；
    0 None，1 Empty，2 PID，3 ADRC（新添加）
  • CC_AXIS_MASK
    设置roll、pitch、yaw哪些轴参与CustomControl

3. 原理解析

• custom controller update function 什么时候被调用的？
  在 main rate controller 调用之后
  在motor output 之前
  这样电机混控器 motor mixer 输入量：_roll_in, _pitch_in, _yaw_in 就可以被custom controller 的输出所覆盖；
• custom controller的期望值来源
  custom controller 和main rate controller 使用相同的期望值，都来源于AC_AttitudeControl library ，更具的说来源于
• 运行自定义控制器后，混合器输入通过“set_roll”、“set_pitch”和“set_yaw”功能发送到电机库。
• 控制器切换回主控制器时会做哪些
  当从自定义控制器切换到主控制器时，会重置主控制器的以下参数：目标值、误差、d项滤波器并正确设置每个轴积分器。一避免由控制量不连续导致的飞机突然的机动。为了在控制器之间实现平稳过渡，当切换出重置主控制器所有三个轴的自定义控制器时，会调用主控制器重置功能。
  相关函数：reset_main_atti_controller
  举例代码：
  _atti_control->get_rate_roll_pid().reset_filter();
  _atti_control->get_rate_roll_pid().set_integrator(_atti_control->rate_bf_targets().x - gyro_latest.x, _motors->get_roll());

4. 使用步骤

• 1）添加自定义控制算法代码进ardupilot中，编译固件：–enable-custom-controller
• 2）参数设置
  CC_TYPE = 2
  CC_AXIS_MASK = 7
  RC6_OPTION = 109
• 3）重启
• 4）设置自定义控制算法的其他参数，以CC3为前缀的参数们
• 5）起飞，悬停，遥控器切换至自定义控制器，观察响应，调整参数…

5. 开发文档、使用文档

• Ardupilot\libraries\AC_CustomControl\README.md
• Adding Custom Attitude Controller to Copter
• 如何添加自定义控制器？
• 假设添加的是第5个custom controller
• 1）复制 AC_CustomControl_Empty.cpp 和 AC_CustomControl_Empty.h 文件到目录Library/AC_CustomControl中，并重命名，如AC_CustomControl_ADRC.cpp、AC_CustomControl_ADRC.h；
  将 AC_CustomControl_ADRC.cpp、AC_CustomControl_ADRC.h 中所有的
  AC_CustomControl_Empty 替换为 AC_CustomControl_ADRC。
  CUSTOMCONTROL_EMPTY_ENABLED 替换为 CUSTOMCONTROL_ADRC_ENABLED
  将实现自定义算法ADRC的头文件包含进AC_CustomControl_ADRC.h中：

#include "AC_ADRC/AC_ADRC.h"

• 2）修改宏定义的值 CUSTOMCONTROL_MAX_TYPES；
  这个宏定义的含义是：自定义控制器个数；
  在文件AC_CustomControl.h中，找不到就全局搜索一下；
  当前自定义控制器总共有：NONE，EMPTY，PID 共3中，但是从0开始，因此CUSTOMCONTROL_MAX_TYPES默认值为2，现在多增加了一种ADRC，所以要把CUSTOMCONTROL_MAX_TYPES定义为3，如：
  #define CUSTOMCONTROL_MAX_TYPES 3
• 3）增加一个枚举值

enum class CustomControlType : uint8_t {
	CONT_NONE            = 0,
	CONT_EMPTY           = 1,             
	CONT_PID             = 2,
	CONT_ADRC             = 3,
}; 

• 4）在 AC_CustomControl.cpp 中增加刚创建的头文件：

#include "AC_CustomControl_ADRC.h"

• 5）在参数列表var_info中增加自定义控制器的相关参数
  注意：前缀、下标加一，如：

// parameters for empty controller
AP_SUBGROUPVARPTR(_backend, "1_", 6, AC_CustomControl, _backend_var_info[0]),
// parameters for PID controller
AP_SUBGROUPVARPTR(_backend, "2_", 7, AC_CustomControl, _backend_var_info[1]),
// parameters for ADRC controller
AP_SUBGROUPVARPTR(_backend, "3_", 8, AC_CustomControl, _backend_var_info[2]),
AP_GROUPEND

• 6）在 AC_CustomControl.cpp 中的 init 函数中增加允许生成新自定义控制器后台的相关代码，如：

case CustomControlType::CONT_PID:
    _backend = new AC_CustomControl_PID(*this, _ahrs, _att_control, _motors, _dt);
    _backend_var_info[get_type()] = AC_CustomControl_PID::var_info;
    break;
case CustomControlType::CONT_ADRC:
    _backend = new AC_CustomControl_ADRC(*this, _ahrs, _att_control, _motors, _dt);
    _backend_var_info[get_type()] = AC_CustomControl_ADRC::var_info;
    break;
default:
    return;

• 7）在类 AC_CustomControl_ADRC 中添加所需的参数（对象），如：

AC_ADRC _rate_roll_cont;
AC_ADRC _rate_pitch_cont;
AC_ADRC _rate_yaw_cont;

• 8）将这些参数登记到 AC_CustomControl_ADRC.cpp 的 var_info 中，以便可以在地面站读取和修改：

const AP_Param::GroupInfo AC_CustomControl_ADRC::var_info[] = {
    // @Param: RAT_RLL_LM
    // @DisplayName: ADRC roll axis control output limit
    // @User: Advanced
    AP_SUBGROUPINFO(_rate_roll_cont, "RAT_RLL_", 1, AC_CustomControl_ADRC, AC_ADRC),
    // @Param: RAT_PIT_LM
    // @DisplayName: ADRC pitch axis control output limit
    // @User: Advanced
    AP_SUBGROUPINFO(_rate_pitch_cont, "RAT_PIT_", 2, AC_CustomControl_ADRC, AC_ADRC),
    // @Param: RAT_YAW_LM
    // @DisplayName: ADRC yaw axis control output limit
    // @User: Advanced
    AP_SUBGROUPINFO(_rate_yaw_cont, "RAT_YAW_", 3, AC_CustomControl_ADRC, AC_ADRC),

    AP_GROUPEND
};

• 9）将这些参数在类的构造函数中初始化：

_rate_roll_cont(100.0, dt),
_rate_pitch_cont(100.0, dt),
_rate_yaw_cont(10.0, dt)

• 10）将自定义控制算法的代码整合进 AC_CustomControl_ADRC 中的 update 函数和 reset 函数中（请参考本文第6节）；
  其中，update 是控制器单步函数；reset 是重置函数；
  获取目标姿态：atti_control->get_attitude_target_quat()
  获取目标角速度：atti_control->rate_bf_targets()
  获取陀螺仪的最新测量值：_ahrs->get_gyro_latest()
  Take a look at other backends
  update 函数的返回值是一个：a 3-dimensional vector consisting of roll pitch yaw mixer input, respectively.
• 11）在本例子中，因为文件AC_ADRC.h和AC_ADRC.cpp被存放在目录AC_ADRC中，要想编译它们，应该把路径告诉 waf ：
  在 ardupilot/Tools/ardupilotwaf/boards.py 中添加：
  注意添加逗号

'AC_CustomControl',
'AC_CustomControl/AC_ADRC'

• 12）在SITL中测试：
  SITL中默认启用自定义控制器。所以编译时候不需要加–enable-custom-controller选项：

sim_vehicle.py -v ArduCopter --console --map --no-mavproxy

6. ADRC 参考代码

https://github.com/esaldiran/ardupilot.git
分支：custom-cont-ex-INDI-ADRC

7. 调参

在仿真中变现的比较好的一组参数：

如何调参：
参考：https://blog.csdn.net/weixin_41276397/article/details/127353049

---