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;然后您的…

深入解析语音识别中的关键技术:GMM、HMM、DNN和语言模型

目录 一、高斯混合模型&#xff08;GMM&#xff09;与期望最大化&#xff08;EM&#xff09;算法二、隐马尔可夫模型&#xff08;HMM&#xff09;三、深度神经网络&#xff08;DNN&#xff09;四、语言模型&#xff08;LM&#xff09;五、ASR系统的整体工作流程结论 在现代语音…

如何将现有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…

音频模型介绍

在处理音频数据方面&#xff0c;有多种模型表现出色&#xff0c;它们在不同的音频处理任务上有着各自的优势&#xff1a; 自动编码器&#xff1a;包括多通道变分自动编码器、自回归模型和生成对抗网络等&#xff0c;这些模型在音乐生成领域取得了令人印象深刻的成果。 深度生成…

云计算基础:AWS入门指南

&#x1f493; 博客主页&#xff1a;瑕疵的CSDN主页 &#x1f4dd; Gitee主页&#xff1a;瑕疵的gitee主页 ⏩ 文章专栏&#xff1a;《热点资讯》 云计算基础&#xff1a;AWS入门指南 云计算基础&#xff1a;AWS入门指南 云计算基础&#xff1a;AWS入门指南 引言 AWS概述 什么…

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

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

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

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

python 天气数据可视化

Python爬取天气数据及可视化分析 https://blog.csdn.net/weixin_69423932/article/details/135184643

基于Arduino的RGB灯按键控制

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

非关系型数据库(1)---MongoDB

目录 1.MongoDB 1.MongoDB的特点 2.MongoDB的应用场景 3.MongoDB与MySQL的比较 2.数据库操作 1. 创建数据库 2. 切换数据库 3. 查看所有数据库 4. 查看当前数据库 5. 删除数据库 6. 查看数据库统计信息 7. 备份数据库 8. 恢复数据库 9. 创建用户和授权 10. 删除用…

1. pytorch 中冻结模型参数后参数仍会被调整

问题 self.sgf_net.requires_grad_(False)起初设置 requires_grad(False) 优化器也没有添加sgfnet的模型参数。但是在pylightning框架中&#xff0c;每次推理完模型的参数都会被改变&#xff0c;经过仔细排查发现问题 # self.sgf_net.requires_grad_(False)for param in self.s…

大模型-微调与对齐-人类对齐背景与标准

1、目的 确保大模型的行为与人类价值观、人类真实意图和社会伦理相一致 2、大模型有害行为 无法正确遵循指令生成虚假信息产生有害、有误导性、有偏见的表达 3、评估标准 有用性诚实性无害性 4、更细化的对齐标准 行为对齐&#xff1a;要求AI能够做出符合人类期望的行为…

hive数据查询语法

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

Docker lmdeploy 快速部署Qwen2.5模型openai接口

启动服务 我已经把模型下载到/data/xiedong/Qwen2.5-72B-Instruct-GPTQ-Int4 所以docker直接启动: docker run --runtime nvidia --gpus device=0 \-p 23333:23333 \--ipc=host -v /data/xiedong:/data/xiedong/ \openmmlab/lmdeploy:latest \lmdeploy serve api_server /d…

前端UniApp面试题及参考答案(100道题)

目录 UniApp 支持哪些平台? UniApp 在不同平台上的表现有何差异? 如何处理 UniApp 中的平台差异? UniApp 项目创建与目录结构 项目创建 目录结构 如何创建一个 UniApp 项目? UniApp 项目的基本目录结构是什么样的? 解释一下 UniApp 中的页面生命周期钩子函数有哪…