从零开始学习PX4源码2(PX4姿态误差计算)

目录

文章目录

  • 目录
  • 摘要
  • 1.源码
    • 1.1源码路径
    • 1.2源码程序
    • 1.3源码功能
  • 2.源码分析

摘要

本节主要记录PX4姿态误差计算过程,欢迎批评指正。

1.源码

1.1源码路径

PX4-Autopilot/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp

1.2源码程序

matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{
	Quatf qd = _attitude_setpoint_q;

	// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
	const Vector3f e_z = q.dcm_z();
	const Vector3f e_z_d = qd.dcm_z();
	Quatf qd_red(e_z, e_z_d);

	if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f))
	{
		// In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
		// full attitude control anyways generates no yaw input and directly takes the combination of
		// roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
		qd_red = qd;

	} else
	{
		// transform rotation from current to desired thrust vector into a world frame reduced desired attitude
		qd_red *= q;
	}

	// mix full and reduced desired attitude
	Quatf q_mix = qd_red.inversed() * qd;
	q_mix.canonicalize();
	// catch numerical problems with the domain of acosf and asinf
	q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
	q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
	qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

	// quaternion attitude control law, qe is rotation from q to qd
	const Quatf qe = q.inversed() * qd;

	// using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
	// also taking care of the antipodal unit quaternion ambiguity
	const Vector3f eq = 2.f * qe.canonical().imag();

	// calculate angular rates setpoint
	Vector3f rate_setpoint = eq.emult(_proportional_gain);

	// Feed forward the yaw setpoint rate.
	// yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,
	// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
	// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
	// and multiply it by the yaw setpoint rate (yawspeed_setpoint).
	// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
	// such that it can be added to the rates setpoint.
	if (std::isfinite(_yawspeed_setpoint))
	{
		rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
	}

	// limit rates
	for (int i = 0; i < 3; i++)
	{
		rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
	}

	return rate_setpoint;
}

1.3源码功能

实现姿态误差计算,得到目标角速度。

2.源码分析

在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

对应的PDF下载地址:
下载地址

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/430396.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

[项目设计] 从零实现的高并发内存池(三)

&#x1f308; 博客个人主页&#xff1a;Chris在Coding &#x1f3a5; 本文所属专栏&#xff1a;[高并发内存池] ❤️ 前置学习专栏&#xff1a;[Linux学习] ⏰ 我们仍在旅途 ​ 目录 4.CentralCache实现 4.1 CentralCache整体架构 4.2 围绕Span的相关设计…

STM32CubeIDE基础学习-基础外设初始化配置

STM32CubeIDE基础学习-基础外设初始化配置步骤 前言 前面的文章介绍了基础工程的创建步骤&#xff0c;这篇文章就接着在基础工程的基础上来配置相关外设了&#xff0c;下面以STM32F103C8T6的主芯片为例进行简单配置。 基础工程创建步骤回顾 具体的配置步骤流程如下&#xff1…

使用git的小笔记

平时工作中使用git存储项目代码&#xff0c; 常用的命令 拉取仓库代码 git clone http://100.100.100.100:9080/my_test/test.git 拉取到以后&#xff0c; 先切换到自己的分支 git checkout my_name 一顿魔改代码 然后 add 新增的文件或者修改的文件 git add * 然后提交 并写…

美国和中国互相竞争的计算机(大脑)都由哪些东西组成

计算机基础 本章要点 -> 计算机的发展史 -> 计算机的分类 -> PC系统的组成 一 计算机的发展 1946年2月14日&#xff0c;在美国宾夕法尼亚大学&#xff0c;众所周知的世界上 第一台电子数字计算机ENIAC诞生 计算机发展至今&#xff0c;共经历了以下几个阶…

netlink原理及应用

什么是netlink netlink是一种基于网络的通信机制&#xff0c;允许内核内部、内核与用户态应用之间甚至用户态应用之间进行通信&#xff1b;netlink的主要作用是内核与用户态之间通信&#xff1b;它的思想是&#xff0c;基于BSD的socket使用网络框架在内核和用户态之间进行通信…

docker安装ES和kibana

文章目录 一、安装Elasticsearch1. 安装Elasticsearch2. 安装IK分词器3. elasticsearch-head 监控的插件4. 配置跨域 二、安装kibana 提示&#xff1a;以下是本篇文章正文内容&#xff0c;下面案例可供参考 一、安装Elasticsearch 1. 安装Elasticsearch 安装Elasticsearch参…

揭秘麦肯锡的方法:产品经理解决问题指南

您是否想知道世界上最成功的产品经理如何始终如一地提供不仅满足而且超出预期的解决方案&#xff1f;秘密可能就在于世界上最负盛名的咨询公司之一麦肯锡公司所磨练的方法论。本文深入探讨了麦肯锡的问题解决流程&#xff0c;该流程专为希望提升水平的产品经理量身定制。 01. 麦…

java 面试题总结

1锁粗化和锁消除&#xff0c;锁膨胀和锁升级的区别。 https://www.cnblogs.com/xuxinstyle/p/13387778.html .无锁 < 偏向锁 < 轻量级锁 < 重量级锁 &#xff0c;说的时候不要忘记说无锁状态 2.Map 的实现&#xff0c;线程安全的实现 1、ConcurrentHashMap在JDK 1.7…

【AI视野·今日CV 计算机视觉论文速览 第300期】Fri, 1 Mar 2024

AI视野今日CS.CV 计算机视觉论文速览 Fri, 1 Mar 2024 Totally 114 papers &#x1f449;上期速览✈更多精彩请移步主页 Daily Computer Vision Papers DistriFusion: Distributed Parallel Inference for High-Resolution Diffusion Models Authors Muyang Li, Tianle Cai, J…

国创证券|AI手机PC概念拉升,福蓉科技4连板,思泉新材大涨

AI手机PC概念大幅走高&#xff0c;到发稿&#xff0c;思泉新材涨近15%&#xff0c;盘中一度涨停&#xff1b;道明光学、福蓉科技均涨停&#xff0c;诚迈科技涨近8%&#xff0c;亿道信息涨逾6%。 值得注意的是&#xff0c;福蓉科技已接连4个交易日涨停。公司4日晚间发布危险提示…

【c++】STL--List的实现

目录 一. List的数据结构 二. List实现的基本框架 1. list的结点结构类 2. List的迭代器类 正向迭代器 反向迭代器 3. List操作接口的实现 1. 默认成员函数 构造函数 和 析构函数 拷贝构造函数 和 赋值运算符重载 2. 修改相关函数接口 insert 和 erase …

pytorch统计属性

目录 1.normal2. mean, sum, min, max, prod3.argmin, argmax4. topk kthvalue5. compare6.norm 1.normal torch.normal(mean, std, *, generatorNone, outNone) → Tensor返回一个张量&#xff0c;其中的每个元素随机来自独立的标准正态分布。这些分布具有给定的均值和标准差…

12-Linux部署Zookeeper集群

Linux部署Zookeeper集群 简介 ZooKeeper是一个分布式的&#xff0c;开放源码的分布式应用程序协调服务&#xff0c;是Hadoop和Hbase的重要组件。它是一个为分布式应用提供一致性服务的软件&#xff0c;提供的功能包括&#xff1a;配置维护、域名服务、分布式同步、组服务等。…

入门LLMs开发 — LangChain

像OpenAI的GPT-4这样的大型语言模型&#xff08;LLMs&#xff09;已经风靡全球。它们可以自动执行各种任务&#xff0c;如回答问题、翻译语言、分析文本等。LLMs是第一种真正感觉像“人工智能”的机器学习类型。 然而&#xff0c;在将LLMs应用于实际产品时仍然存在挑战。特别是…

特氟龙塑料试剂瓶对比普通塑料和玻璃试剂瓶的优势

试剂瓶作为实验室中常备器皿耗材之一&#xff0c;主要用来盛放和储存样品、试剂&#xff0c;根据使用条件不同&#xff0c;也可叫取样瓶、样品瓶、储样瓶、广口瓶等。 根据瓶口口径不同&#xff0c;可分为广口瓶和窄口瓶&#xff0c;广口瓶口径较大&#xff0c;主要用于储存固…

Vue.js 实用技巧:深入理解 Vue.mixin

&#x1f90d; 前端开发工程师、技术日更博主、已过CET6 &#x1f368; 阿珊和她的猫_CSDN博客专家、23年度博客之星前端领域TOP1 &#x1f560; 牛客高级专题作者、打造专栏《前端面试必备》 、《2024面试高频手撕题》 &#x1f35a; 蓝桥云课签约作者、上架课程《Vue.js 和 E…

JProfiler详解 JVM性能监测内存泄露分析工具

JProfiler详解 JProfiler简介主要功能特点使用场景注意事项使用案例使用步骤Could not verify ssh-ed25519 host key with fingerprint 问题解决内存泄露分析 JProfiler简介 JProfiler是一款业界领先的Java性能分析工具&#xff0c;由ej-technologies公司开发&#xff0c;专门…

【亲测】注册Claude3教程,解决Claude3被封号无法发送手机验证码

【亲测】注册Claude3教程&#xff1a;解决无法发送手机验证码的问题 Anthropic 今日宣布推出其最新大型语言模型&#xff08;LLM&#xff09;系列——Claude 3&#xff0c;这一系列模型在各种认知任务上树立了新的性能标准。Claude 3 系列包括三个子模型&#xff1a;Claude 3 …

微服务架构SpringCloud(2)

热点参数限流 注&#xff1a;热点参数限流默认是对Springmvc资源无效&#xff1b; 隔离和降级 1.开启feign.sentinel.enabletrue 2.FeignClient(fallbackFactory) 3.创建一个类并实现FallbackFactory接口 4.加入依赖 <!--添加Sentienl依赖--><dependency><gro…

Linux开发工具使用

一、Linux软件包管理器 yum 软件包和软件包管理器, 就好比 "App" 和 "应用商店" &#xff0c;我们现在要安装的yum就是相当于在我们的Linux终端安装一个"应用商店"。 但使用yum时&#xff0c;我们一定要保证主机(虚拟机)网络畅通!这点也非常好理…