匿名无人机飞控代码整理5

2023-05-16

先跳过高度部分,这里看location部分

在这里插入图片描述
可以看出来这部分也是直接与我们的循迹、追踪任务相关的。所以这部分的掌握应该很重要把。
前面有很多暂时不需要看,flyctrl那个巨长。。。所以就先看速度环控制部分了。

if(switchs.uwb_on && (!switchs.of_flow_on) && (!switchs.gps_on))
{
	mode_f[1] = 3;
	if(mode_f[1] != mode_f[0])
	{
		Loc_1level_PID_Init();
		mode_f[0] = mode_f[1];
	}	
	
	loc_ctrl_1.out[X] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[X] ;
	loc_ctrl_1.out[Y] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[Y] ;
		
}

如果只有uwb(其实没有),就对loc_ctrl_1.out赋值,这个结构体是这样的:

typedef struct
{
	float exp[VEC_XYZ];
	s32 fb[VEC_XYZ];

	
	float out[VEC_XYZ];
}_loc_ctrl_st;

做pid用。

	else if(switchs.uwb_on && switchs.of_flow_on && (!switchs.gps_on))
	{
		mode_f[1] = 4;
		if(mode_f[1] != mode_f[0])
		{
			Loc_1level_PID_Init();
			mode_f[0] = mode_f[1];
		}	



		h2w_2d_trans(fs.speed_set_h,imu_data.hx_vec,loc_ctrl_1.exp);
		
		LPF_1_(5.0f,dT_ms*1e-3f,imu_data.w_acc[X],vel_fb_d_lpf[X]);
		LPF_1_(5.0f,dT_ms*1e-3f,imu_data.w_acc[Y],vel_fb_d_lpf[Y]);		
	
		if(sens_hd_check.of_ok)
		{
			vel_fb_h[0] = OF_DX2;
			vel_fb_h[1] = OF_DY2;
		}
		else//sens_hd_check.of_df_ok
		{
			vel_fb_h[0] = of_rdf.gnd_vel_est_h[X];
			vel_fb_h[1] = of_rdf.gnd_vel_est_h[Y];	
		}
		
		h2w_2d_trans(vel_fb_h,imu_data.hx_vec,vel_fb_w);
		
		loc_ctrl_1.fb[X] = vel_fb_w[0] + 0.03f *vel_fb_d_lpf[X];
		loc_ctrl_1.fb[Y] = vel_fb_w[1] + 0.03f *vel_fb_d_lpf[Y];
		
		fb_speed_fix[0] = uwb_data.w_vel_cmps[0];
		fb_speed_fix[1] = uwb_data.w_vel_cmps[1];
		
		for(u8 i =0;i<2;i++)
		{
			PID_calculate( dT_ms*1e-3f,           
										loc_ctrl_1.exp[i] ,				
										loc_ctrl_1.exp[i] ,				
										loc_ctrl_1.fb[i] ,			
										&loc_arg_1[i], 
										&loc_val_1[i],	
										50,
										10 *flag.taking_off			
										 )	;	
			
			
			PID_calculate( dT_ms*1e-3f,           
										loc_ctrl_1.exp[i] ,				
										loc_ctrl_1.exp[i] ,				
										fb_speed_fix[i] ,			
										&loc_arg_1_fix[i], 
										&loc_val_1_fix[i],	
										50,
										10 *flag.taking_off		
										 )	;	
			
			pos_ctrl_w_out[i] = loc_val_1[i].out + loc_val_1_fix[i].out;	
		}	
		
		w2h_2d_trans(pos_ctrl_w_out,imu_data.hx_vec,pos_ctrl_h_out); 
		
		loc_ctrl_1.out[0] = pos_ctrl_h_out[0];
		loc_ctrl_1.out[1] = pos_ctrl_h_out[1];	
	}

否则,如果只有光流和uwb,没有gps,那么对mode做相同的操作,接着使用了物坐标系对世界坐标的转换:

void h2w_2d_trans(float h[VEC_XYZ],float ref_ax[VEC_XYZ],float w[VEC_XYZ])
{
	w[X] = h[X] *ref_ax[X] + h[Y] *(-ref_ax[Y]);
	w[Y] = h[X] *ref_ax[Y] + h[Y] *  ref_ax[X];
	
}

也就是把速度(相对于物坐标系的),转换成世界坐标系的,赋给loc_ctrl_1.exp,也就是预期的速度。怎么理解呢。。。不知道,再看。。

低通滤波是干嘛的,不知道。。

	if(sens_hd_check.of_ok)
	{
		vel_fb_h[0] = OF_DX2;
		vel_fb_h[1] = OF_DY2;
	}
	else
	{
		vel_fb_h[0] = of_rdf.gnd_vel_est_h[X];
		vel_fb_h[1] = of_rdf.gnd_vel_est_h[Y];	
	}

如果光流ok,那么把物坐标系的速度期望值赋为,这个,否则赋为那个,是这样吧。。
然后又转坐标系:

h2w_2d_trans(vel_fb_h,imu_data.hx_vec,vel_fb_w);

把物坐标系的测量来的速度信息转化为世界坐标系。

loc_ctrl_1.fb[X] = vel_fb_w[0] + 0.03f *vel_fb_d_lpf[X];
loc_ctrl_1.fb[Y] = vel_fb_w[1] + 0.03f *vel_fb_d_lpf[Y];

对控制结构体中的反馈部分赋值。

	fb_speed_fix[0] = uwb_data.w_vel_cmps[0];
	fb_speed_fix[1] = uwb_data.w_vel_cmps[1];

速度修正,可能利用了uwb。。。
然后用了两个pid,一个结果加一个修正值,结果pos_ctrl_w_out[i]等于两个值相加

w2h_2d_trans(pos_ctrl_w_out,imu_data.hx_vec,pos_ctrl_h_out); 

再把得到的结果转回物坐标系。

	loc_ctrl_1.out[0] = pos_ctrl_h_out[0];
	loc_ctrl_1.out[1] = pos_ctrl_h_out[1];	

转完输出给loc_ctrl_1.out[]。。。要记住这个loc_ctrl_1就是物坐标系

如果仅有光流:

else if(switchs.of_flow_on && (!switchs.gps_on))
{
	mode_f[1] = 1;
	if(mode_f[1] != mode_f[0])
	{
		Loc_1level_PID_Init();
		mode_f[0] = mode_f[1];
	}
	
	loc_ctrl_1.exp[X] = fs.speed_set_h[X];
	loc_ctrl_1.exp[Y] = fs.speed_set_h[Y];
	//
	LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[X],vel_fb_d_lpf[X]);
	LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[Y],vel_fb_d_lpf[Y]);		
	
	if(sens_hd_check.of_ok)
	{
		loc_ctrl_1.fb[X] = OF_DX2 + 0.03f *vel_fb_d_lpf[X];
		loc_ctrl_1.fb[Y] = OF_DY2 + 0.03f *vel_fb_d_lpf[Y];
		
		fb_speed_fix[0] = OF_DX2FIX;
		fb_speed_fix[1] = OF_DY2FIX;
	}
	else//sens_hd_check.of_df_ok
	{
		loc_ctrl_1.fb[X] = of_rdf.gnd_vel_est_h[X] + 0.03f *vel_fb_d_lpf[X];
		loc_ctrl_1.fb[Y] = of_rdf.gnd_vel_est_h[Y] + 0.03f *vel_fb_d_lpf[Y];
		
		fb_speed_fix[0] = of_rdf.gnd_vel_est_h[X];
		fb_speed_fix[1] = of_rdf.gnd_vel_est_h[Y];		
	}
	
	for(u8 i =0;i<2;i++)
	{
		PID_calculate( dT_ms*1e-3f,            //ÖÜÆÚ£¨µ¥Î»£ºÃ룩
									loc_ctrl_1.exp[i] ,				//Ç°À¡Öµ
									loc_ctrl_1.exp[i] ,				//ÆÚÍûÖµ£¨É趨ֵ£©
									loc_ctrl_1.fb[i] ,			//·´À¡Öµ£¨£©
									&loc_arg_1[i], //PID²ÎÊý½á¹¹Ìå
									&loc_val_1[i],	//PIDÊý¾Ý½á¹¹Ìå
									50,//»ý·ÖÎó²îÏÞ·ù
									10 *flag.taking_off			//integration limit£¬»ý·ÖÏÞ·ù
									 )	;	
		
		//fix
		PID_calculate( dT_ms*1e-3f,            //ÖÜÆÚ£¨µ¥Î»£ºÃ룩
									loc_ctrl_1.exp[i] ,				//Ç°À¡Öµ
									loc_ctrl_1.exp[i] ,				//ÆÚÍûÖµ£¨É趨ֵ£©
									fb_speed_fix[i] ,			//·´À¡Öµ£¨£©
									&loc_arg_1_fix[i], //PID²ÎÊý½á¹¹Ìå
									&loc_val_1_fix[i],	//PIDÊý¾Ý½á¹¹Ìå
									50,//»ý·ÖÎó²îÏÞ·ù
									10 *flag.taking_off			//integration limit£¬»ý·ÖÏÞ·ù
									 )	;	
		
		loc_ctrl_1.out[i] = loc_val_1[i].out + loc_val_1_fix[i].out;	//(PD)+(I)	
	}		
}

处理差不多好像。。。但是少了坐标变换。。?不太懂

接下来一个就是有gps的。。。好长的判断语句,不看了。。。

最后一个是gps、光流、uwb都没有,那就直接利用期望速度得出期望姿态角,这里比较好理解,但恐怕实际上的情况是有光流无gps、uwb,所以应该是第三段代码,但有光流实际上就是在定位。。。

所以说在实际中,我们想要前进,就改变switch。
打开of的switch,那么就可以实现定位,关闭并设置期望速度,就可以做到前进。。

在具体一点,怎么移动、物坐标系和世界坐标系等等,再看。。。

本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)

匿名无人机飞控代码整理5 的相关文章

  • 树莓派登录问题

    在使用树莓派的过程中 xff0c 多次遇到了开机后输入正确密码却被重复多次要求输入的情况 xff0c 在查看他人经验后 xff0c 总结出以下两种情况及可行的方法 不确定密码是否正确 参考博客 纸末 树莓派忘记密码的解决方法 1 xff0c
  • 什么是SDK和API?

    SDK xff1a 在搭建环境的时候知道的sdk xff0c 但并不知道有什么用 SDK Software Development Kit 翻译成中文就是 34 软体开发工具组 34 是用来帮一个 产品 或 平台 开发应用程式的工具组 xf
  • 华为开发者大会总结——个人总结

    方舟编译器 xff08 开源 xff09 xff1a 干掉Java虚拟机 将java代码直接编译成机器码 xff0c 静态语义好编译 xff0c 核心是静态编译出动态语义 xff08 通过华为编译实验室的核心专利 xff09 xff0c 代
  • LoRa关键参数 1、扩频因子(SF) 2、编码率(CR) 3、信号带宽(BW) 4、LoRa信号带宽BW、符号速率Rs和数据速率DR的关系 5、

    LoRa 学习 xff1a LoRa 关键参数 xff08 扩频因子 xff0c 编码率 xff0c 带宽 xff09 的设定及解释 1 扩频因子 xff08 SF xff09 2 编码率 xff08 CR xff09 3 信号带宽 xff
  • MarkDown的语法

    使用MarkDoown时应该注意些什么呢 目录 一 添加标题 二 引用代码块 三 嵌入图片 1 本地图片 2 互联网图片 四 快捷键使用 五 分界线的使用 一 添加标题 在文字前加入相应数量的 一级标题 二级标题 三级标题 四级标题 五级标
  • python试爬李毅吧贴子标题,爬虫最初级

    注 xff1a 以下所有python代码均运行于2 7 0 最近想抓点数据存起来 xff0c 开始捣鼓python 爬虫技术以前没接触过 xff0c 这一回就当练手 xff0c 从零开始 xff0c 从最原始的方式开始 先定个小目标 xff
  • Activity的生命周期

    图来自百度百科 onCreate 启动activity时被调用 xff0c 用于进行初始化操作 xff08 加载布局 绑定事件等 xff09 xff0c 不应在onCreate 中做过多的不必要操作 xff0c 避免造成打开activity
  • Activity的启动模式

    以下为读书笔记 xff1a 实际项目中 xff0c 我们要通过特定的需求 xff0c 为每个活动指定恰当的启动模式 Android一共有4种启动模式 xff1a standard singleTop singleTask singleIns
  • PID参数 Ziegler-Nichols基于时域响应曲线的整定 反应曲线法

    PID控制器是工业过程控制中广泛采用的一种控制器 xff0c 其中 xff0c P I D分别为比例 xff08 Proportion xff09 积分 xff08 Integral xff09 微分 xff08 Differential
  • 郑学坚《微型计算机原理及应用》考点精讲 36讲

    链接 xff1a https pan baidu com s 12 vGBRrjOd UtO8P4e9bow 提取码 xff1a tqmp 当初考研时买的网课 xff0c 现在也用不着了 xff0c 赠与有缘人 书籍记录着别人多年时光总结出
  • 几种常用排序算法

    排序算法代码如下 xff1a void Sort Algorithm Bubble Sort int amp nums const int len bool haschange 61 true for int i 61 0 i lt len
  • 四旋翼姿态解算

    代码思路如下 xff1a 姿态解算 void IMU update float dT imu state st state float gyr VEC XYZ s32 acc VEC XYZ s16 mag val VEC XYZ imu
  • 树莓派4B-基于MCP2515的CAN通信过程记录篇

    本文主要记录使用树莓派4B xff0c 通过外接MCP2515模块来实现CAN通信 xff0c 使用基于Can utils实现CAN消息的收发 准备工作 xff1a 树莓派MCP2515模块杜邦线若干 知乎上一篇较为详细的参考 xff0c
  • nuxt踩坑集

    目录结构 assets 资源文件 用于组织未编译的静态资源如 LESS SASS或 JavaScript components 组件 layouts page 模板页面 xff0c 默认为 default vue可以在这个目录下创建全局页面
  • 记录个人在安装D435i驱动时出现的问题及解决方案

    目录 1 驱动安装 xff1a 2 启动Intel RealSense Viewer后报错No Frames Recevied 3 Record时报错Error opening file 1 驱动安装 xff1a 参考这篇博客 xff1a
  • 深度学习常见的优化算法

    文章目录 1 梯度下降算法 xff08 batch gradient descent BGD 2 随机梯度下降法 xff08 Stochastic gradient descent SGD 3 小批量梯度下降 Mini batch grad
  • vscode如何链接github

    1 首先安装配置好vscode xff1a https code visualstudio com download 然后下载git xff1a https git scm com download 根据自己的电脑选择相应的版本的下载就好了
  • 串口(uart)开发流程

    UART概述 UART全称 xff0c Universal Asynchronous Receiver and Transmitter UART xff0c 一般是每个单片机或开发板必备的一个功能模块 xff0c 可以用来调试 xff0c
  • *33.硬中断和软中断是什么?区别?

    1 硬中断 硬件中断 像磁盘 xff0c 网卡 xff0c 键盘 xff0c 时钟等 2 软中断 软件中断 进程产生 区别 xff1a xff08 产生机制 处理顺序 可否屏蔽 中断信号由来 xff09 1 软中断的发生的时间是由程序控制的
  • gazebo学习时遇到的问题(PX4篇)

    最近在研究如何使用UAV xff0c 由于是初学者 xff0c 不好直接上手实体无人机 xff0c 因此考虑通过在gazebo中模拟的方式进行一下无人机的简单使用模拟 xff0c 于是了解到了PX4这个东西 xff0c 他不仅支持固件写入

随机推荐

  • 阿里云服务器的搭建和部署(小白教程)

    阿里云服务器的搭建和部署 xff08 小白教程 xff09 一 购买服务器二 管理云服务器三 环境配置1 JDK的配置与下载2 tomcat的配置与下载3 MySQL的安装与配置 四 外网访问服务器 个人推荐阿里云 因为里面有一个大学生的云
  • 一篇带你搞懂Python3 中的 def __init__

    在学习python 100 days时 xff0c 在面向对象编程基础的那一块 xff0c 封装一个class时 xff0c 突然出现def init 的方法 xff0c 刚开始时 xff0c 对他的理解很模糊 xff0c 为什么定义一个类
  • 时间序列异常检测综述

    1 Introduction 时序异常检测几个可以运用的点 xff1a 1 欺诈检测 2 工业数据检测 简介一下 xff1a 异常检测很久之前就有了 最早可以追溯到 J Fox 1972 Outliers in Time Series Jo
  • Linux基础----Makefile(1)

    前言 刚开始学习Linux xff0c 这些那真的是新知识 xff0c 由于感觉将来FPGA的学习会越来越靠近软件的方向发展 xff0c 所以觉得有必要好好地学习一下嵌入式的东西 xff0c 因此有必要把学习的过程记录下来 xff0c 方便
  • 【OpenCV】ChArUco标定板角点的检测Detection of ChArUco Corners

    opencv3 4 15源文档链接 link ChArUco标定板角点的检测 GoalSource codeCharuco板创建ChArUco板检测ChArUco姿势估计 ArUco标记和板的快速检测和多功能性是非常有用的 然而 xff0c
  • 错误 MSB3721

    错误 MSB3721 命令 C Program Files NVIDIA GPU Computing Toolkit CUDA v10 2 bin nvcc exe gencode 61 arch 61 compute 30 code 61
  • 倒立摆状态反馈控制——分析、建模与仿真(matlab)

    倒立摆状态反馈控制 引言 倒立摆是机器人学中一个非常重要的模型 xff0c 火箭 导弹 独轮车 双足机器人 四足机器人 xff0c 基本都是倒立摆的变形 问题描述 关于倒立摆的问题描述如下 xff1a 如图所示的倒立摆系统 摆的关节连接在一
  • docker pull rabbitmq:management 怎样指定版本

    docker pull rabbitmq 3 8 1 management
  • git 命令学习

    git 命令学习 git clone https github com zhaji01 notes git 克隆远程仓库 git status 本地仓库状态 git add lt file gt 把修改 xff08 包括创建 修改 删除 解
  • 【实例记录】在ubuntu上运行python实现与单片机多线程串口通信

    文章目录 工具步骤 工具 工具 xff1a 自己电脑双系统的ubuntu18 04 单片机esp8266 一个usb转ttl模块 xff0c ubuntu上已经下载了python的3 6和pip xff0c 还需要pip install s
  • 【过程记录 】windows和ubuntu两台电脑局域网进行socket通信收发数据和传输文件

    实验图片和前期准备 xff1a ubuntu作为服务端 xff0c windows客户端传输数据和文件 xff1a windows作为服务端 xff0c ubuntu作为客户端 xff1a 实验图片和前期准备 xff1a 服务端发送和接受i
  • 过程记录 yolov3目标检测输出目标坐标和原图物体像素坐标比较

    设备 xff1a jetsonnano B01 一个usb广角摄像头 语言为python 库为opencv 框架pytorch 大家都知道yolov3的坐标变换过程比较复杂 xff0c 大概要经过图片缩放 xff0c 对数空间变换等等 xf
  • Rt-Thread学习笔记-----信号量(五)

    线程间同步 1 什么是线程间同步 xff1f 同步是指按预定的先后次序进行运行 xff0c 线程同步是指多个线程通过特定的机制来控制线程之间的执行顺序 xff0c 也可以说是在线程之间通过同步建立起执行顺序的关系 xff0c 如果没有同步
  • C++类和对象——空指针访问成员函数

    C 43 43 中空指针也是可以访问成员函数的 xff0c 但是也要注意有没有用到this指针 xff1b 如果用到this指针 xff0c 需要加以判断保证代码的健壮性 span class token macro property sp
  • nodejs实现简单的自动化部署

    如题 xff0c nodejs通过码云提供的web hooks实现简单的服务器自动部署 大致流程 xff1a 1 通过码云提供的web hooks xff0c 创建一个request通过后的回调接口 xff08 说白了就是配置一个码云审批通
  • 2022届春招实习面经(CV岗)——offer经

    已拿offer公司 xff1a 腾讯 ponyai vivo研究院 华为 字节 面了一面就没继续面了 xff1a 海康威视 虎牙 京东 本科211 xff0c 硕士985 二区在投论文 43 顶会下的top5比赛 43 小厂三个月实习经历
  • LeetCode刷题笔记 --- python

    目录 一 python交换两个变量的值1 1 使用tuple1 2 使用临时变量temp 二 python中 和 区别三 python列表的使用3 1 列表的基本操作3 2 列表实现栈操作3 3 列表实现排序3 4 列表的算法效率 四 py
  • 无人机高度控制

    这个第一次看就完全没看懂 xff0c 现在再看还是没看懂orz xff0c 而且定高还是重要的功能和考点 xff0c 详细分析一下程序吧 首先是表示高度信息的结构体 xff1a 数据结构声明 typedef struct float Z S
  • ssh_exchange_identification: Connection closed by remote host

    ssh exchange identification Connection closed by remote host 导致这个问题的原因有很多 xff0c 笔者是因为在ssh中设置了代理 xff0c 这个可以作为阅读者排查问题的一个点
  • 匿名无人机飞控代码整理5

    先跳过高度部分 xff0c 这里看location部分 可以看出来这部分也是直接与我们的循迹 追踪任务相关的 所以这部分的掌握应该很重要把 前面有很多暂时不需要看 xff0c flyctrl那个巨长 所以就先看速度环控制部分了 if swi