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