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 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.mzph.cn/news/885142.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

ReactPress:重塑内容管理的未来

ReactPress Github项目地址&#xff1a;https://github.com/fecommunity/reactpress 欢迎提出宝贵的建议&#xff0c;欢迎一起共建&#xff0c;感谢Star。 ReactPress&#xff1a;重塑内容管理的未来 在当今信息爆炸的时代&#xff0c;一个高效、易用的内容管理系统&#xff0…

云集电商:如何通过 OceanBase 实现降本 87.5%|OceanBase案例

云集电商&#xff0c;一家聚焦于社交电商的电商公司&#xff0c;专注于‘精选’理念&#xff0c;致力于为会员提供超高性价比的全品类精选商品&#xff0c;以“批发价”让亿万消费者买到质量可靠的商品。面对近年来外部环境的变化&#xff0c;公司对成本控制提出了更高要求&…

vue3实现一个无缝衔接、滚动平滑的列表自动滚屏效果,支持鼠标移入停止移出滚动

文章目录 前言一、滚动元素相关属性回顾一、实现分析二、代码实现示例&#xff1a;2、继续添加功能&#xff0c;增加鼠标移入停止滚动、移出继续滚动效果2、继续完善 前言 列表自动滚屏效果常见于大屏开发场景中&#xff0c;本文将讲解用vue3实现一个无缝衔接、滚动平滑的列表自…

三周精通FastAPI:36 OpenAPI 回调

官方文档&#xff1a; OpenAPI 回调 您可以创建触发外部 API 请求的路径操作 API&#xff0c;这个外部 API 可以是别人创建的&#xff0c;也可以是由您自己创建的。 API 应用调用外部 API 时的流程叫做回调。因为外部开发者编写的软件发送请求至您的 API&#xff0c;然后您的…

如何将现有VUE项目所有包更新到最新稳定版

更新有风险,Enter要谨慎!!! 要将项目中的所有 npm 包更新到最新稳定版&#xff0c;可以使用 npm-check-updates 工具。以下是具体步骤&#xff1a; 步骤一&#xff1a;安装 npm-check-updates 首先&#xff0c;全局安装 npm-check-updates 工具&#xff1a; npm install -g…

如何使用 C# 编写一个修改文件时间属性的小工具?

下面是简鹿办公一个用 C# 编写的简单工具&#xff0c;它可以批量修改文件的创建时间、最后访问时间和最后修改时间。我们将使用 .NET Framework 或 .NET Core 来实现这个功能。 完整示例代码 1. 创建一个新的 C# 控制台应用程序 您可以使用 Visual Studio 或 .NET CLI 创建一个…

使用PyQt5设计一个简易计算器

目录 设计UI图 代码 结果展示 设计UI图 代码 from PyQt5 import QtCore, QtGui, QtWidgets from PyQt5.QtCore import * from PyQt5.QtGui import * from PyQt5.QtWidgets import QFileDialog, QMainWindow, QMessageBox from untitled import Ui_MainWindow import sysclass…

动态规划-两个数组的dp问题——712.两个字符串的最小ASCII删除和

1.题目解析 题目来源 712.两个字符串的最小ASCII删除和——力扣 测试用例 2.算法原理 1.状态表示 由于如果直接求本题会发现无从下手&#xff0c;不妨根据正难则反的原理&#xff0c;反向求公共子序列的ASCII码最大值即可&#xff0c;于是就转化为求公共子序列的问题&#x…

elementui中的新增弹窗在新增数据成功后再新增 发现数据无法清除解决方法

elementui中的新增弹窗在新增数据成功后再新增 发现数据无法清除解决方法 试过网上其他方法&#xff0c;发现表单清空数据还是有问题&#xff0c;索性用下面方法解决: // 给弹框里面添加 v-ifvisible测试无问题&#xff0c;暂时先这样解决&#xff0c;如果有其他方法&#x…

基于Arduino的RGB灯按键控制

一.简介 通过按键控制RGB灯分别显示7种颜色&#xff1a;红 、绿、 蓝、 黄、 青、 紫、 白。 二.按键控制RGB灯原理 1)RGB全彩LED: LED由三个颜色分别为&#xff1a;红&#xff08;Red&#xff09;、绿&#xff08;Green&#xff09;、蓝&#xff08;Blue&#xff09;的LED…

hive数据查询语法

思维导图 基本查询 基本语法 SELECT [ALL | DISTINCT] 字段名, 字段名, ... FROM 表名 [inner | left outer | right outer | full outer | left semi JOIN 表名 ON 关联条件 ] [WHERE 非聚合条件] [GROUP BY 分组字段名] [HAVING 聚合条件] [ORDER BY 排序字段名 asc | desc…

前端小知识:我居然没学会用 split 方法?!

小伙伴们&#xff0c;你们会用 JavaScript 的 split 方法吗&#xff1f;最近我才发现&#xff0c;原来我多年来一直没真正掌握它&#xff0c;结果在解题时被卡住了。所以今天&#xff0c;我决定好好整理一下这个方法的用法。 在讨论问题之前&#xff0c;先来看一下 split 的两种…

VTK知识学习(2)-环境搭建

1、c方案 1.1下载源码编译 官网获取源码。 利用Cmake进行项目构建。 里面要根据实际使用的情况配置相关的模块哟&#xff0c;这个得你自行研究下了。 CMAKEINSTALLPREFIX--这个选项的值表示VTK的安装路径&#xff0c;默认的路径是C:/Program Files/VTK。该选项的值可不作更…

Halcon 从XML中读取配置参数

1、XML示例 以下是一个XML配置文件的示例,该文件包含了AOI(自动光学检测)算法的环境参数和相机逻辑参数: <AOI><!--AOI算法参数 20241106--><Env><!--环境参数--><Param name="GPUName" value="NVIDIA GeForce RTX 405…

SQL--查询连续三天登录数据详解

问题&#xff1a; 现有用户登录记录表&#xff0c;请查询出用户连续三天登录的所有数据记录 id dt1 2024-04-25 1 2024-04-26 1 2024-04-27 1 2024-04-28 1 2024-04-30 1 2024-05-01 1 2024-05-02 1 2024-05-04 1 2024-05-05 2 20…

结构方程、生物群落、数据统计、绘图分析在生态领域的应用

R语言结构方程模型&#xff08;SEM&#xff09;在生态学领域中的实践应用 结构方程模型&#xff08;Sructural Equation Model&#xff09;是一种建立、估计和检验研究系统中多变量间因果关系的模型方法&#xff0c;它可以替代多元回归、因子分析、协方差分析等方法&#xff0…

vue使用canves把数字转成图片验证码

<canvas id"captchaCanvas" width"100" height"40"></canvas>function drawCaptcha(text) {const canvas document.getElementById(captchaCanvas);const ctx canvas.getContext(2d);// 设置背景颜色ctx.fillStyle #f0f0f0;ctx.f…

双指针算法习题解答

1.移动零 题目链接&#xff1a;283. 移动零 - 力扣&#xff08;LeetCode&#xff09; 题目解析&#xff1a;该题要求将数组中为0的元素全部转移到数组的末尾&#xff0c;同时不能改变非零元素的相对位置。 解题思路&#xff1a;我们可以用变量dest和cur将该数组分为三个区域。…

「Mac畅玩鸿蒙与硬件23」鸿蒙UI组件篇13 - 自定义组件的创建与使用

自定义组件可以帮助开发者实现复用性强、逻辑清晰的界面模块。通过自定义组件&#xff0c;鸿蒙应用能够提高代码的可维护性&#xff0c;并简化复杂布局的构建。本篇将介绍如何创建自定义组件&#xff0c;如何向组件传递数据&#xff0c;以及如何在不同页面间复用这些组件。 关键…

【SpringCloud】Nacos微服务注册中心

微服务的注册中心 注册中心可以说是微服务架构中的"通讯录"&#xff0c;它记录了服务和服务地址的映射关系 。在分布式架构中&#xff0c; 服务会注册到这里&#xff0c;当服务需要调⽤其它服务时&#xff0c;就从这里找到服务的地址&#xff0c;进行调用。 注册中心…