Ardusub中添加自定义控制器

devtools/2024/11/8 7:20:03/

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/devtools/132241.html

相关文章

清华大学提出Mini-Omni2：开源多模态模型，功能与GPT-4o媲美！

清华大学提出Mini-Omni2：开源多模态模型，功能与GPT-4o媲美！

&#x1f310; 在人工智能领域&#xff0c;多模态模型的发展正如火如荼。今天&#xff0c;我们要介绍的是由清华大学提出的Mini-Omni2&#xff0c;这是一个开源的多模态语言模型&#xff0c;它在功能上与GPT-4o相媲美&#xff0c;能够理解和生成视觉、听觉和文本内容&#xff0…
阅读更多...
使用C语言进行信号处理：从理论到实践的全面指南

使用C语言进行信号处理：从理论到实践的全面指南

1. 引言 在现代操作系统中&#xff0c;信号是一种进程间通信机制&#xff0c;它允许操作系统或其他进程向一个进程发送消息。信号可以用来通知进程发生了一些重要事件&#xff0c;如用户请求终止进程、硬件异常、定时器超时等。掌握信号处理技术对于开发健壮、高效的系统程序至…
阅读更多...
Docker 基础命令简介

Docker 基础命令简介

目录 Docker 基础命令 1. Docker 版本信息 2. 获取 Docker 帮助 3. 列出所有运行中的容器 4. 运行一个新的容器 5. 查看容器日志 6. 停止容器 7. 启动已停止的容器 8. 删除容器 9. 列出所有镜像 10. 拉取镜像 11. 构建镜像 12. 删除镜像 13. 执行命令 14. 查看容…
阅读更多...
MySQL_数据类型建表

MySQL_数据类型建表

复习&#xff1a; 我们昨天学习的知识都忘了嘛&#xff1f;如果忘了也不要担心&#xff0c;我来带大家来复习一遍吧&#xff01;&#xff01;&#xff01; 1.查看所有数据库 show databases;2.创建属于自己的数据库 create database 数据库名; 检查自己创建的数据库是…
阅读更多...
Docker在CentOS上的安装与配置

Docker在CentOS上的安装与配置

前言 随着云计算和微服务架构的兴起&#xff0c;Docker作为一种轻量级的容器技术&#xff0c;已经成为现代软件开发和运维中的重要工具。本文旨在为初学者提供一份详尽的指南&#xff0c;帮助他们在CentOS系统上安装和配置Docker及相关组件&#xff0c;如Docker Compose和私有…
阅读更多...
爬虫技术——小白入狱案例

爬虫技术——小白入狱案例

知孤云出岫 目录 1. 案例概述2. 案例需求分析3. 实现步骤Step 1: 环境准备Step 2: 分析百度图片URL请求规律Step 3: 编写爬虫代码代码解析 4. 运行代码5. 注意事项6. 案例总结 要实现大批量爬取百度图片&#xff0c;可以使用Python编写一个网络爬虫&#xff0c;通过发送HTTP请求…
阅读更多...
CSS Grid 布局在 IE 中不兼容的原因与解决方案

CSS Grid 布局在 IE 中不兼容的原因与解决方案

CSS Grid 布局在 IE 中不兼容的原因与解决方案 文章目录 CSS Grid 布局在 IE 中不兼容的原因与解决方案1. 引言2. CSS Grid 布局概述2.1 什么是CSS Grid布局&#xff1f;2.2 CSS Grid 与传统布局方法的区别 3. IE 对 CSS Grid 的支持情况3.1 IE11 对 CSS Grid 的支持3.2 其他IE…
阅读更多...
大众汽车合肥社招入职笔试测评SHL题库：综合能力、性格问卷、英语口语真题考什么？

大众汽车合肥社招入职笔试测评SHL题库：综合能力、性格问卷、英语口语真题考什么？

大众汽车合肥社招入职笔试测评包括综合能力测试、性格问卷和英语口语测试。以下是各部分的具体内容&#xff1a; 1. **综合能力测试**&#xff1a; - 这部分测试需要46分钟完成&#xff0c;建议准备计算器和纸笔。 - 测试内容涉及问题解决能力、数值计算能力和逻辑推理能力。 -…
阅读更多...
最新文章

Copyright @ 2022~2023