Ardusub中添加自定义控制器

embedded/2024/11/13 3:58:52/

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 controllerAP_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 filtervoid reset_filter() {_flags.reset_filter = true;}// flagsstruct 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:// parametersAP_Float _wc;          // Response bandwidth in rad/sAP_Float _wo;          // State estimation bandwidth in rad/sAP_Float _b0;          // Control gainAP_Float _limit;AP_Float _delta;AP_Int8  _order;// ESO interal variablesfloat _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 gainAP_GROUPINFO("B0",3,AC_ADRC,_b0,10),// @Param: DELT// @DisplayName: ADRC control linear zone lengthAP_GROUPINFO("DELT",4,AC_ADRC,_delta,1.0),// @Param: ORDR// @DisplayName: ADRC control model orderAP_GROUPINFO("ORDR",5,AC_ADRC,_order,1),// @Param: LM// @DisplayName: ADRC control output limitAP_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 NaNif (!isfinite(target) || !isfinite(measurement)) {return 0.0;}if (_flags.reset_filter) {_flags.reset_filter = false;}// Get controller errorfloat e1 = target - _z1;// control derivation errorfloat e2 = -_z2;// state estimation errorfloat 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 lawoutput = (_wc * fal(e1,0.5f,_delta)  - sigma * _z2)/_b0;// Limit outputif (is_zero(_limit.get())) {output_limited = output;} else {output_limited = constrain_float(output * dmod,-_limit,_limit);}// State estimationfloat 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 lawoutput = (kp * fal(e1,0.5f,_delta) + kd * fal(e2,0.25,_delta) - sigma * _z3)/_b0;// Limit outputif (is_zero(_limit.get())) {output_limited = output * dmod;} else {output_limited = constrain_float(output * dmod,-_limit,_limit);}// State estimationfloat 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: AdvancedAP_SUBGROUPINFO(_rate_roll_cont, "RAT_RLL_", 1, AC_CustomControl_ADRC, AC_ADRC),// @Param: RAT_PIT_LM// @DisplayName: ADRC pitch axis control output limit// @User: AdvancedAP_SUBGROUPINFO(_rate_pitch_cont, "RAT_PIT_", 2, AC_CustomControl_ADRC, AC_ADRC),// @Param: RAT_YAW_LM// @DisplayName: ADRC yaw axis control output limit// @User: AdvancedAP_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 implementedVector3f 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


http://www.ppmy.cn/embedded/136867.html

相关文章

Git核心概念

目录 版本控制 什么是版本控制 为什么要版本控制 本地版本控制系统 集中化的版本控制系统 分布式版本控制系统 认识Git Git简史 Git与其他版本管理系统的主要区别 Git的三种状态 Git使用快速入门 获取Git仓库 记录每次更新到仓库 一个好的 Git 提交消息如下&#…

6、If、While、For、Switch

6、If、While、For、Switch 一、If 1、if-else if (boolean) {代码块 } else if (boolean) {代码块 } else if (boolean) {代码块 } else { // 默认情况代码块 }关于IDEA单元测试控制台不能输入数据的问题&#xff1a; https://blog.csdn.net/m0_72900498/article/details/…

数据结构:七种排序及总结

文章目录 排序一插入排序1直接插入排序2希尔排序二选择排序3直接选择排序4堆排序三 交换排序5冒泡排序6快速排序四 归并排序7归并排序源码 排序 我们数据结构常见的排序有四大种&#xff0c;四大种又分为七小种&#xff0c;如图所示 排序&#xff1a;所谓排序&#xff0c;就是…

秒杀系统的设计与压测

环境准备 数据库 完成demo至少需要两个数据表&#xff0c;一个customer表示秒杀的用户&#xff0c;一个sec_product表示被秒杀的商品。 create database sec_kill;use sec_kill; create table customer(id int primary key auto_increment not null,name varchar(20),phone …

蓝桥杯备赛(持续更新)

16届蓝桥杯算法类知识图谱.pdf 1. 格式打印 %03d&#xff1a;如果是两位数&#xff0c;将会在前面添上一位0 %.2f&#xff1a;会保留两位小数 如果是long&#xff0c;必须在数字后面加上L。 2. 进制转化 2.1. 十进制转任意进制&#xff1a; 十进制转任意进制时&#xff…

Android中桌面小部件framework层使用到的设计模式

在Android中&#xff0c;桌面小部件&#xff08;App Widget&#xff09;的Framework层采用了多种设计模式&#xff0c;以实现模块化、可维护性和高效的交互。 以下是Android桌面小部件Framework层中常用的设计模式及其具体应用&#xff1a; 1. 观察者模式&#xff08;Observe…

redis开启远程登录

redis开启远程登录 Redis 默认配置下仅允许本地访问,要开启远程登录,你需要修改 Redis 配置文件中的一些设置。以下步骤将指导你如何配置 Redis 以允许远程访问: 找到 Redis 配置文件: Redis 的配置文件通常名为 redis.conf。在 Linux 系统上,它可能位于 /etc/redis/redi…

重构代码之移动字段

移动字段用于将字段从一个类移动到另一个更合适的类中。通常&#xff0c;当某个字段在当前类中的使用很少&#xff0c;或者更多地被其他类依赖时&#xff0c;可以考虑将其移动到更加合适的类中&#xff0c;以提升代码的聚合性和可维护性。 一、适用场景 字段主要为其他类提供…