ICM20948 DMP代码详解(13)

news2024/11/24 10:57:54

接前一篇文章:ICM20948 DMP代码详解(12)

上一回完成了对inv_icm20948_set_chip_to_body_axis_quaternion函数第2步即inv_rotation_to_quaternion函数的解析。回到inv_icm20948_set_chip_to_body_axis_quaternion中来,继续往下进行解析。为了便于理解和回顾,再次贴出该函数源码:

void inv_icm20948_set_chip_to_body_axis_quaternion(struct inv_icm20948 *s, signed char *accel_gyro_matrix, float angle)
{
    int i;
    float rot[9];
    long qcb[4];
    long q_all[4];
    long q_adjust[4];
 
    for (i=0; i<9; i++)
        rot[i] = (float)accel_gyro_matrix[i];
 
    //convert Chip to Body transformation matrix to quaternion
    //inv_icm20948_convert_matrix_to_quat_fxp(rot, qcb);
	inv_rotation_to_quaternion(rot, qcb);
 
    //The quaterion generated is the inverse, take the inverse again.
    qcb[1] = -qcb[1];
    qcb[2] = -qcb[2];
    qcb[3] = -qcb[3];
 
    //now rotate by angle, negate angle to rotate other way
    q_adjust[0] = (long)((1L<<30) * cosf(-angle*(float)M_PI/180.f/2.f));
    q_adjust[1] = 0;
    q_adjust[2] = (long)((1L<<30) * sinf(-angle*(float)M_PI/180.f/2.f));
    q_adjust[3] = 0;
    invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);
    inv_icm20948_set_chip_to_body(s, q_all);
}

接下来是第3段代码:

    //The quaterion generated is the inverse, take the inverse again.
    qcb[1] = -qcb[1];
    qcb[2] = -qcb[2];
    qcb[3] = -qcb[3];

在第2步中由旋转矩阵(数组rot[9])得到了四元数,在这里还需要进一步对于qcb[1]~qcb[3]做乘以-1操作。

接下来是第4段代码:

    //now rotate by angle, negate angle to rotate other way
    q_adjust[0] = (long)((1L<<30) * cosf(-angle*(float)M_PI/180.f/2.f));
    q_adjust[1] = 0;
    q_adjust[2] = (long)((1L<<30) * sinf(-angle*(float)M_PI/180.f/2.f));
    q_adjust[3] = 0;
    invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);
    inv_icm20948_set_chip_to_body(s, q_all);

代码中的angle来自inv_icm20948_set_chip_to_body_axis_quaternion函数的第3个参数float angle,其对应的实参在inv_icm20948_init_matrix函数中传入,为0.0。

由于这里传入的值为0.0,因此以上代码片段

    //now rotate by angle, negate angle to rotate other way
    q_adjust[0] = (long)((1L<<30) * cosf(-angle*(float)M_PI/180.f/2.f));
    q_adjust[1] = 0;
    q_adjust[2] = (long)((1L<<30) * sinf(-angle*(float)M_PI/180.f/2.f));
    q_adjust[3] = 0;

实际上是:

    //now rotate by angle, negate angle to rotate other way
    q_adjust[0] = (long)((1L<<30);
    q_adjust[1] = 0;
    q_adjust[2] = 0;
    q_adjust[3] = 0;

接下来是invn_convert_quat_mult_fxp函数。

    invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);

其在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.c中,代码如下:

static void invn_convert_quat_mult_fxp(const long *quat1_q30, const long *quat2_q30, long *quatProd_q30)
{
    quatProd_q30[0] = inv_icm20948_convert_mult_q30_fxp(quat1_q30[0], quat2_q30[0]) - inv_icm20948_convert_mult_q30_fxp(quat1_q30[1], quat2_q30[1]) -
               inv_icm20948_convert_mult_q30_fxp(quat1_q30[2], quat2_q30[2]) - inv_icm20948_convert_mult_q30_fxp(quat1_q30[3], quat2_q30[3]);

    quatProd_q30[1] = inv_icm20948_convert_mult_q30_fxp(quat1_q30[0], quat2_q30[1]) + inv_icm20948_convert_mult_q30_fxp(quat1_q30[1], quat2_q30[0]) +
               inv_icm20948_convert_mult_q30_fxp(quat1_q30[2], quat2_q30[3]) - inv_icm20948_convert_mult_q30_fxp(quat1_q30[3], quat2_q30[2]);

    quatProd_q30[2] = inv_icm20948_convert_mult_q30_fxp(quat1_q30[0], quat2_q30[2]) - inv_icm20948_convert_mult_q30_fxp(quat1_q30[1], quat2_q30[3]) +
               inv_icm20948_convert_mult_q30_fxp(quat1_q30[2], quat2_q30[0]) + inv_icm20948_convert_mult_q30_fxp(quat1_q30[3], quat2_q30[1]);

    quatProd_q30[3] = inv_icm20948_convert_mult_q30_fxp(quat1_q30[0], quat2_q30[3]) + inv_icm20948_convert_mult_q30_fxp(quat1_q30[1], quat2_q30[2]) -
               inv_icm20948_convert_mult_q30_fxp(quat1_q30[2], quat2_q30[1]) + inv_icm20948_convert_mult_q30_fxp(quat1_q30[3], quat2_q30[0]);
}

这个函数中最主要就是调用了inv_icm20948_convert_mult_q30_fxp函数进行计算。inv_icm20948_convert_mult_q30_fxp函数在同文件中,代码如下:

long inv_icm20948_convert_mult_q30_fxp(long a_q30, long b_q30)
{
	long long temp;
	long result;

	temp = (long long)a_q30 * b_q30;
	result = (long)(temp >> 30);

	return result;
}

其实就是简单的乘法,之后再右移30位。

那么invn_convert_quat_mult_fxp函数的作用实际上就是计算两个四元数相乘的结果。

参考:四元数乘法计算-CSDN博客

回到inv_icm20948_set_chip_to_body_axis_quaternion函数中,接下来是inv_icm20948_set_chip_to_body函数。

    invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);
    inv_icm20948_set_chip_to_body(s, q_all);

其也在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.c中,代码如下:

/** Set the transformation used for chip to body frame
*/
void inv_icm20948_set_chip_to_body(struct inv_icm20948 * s, long *quat)
{
    memcpy(s->s_quat_chip_to_body, quat, sizeof(s->s_quat_chip_to_body));
}

这个函数很简单、也很好理解,就是把上一步计算好的q_all[4],赋给s->s_quat_chip_to_body。s_quat_chip_to_body是struct inv_icm20948的成员,相关定义如下:

typedef struct inv_icm20948 {
	struct inv_icm20948_serif serif;
    ……
    /* data converter */
	long s_quat_chip_to_body[4];
    ……
} inv_icm20948_t;

s->s_quat_chip_to_body在前文书解析inv_icm20948_init_matrix函数的时候讲到过,相关代码如下:

和上边的q_adjust数组类似。

    //now rotate by angle, negate angle to rotate other way
    q_adjust[0] = (long)((1L<<30);
    q_adjust[1] = 0;
    q_adjust[2] = 0;
    q_adjust[3] = 0;

至此,inv_icm20948_set_chip_to_body_axis_quaternion函数就解析完了。inv_icm20948_init_matrix函数也就解析完了。

回到icm20948_sensor_setup函数中,当前完成了第2段代码的解析,

	/* Setup accel and gyro mounting matrix and associated angle for current board */
	inv_icm20948_init_matrix(&icm_device);

对于接下来步骤的解析请看下回。

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

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

相关文章

开放式激光振镜运动控制器在Ubuntu+Qt下的文本标刻

开放式激光振镜运动控制器在UbuntuQt下的文本标刻 上节课程我们讲述了如何通过UbuntuQt进行振镜校正&#xff08;详情点击→开放式激光振镜运动控制器在UbuntuQt下的激光振镜校正&#xff09;&#xff0c;本节文本标刻是在振镜校正的前提下实现的。 在正式学习之前&#xff0…

首个大模型供应链安全领域的国际标准,WDTA《大模型供应链安全要求》标准解读

9月6日,WDTA世界数字技术院在2024 外滩大会上正式发布了国际标准《大模型供应链安全要求》。这一标准为管理大型语言模型(LLM)供应链中的安全风险提出了系统性的框架,涵盖了LLM的整个生命周期,从开发、训练到部署和维护,为每个阶段提供了详尽的指导。 文末附标准获取方式 一…

re题(17)BUUCTF-[BJDCTF2020]JustRE

BUUCTF在线评测 (buuoj.cn) 放到ida&#xff0c;shiftF12可以直接看到有个类似flag的字符串&#xff0c;可以跳转过去 这里我们先不跳转&#xff0c;进入main&#xff08;&#xff09;的各个函数看一下 找到flag 本题学到一堆c的系统库函数

ImDisk Toolkit将一部分RAM模拟成硬盘分区

ImDisk Toolkit 是一个用于虚拟磁盘管理的工具&#xff0c;它可以将 RAM&#xff08;一部分内存&#xff09;模拟成硬盘分区&#xff0c;从而创建一个高速的临时存储空间&#xff0c;通常称为“RAM Disk”。以下是如何使用 ImDisk Toolkit 来将一部分 RAM 模拟成硬盘分区的步骤…

C++ | Leetcode C++题解之第398题随机数索引

题目&#xff1a; 题解&#xff1a; class Solution {vector<int> &nums; public:Solution(vector<int> &nums) : nums(nums) {}int pick(int target) {int ans;for (int i 0, cnt 0; i < nums.size(); i) {if (nums[i] target) {cnt; // 第 cnt 次…

Python OpenCV精讲系列 - 高级图像处理技术(三)

&#x1f496;&#x1f496;⚡️⚡️专栏&#xff1a;Python OpenCV精讲⚡️⚡️&#x1f496;&#x1f496; 本专栏聚焦于Python结合OpenCV库进行计算机视觉开发的专业教程。通过系统化的课程设计&#xff0c;从基础概念入手&#xff0c;逐步深入到图像处理、特征检测、物体识…

高效+灵活,万博智云全球发布AWS无代理跨云容灾方案!

摘要 近日&#xff0c;万博智云推出了基于AWS的无代理跨云容灾解决方案&#xff0c;并与拉丁美洲&#xff0c;中东&#xff0c;亚洲的合作伙伴面向全球开展了联合发布。这一方案以AWS应用环境为基础&#xff0c;将HyperBDR平台的高效、灵活和成本效益优势与无代理功能相结合&a…

HAProxy--高性能反向代理

文章目录 Web架构负载均衡介绍为什么使用负载均衡负载均衡类型 HAProxy简介应用场景HAProxy是什么HAProxy功能 脚本安装HAProxy基础配置global多进程和线程HAProxy日志配置项 Proxies配置-listen-frontend-backendserver配置 frontendbackend配置实例子配置文件 HAProxy调度算法…

SRS流媒体服务器从入门到精通(其一,环境搭建)

欢迎诸位来阅读在下的博文~ 在这里&#xff0c;在下会不定期发表一些浅薄的知识和经验&#xff0c;望诸位能与在下多多交流&#xff0c;共同努力! 江山如画&#xff0c;客心如若&#xff0c;欢迎到访&#xff0c;一展风采 文章目录 一、SRS简介二、SRS的应用场景三、环境搭建…

(计算机网络)应用层

1.为什么需要应用层 应用层提供使用tcp&#xff0c;udp使用的方式 协议就是制定的规则 2.域名服务器概述 域名是唯一的 新增域名&#xff0c;大家都要修改这个文本文件&#xff0c;所以要进行集中管理这个文本文件&#xff0c;而不是使用本地的hosts文件 hosts文件在Windows系统…

智能厕所系统高科技打造公厕新生态丨深圳讯鹏科技

从人们踏入智能厕所的那一刻起&#xff0c;便能深切感受到科技的力量。智能感应门悄然无声地开启&#xff0c;仿佛在欢迎每一位使用者。这种感应门不仅方便快捷&#xff0c;更避免了传统公厕门的直接接触&#xff0c;减少了细菌交叉感染的风险。走进厕所内部&#xff0c;智能灯…

Golang | Leetcode Golang题解之第397题整数替换

题目&#xff1a; 题解&#xff1a; func integerReplacement(n int) (ans int) {for n ! 1 {switch {case n%2 0:ansn / 2case n%4 1:ans 2n / 2case n 3:ans 2n 1default:ans 2n n/2 1}}return }

掌握Flux的各种风格表现,另外,flux也有了滑块Lora

在AIGC知识库通过#flux标签&#xff0c;汇总了下最近整理的内容&#xff0c;部分如下&#xff0c;更多可见AIGC知识库。 shadow&#xff1a; 经常在画图的时候&#xff0c;会有选择困难症&#xff0c;这下可以直接翻阅查看自己喜欢的风格&#xff0c;参考使用 ↓ Flux Style Te…

安装oh-my-zsh后报错zsh: command not found: conda问题解决

zsh: command not found: conda问题解决 一、问题介绍与环境介绍 系统为macOS Sonoma 14.5 所用终端为zsh 主要问题&#xff1a;安装了oh-my-zsh之后conda命令在终端中不可用。 二、原因分析 终端中zsh的可访问的程序一般放在/bin, /usr/bin, /usr/local/bin&#xff0c;~/bi…

快速了解高并发解决方案

对《高并发的哲学原理》的个人总结&#xff0c;原书地址如下 https://pphc.lvwenhan.com/ 本书的核心思想就是拆分&#xff0c;服务细化拆分多资源并行。 通用设计方法 例子&#xff1a;每秒100万次http请求 通过架构解决性能问题&#xff0c;在面对并发需求时&#xff…

【Python系列】JSON和JSONL简介

&#x1f49d;&#x1f49d;&#x1f49d;欢迎来到我的博客&#xff0c;很高兴能够在这里和您见面&#xff01;希望您在这里可以感受到一份轻松愉快的氛围&#xff0c;不仅可以获得有趣的内容和知识&#xff0c;也可以畅所欲言、分享您的想法和见解。 推荐:kwan 的首页,持续学…

动手学深度学习(一)简介+预备知识+基础知识(上)

一、简介 1、机器学习 机器学习研究如何使用经验改善计算机系统的性能。 2、表征学习 表征学习是机器学习的一类&#xff0c;研究的是&#xff0c;如何自动学习出数据合适的表示方式&#xff0c;更好地由输入得到正确的输出。 3、深度学习 深度学习是具有多级表示的表征学…

SpringCloud微服务详细笔记(一):微服务介绍--微服务拆分--RestTemplate远程调用--Nacos注册中心

目录 1.认识微服务 1.1单体架构 1.2微服务 1.3SpringCloud 2.微服务拆分 2.1服务拆分原则 2.1.1什么时候拆&#xff1f; 2.1.2怎么拆&#xff1f; 2.2微服务项目结构&#xff1a; 2.3服务拆分示例&#xff1a; 2.4远程调用 2.4.1RestTemplate 2.4.2远程调用示例 …

Qt篇——Qt使用C++获取Windows电脑上所有外接设备的名称、物理端口位置等信息

我之前有发过一篇文章《Qt篇——获取Windows系统上插入的串口设备的物理序号》&#xff0c;文章中主要获取的是插入的USB串口设备的物理序号&#xff1b;而本篇文章则进行拓展&#xff0c;可以获取所有外接设备的相关信息&#xff08;比如USB摄像头、USB蓝牙、USB网卡、其它一些…

前端转鸿蒙好做吗

在科技不断发展的当下&#xff0c;许多前端开发者可能会思考一个问题&#xff1a;前端转鸿蒙好做吗? 一、前端与鸿蒙开发的差异 1. 技术栈的变化 前端开发主要涉及 HTML、CSS、JavaScript 等技术&#xff0c;而鸿蒙开发则需要掌握 Java、Kotlin、JavaScript 等语言&#xff0c…