Ardusub中添加自定义控制器
1.建议保留原程序
复制ardupilot文件夹到当前目录，命名为ardupilot_ARDC
cp -r ardupilot ardupilot_copy
2.切换Sub-4.5版本
Sub-4.1版本libraries里没有AC_CustomControl文件夹，我们需要用到这个文件夹所以需要进行切换分支，在当前ardupilot_ARDC终端目录下输入命令切换分支。
git fetch --tags
git checkout -b new-branch origin/Sub-4.5
git submodule update --init --recursive切换分支后，可能会遗留一些与旧分支不兼容的构建文件。可以尝试清理构建文件：
./waf clean
我这里遇到了一个错误提示我需要安装 empy，安装命令
python -m pip install empy==3.3.4
更新 pip： 首先，尝试更新 pip 版本。运行以下命令
python3 -m pip install --upgrade pip
更新后可以试试是否可以编译成功
./waf sub成功切换分支后libraries目录下会出现AC_CustomControl文件夹
添加自定义控制器
1.复制
复制Library/AC_CustomControl文件夹下面的AC_CustomControl_Empty.cpp 和 AC_CustomControl_Empty.h 并重命名，如AC_CustomControl_ADRC.cpp、AC_CustomControl_ADRC.h；
2.更改函数名
将 AC_CustomControl_ADRC.cpp、AC_CustomControl_ADRC.h 中所有的
AC_CustomControl_Empty 替换为 AC_CustomControl_ADRC。
将.cpp .h中所有的宏定义AP_CUSTOMCONTROL_EMPTY_ENABLED替换为 AP_CUSTOMCONTROL_ADRC_ENABLED
3.包含头文件
将实现自定义算法ADRC的头文件包含进AC_CustomControl_ADRC.h中：
4.自定义控制器个数增加一个
将自定义控件变量的最大数量增加 1 ，在文件AC_CustomControl.h中，找不到就全局搜索一下；
当前自定义控制器总共有：NONE，EMPTY，PID 共3中，但是从0开始，因此CUSTOMCONTROL_MAX_TYPES默认值为2，现在多增加了一种ADRC，所以要把CUSTOMCONTROL_MAX_TYPES定义为3，如：
#define CUSTOMCONTROL_MAX_TYPES 3
5.增加一个枚举值
在文件AC_CustomControl.h中找到一下部分，增加 CONT_ADRC = 3,
enum class CustomControlType : uint8_t {
CONT_NONE = 0,
CONT_EMPTY = 1,
CONT_PID = 2,
CONT_ADRC = 3,
}; // controller that should be used 
6. 添加一个新的后端头文件
在 AC_CustomControl.cpp 中增加刚创建的头文件，将其放在其他后端包含下。
#include "AC_CustomControl_ADRC.h"
7.添加新的后端参数
还是AC_CustomControl.cpp 中在参数列表var_info中增加自定义控制器的相关参数。
数组索引、后端参数前缀和参数表索引加 1。将其放在另一个后端的参数下。
// parameters for ADRC controller
AP_SUBGROUPVARPTR(_backend, "3_", 8, AC_CustomControl, _backend_var_info[2]),
8. init 函数中增加相关代码
在 AC_CustomControl.cpp 中的 init 函数中增加允许生成新自定义控制器后台的相关代码，如：
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;
9.添加自定义控制算法文件
在AC_CustomControl文件夹下添加，我添加文件夹名为AC_ADRC，AC_ADRC文件下为AC_ADRC.h，AC_ADRC.cpp
AC_ADRC.h如下
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
class AC_ADRC {
public:
AC_ADRC(float B00, float dt);
CLASS_NO_COPY(AC_ADRC);
float update_all(float target, float measurement, bool limit);
void reset_eso(float measurement);
float fal(float e, float alpha, float delta);
float sign(float x);
// Reset filter
void reset_filter() {
_flags.reset_filter = true;
}
// flags
struct ap_adrc_flags {
bool reset_filter :1; // true when input filter should be reset during next call to set_input
} _flags;
static const struct AP_Param::GroupInfo var_info[];
protected:
// parameters
AP_Float _wc; // Response bandwidth in rad/s
AP_Float _wo; // State estimation bandwidth in rad/s
AP_Float _b0; // Control gain
AP_Float _limit;
AP_Float _delta;
AP_Int8 _order;
// ESO interal variables
float _z1;
float _z2;
float _z3;
float _dt;
};AC_ADRC.cpp如下
#include <AP_Math/AP_Math.h>
#include "AC_ADRC.h"
#define __AP_LINE__ __LINE__
// table of user settable parameters
const AP_Param::GroupInfo AC_ADRC::var_info[] = {
// @Param: WC
// @DisplayName: ADRC control bandwidth(rad/s)
AP_GROUPINFO("WC",1,AC_ADRC,_wc,10),
// @Param: WO
// @DisplayName: ADRC ESO bandwidth(rad/s)
AP_GROUPINFO("WO",2,AC_ADRC,_wo,15),
// @Param: B0
// @DisplayName: ADRC control input gain
AP_GROUPINFO("B0",3,AC_ADRC,_b0,10),
// @Param: DELT
// @DisplayName: ADRC control linear zone length
AP_GROUPINFO("DELT",4,AC_ADRC,_delta,1.0),
// @Param: ORDR
// @DisplayName: ADRC control model order
AP_GROUPINFO("ORDR",5,AC_ADRC,_order,1),
// @Param: LM
// @DisplayName: ADRC control output limit
AP_GROUPINFO("LM",6,AC_ADRC,_limit,1.0f),
AP_GROUPEND
};
AC_ADRC::AC_ADRC(float B00, float dt)
{
AP_Param::setup_object_defaults(this, var_info);
_dt = dt;
_b0.set(B00);
// reset input filter to first value received
_flags.reset_filter = true;
}
float AC_ADRC::update_all(float target, float measurement, bool limit)
{
// don't process inf or NaN
if (!isfinite(target) || !isfinite(measurement)) {
return 0.0;
}
if (_flags.reset_filter) {
_flags.reset_filter = false;
}
// Get controller error
float e1 = target - _z1;
// control derivation error
float e2 = -_z2;
// state estimation error
float e = _z1 - measurement;
float output = 0.0;
float output_limited = 0;
float dmod = 1.0f;
float sigma = 1.0f/(sq(e) + 1.0f);
switch (_order)
{
case 1:
{
// Nonlinear control law
output = (_wc * fal(e1,0.5f,_delta) - sigma * _z2)/_b0;
// Limit output
if (is_zero(_limit.get())) {
output_limited = output;
} else {
output_limited = constrain_float(output * dmod,-_limit,_limit);
}
// State estimation
float fe = fal(e,0.5,_delta);
float beta1 = 2 * _wo;
float beta2 = _wo * _wo;
_z1 = _z1 + _dt * (_z2 - beta1*e + _b0 * output_limited);
_z2 = _z2 + _dt * (-beta2 * fe);
}
break;
case 2:
{
float kp = sq(_wc);
float kd = 2*_wc;
// Nonlinear control law
output = (kp * fal(e1,0.5f,_delta) + kd * fal(e2,0.25,_delta) - sigma * _z3)/_b0;
// Limit output
if (is_zero(_limit.get())) {
output_limited = output * dmod;
} else {
output_limited = constrain_float(output * dmod,-_limit,_limit);
}
// State estimation
float beta1 = 3 * _wo;
float beta2 = 3 * _wo * _wo;
float beta3 = _wo * _wo * _wo;
float fe = fal(e,0.5,_delta);
float fe1 = fal(e,0.25,_delta);
_z1 = _z1 + _dt * (_z2 - beta1 * e);
_z2 = _z2 + _dt * (_z3 - beta2 * fe + _b0 * output_limited);
_z3 = _z3 + _dt * (- beta3 * fe1);;
}
break;
default:
output_limited = 0.0;
break;
}
return output_limited;
}
void AC_ADRC::reset_eso(float measurement)
{
_z1 = measurement;
_z2 = 0.0;
_z3 = 0.0;
}
float AC_ADRC::fal(float e, float alpha, float delta)
{
if (is_zero(delta)) {
return e;
}
if (fabsf(e) < delta) {
return e / (powf(delta, 1.0f-alpha));
} else {
return powf(fabsf(e), alpha) * sign(e);
}
}
float AC_ADRC::sign(float x)
{
if (x > 0) {
return 1;
} else if (x < 0) {
return -1;
} else {
return 0;
}
}10.添加新参数。
可以按照此文章将参数添加到库
在类 AC_CustomControl_ADRC 中添加所需的参数（对象），如：
AC_ADRC _rate_roll_cont;
AC_ADRC _rate_pitch_cont;
AC_ADRC _rate_yaw_cont;
将这些参数登记到 AC_CustomControl_ADRC.cpp 的 var_info 中，包括 @Param ~ @Increment 注释，以便可以在地面站读取和修改：
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
};
11.在后端的构造函数中初始化类对象
在AC_CustomControl_ADRC.cpp中初始化参数对象
_rate_roll_cont(100.0, dt),
_rate_pitch_cont(100.0, dt),
_rate_yaw_cont(10.0, dt)
别忘记逗号啊！！！
12.将自定义控制算法的代码整合进 AC_CustomControl_ADRC .cpp中的 update 函数和 reset 函数中
其中，update 是控制器单步函数；reset 是重置函数；
在 file 的 update 函数中添加您的控制器。此函数返回一个 3 维向量，分别由 roll、pitch、yaw 和 mixer 输入组成
// Active disturbance rejection controller
// only rate control is implemented
Vector3f rate_target = _att_control->rate_bf_targets();
Vector3f gyro_latest = _ahrs->get_gyro_latest();
Vector3f motor_out;
motor_out.x = _rate_roll_cont.update_all(rate_target.x, gyro_latest.x, _motors->limit.roll);
motor_out.y = _rate_pitch_cont.update_all(rate_target.y, gyro_latest.y, _motors->limit.pitch);
motor_out.z = _rate_yaw_cont.update_all(rate_target.z, gyro_latest.z, _motors->limit.yaw);
return motor_out;
在文件的 reset 函数中添加了 reset 功能。用户有责任添加适当的控制器重置功能。这在很大程度上取决于控制器，如果不在 SITL 中测试，就不应该从另一个后端复制粘贴它。
Vector3f gyro_latest = _ahrs->get_gyro_latest();
_rate_roll_cont.reset_eso(gyro_latest.x);
_rate_pitch_cont.reset_eso(gyro_latest.y);
_rate_yaw_cont.reset_eso(gyro_latest.z);
获取目标姿态：atti_control->get_attitude_target_quat()
获取目标角速度：atti_control->rate_bf_targets()
获取陀螺仪的最新测量值：_ahrs->get_gyro_latest()
13.添加编译路径
在本例子中，因为文件AC_ADRC.h和AC_ADRC.cpp被存放在目录AC_ADRC中，要想编译它们，应该把路径告诉 waf ：
在 ardupilot/Tools/ardupilotwaf/boards.py 中找到'AC_CustomControl'添加：
注意添加逗号！！！
'AC_CustomControl/AC_ADRC'
14.编译生成固件
./waf configure --board fmuv3 --enable-custom-controller --target sub
./waf sub