ICM20948 DMP代码详解(12)

news2024/9/25 15:27:51

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

上一回开始解析icm20948_sensor_setup函数的第2段代码也即inv_icm20948_init_matrix函数:

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

讲到了inv_icm20948_init_matrix中最后调用的函数inv_icm20948_set_chip_to_body_axis_quaternion。

void inv_icm20948_init_matrix(struct inv_icm20948 *s)
{
	// initialize chip to body
	s->s_quat_chip_to_body[0] = (1L<<30);
	s->s_quat_chip_to_body[1] = 0;
	s->s_quat_chip_to_body[2] = 0;
	s->s_quat_chip_to_body[3] = 0;
	//initialize mounting matrix
	memset(s->mounting_matrix, 0, sizeof(s->mounting_matrix));
	s->mounting_matrix[0] = 1;
	s->mounting_matrix[4] = 1;
	s->mounting_matrix[8] = 1;
	//initialize soft iron matrix
	s->soft_iron_matrix[0] = (1L<<30);
	s->soft_iron_matrix[4] = (1L<<30);
	s->soft_iron_matrix[8] = (1L<<30);
 
	inv_icm20948_set_chip_to_body_axis_quaternion(s, s->mounting_matrix, 0.0);
}

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

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);
}

仍旧一段一段来看。先来看第1段代码。

    for (i=0; i<9; i++)
        rot[i] = (float)accel_gyro_matrix[i];

这段很好理解,就是依次将accel_gyro_matrix数组的每一个值赋给rot数组对应的项。

rot数组就是函数中定义的局部变量

    float rot[9];

accel_gyro_matrix是函数的第2个参数signed char *accel_gyro_matrix,对应的实参为inv_icm20948_init_matrix函数中的s->mounting_matrix([9])。

接下来是第2段代码:

    //convert Chip to Body transformation matrix to quaternion
    //inv_icm20948_convert_matrix_to_quat_fxp(rot, qcb);
	inv_rotation_to_quaternion(rot, qcb);

根据注释,这段代码的意思是将Chip to Body变换矩阵转换为四元数。

inv_rotation_to_quaternion函数在同文件(EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.c)中,代码如下:

static void inv_rotation_to_quaternion(float Rcb[9], long Qcb_fp[4])
{	
	float q[4]; 
	inv_icm20948_convert_matrix_to_quat_flt(Rcb, q); 
	INVN_CONVERT_FLT_TO_FXP(q, Qcb_fp, 4, 30); 
}

inv_rotation_to_quaternion函数的两个参数的实参分别对应于inv_icm20948_set_chip_to_body_axis_quaternion函数中的float rot[9]和long qcb[4]。

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

void inv_icm20948_convert_matrix_to_quat_flt(float *R, float *q)
{
	float r11,r12,r13, r21,r22,r23, r31,r32,r33;

	r11 = R[0]; //assume matrix is stored row wise first, that is rot[1] is row 1, col 2
	r12 = R[1];
	r13 = R[2];

	r21 = R[3];
	r22 = R[4];
	r23 = R[5];

	r31 = R[6];
	r32 = R[7];
	r33 = R[8];

	q[0] = (1.f + r11 + r22 + r33) / 4.f;
	q[1] = (1.f + r11 - r22 - r33) / 4.f;
	q[2] = (1.f - r11 + r22 - r33) / 4.f;
	q[3] = (1.f - r11 - r22 + r33) / 4.f;

	if(q[0] < 0.0f) q[0] = 0.0f;
	if(q[1] < 0.0f) q[1] = 0.0f;
	if(q[2] < 0.0f) q[2] = 0.0f;
	if(q[3] < 0.0f) q[3] = 0.0f;
	q[0] = sqrtf(q[0]);
	q[1] = sqrtf(q[1]);
	q[2] = sqrtf(q[2]);
	q[3] = sqrtf(q[3]);

	/* Above paragraph could be reduced in :
	q[0] =(q[0] < 0.0f) ? q[0] = 0.0f : sqrtf(q[0]);
	q[1] =(q[1] < 0.0f) ? q[1] = 0.0f : sqrtf(q[1]);
	q[2] =(q[2] < 0.0f) ? q[2] = 0.0f : sqrtf(q[2]);
	q[3] =(q[3] < 0.0f) ? q[3] = 0.0f : sqrtf(q[3]);
	*/
	
	if(q[0] >= q[1] && q[0] >= q[2] && q[0] >= q[3]) //q[0] is max
	{
		 q[1] = (r23 - r32)/(4.f*q[0]);
		 q[2] = (r31 - r13)/(4.f*q[0]);
		 q[3] = (r12 - r21)/(4.f*q[0]);
	}
	else if(q[1] >= q[0] && q[1] >= q[2] && q[1] >= q[3]) //q[1] is max
	{
		 q[0] = (r23 - r32)/(4.f*q[1]);
		 q[2] = (r12 + r21)/(4.f*q[1]);
		 q[3] = (r31 + r13)/(4.f*q[1]);
	}
	else if(q[2] >= q[0] && q[2] >= q[1] && q[2] >= q[3]) //q[2] is max
	{
		 q[0] = (r31 - r13)/(4.f*q[2]);
		 q[1] = (r12 + r21)/(4.f*q[2]);
		 q[3] = (r23 + r32)/(4.f*q[2]);
	}
	else if(q[3] >= q[0] && q[3] >= q[1] && q[3] >= q[2]) //q[3] is max
	{
		 q[0] = (r12 - r21)/(4.f*q[3]);
		 q[1] = (r31 + r13)/(4.f*q[3]);
		 q[2] = (r23 + r32)/(4.f*q[3]);
	}
}

看似代码较长较复杂,其实并不难理解。

1)先把R[0]~R[8](即rot[0]~rot[8])由行向量转换为矩阵(r11、r12、……,r33 )。代码片段如下:

	float r11,r12,r13, r21,r22,r23, r31,r32,r33;
	
    r11 = R[0]; //assume matrix is stored row wise first, that is rot[1] is row 1, col 2
	r12 = R[1];
	r13 = R[2];

	r21 = R[3];
	r22 = R[4];
	r23 = R[5];

	r31 = R[6];
	r32 = R[7];
	r33 = R[8];

2)然后依次计算得到q[0]~q[3]的值,并作一定修正。代码片段如下:

	q[0] = (1.f + r11 + r22 + r33) / 4.f;
	q[1] = (1.f + r11 - r22 - r33) / 4.f;
	q[2] = (1.f - r11 + r22 - r33) / 4.f;
	q[3] = (1.f - r11 - r22 + r33) / 4.f;

	if(q[0] < 0.0f) q[0] = 0.0f;
	if(q[1] < 0.0f) q[1] = 0.0f;
	if(q[2] < 0.0f) q[2] = 0.0f;
	if(q[3] < 0.0f) q[3] = 0.0f;

3)依次对于q[0]~q[3]做开方运算。代码片段如下:

	q[0] = sqrtf(q[0]);
	q[1] = sqrtf(q[1]);
	q[2] = sqrtf(q[2]);
	q[3] = sqrtf(q[3]);

4)根据最大值具体出自于q[0]/q[1]/q[2]/q[3],依次做不同运算。代码片段如下:

	if(q[0] >= q[1] && q[0] >= q[2] && q[0] >= q[3]) //q[0] is max
	{
		 q[1] = (r23 - r32)/(4.f*q[0]);
		 q[2] = (r31 - r13)/(4.f*q[0]);
		 q[3] = (r12 - r21)/(4.f*q[0]);
	}
	else if(q[1] >= q[0] && q[1] >= q[2] && q[1] >= q[3]) //q[1] is max
	{
		 q[0] = (r23 - r32)/(4.f*q[1]);
		 q[2] = (r12 + r21)/(4.f*q[1]);
		 q[3] = (r31 + r13)/(4.f*q[1]);
	}
	else if(q[2] >= q[0] && q[2] >= q[1] && q[2] >= q[3]) //q[2] is max
	{
		 q[0] = (r31 - r13)/(4.f*q[2]);
		 q[1] = (r12 + r21)/(4.f*q[2]);
		 q[3] = (r23 + r32)/(4.f*q[2]);
	}
	else if(q[3] >= q[0] && q[3] >= q[1] && q[3] >= q[2]) //q[3] is max
	{
		 q[0] = (r12 - r21)/(4.f*q[3]);
		 q[1] = (r31 + r13)/(4.f*q[3]);
		 q[2] = (r23 + r32)/(4.f*q[3]);
	}

inv_icm20948_convert_matrix_to_quat_flt函数代码本身很好理解,但是为什么这样计算?公式从何而来?参考以下文章:

‌四元数的基本定义和性质‌‌12

四元数是一种扩展的复数,由一个实数部分和三个虚数部分组成。每个四元数可以表示为a + ‌bi + cj + dk,其中a、b、c、d是实数,i、j、k是虚数单位,满足i² = j² = k² = -1。

四元数与‌旋转矩阵的关系

旋转矩阵和四元数之间存在着一种对应关系,可以通过这种关系将旋转矩阵转换成四元数。具体来说,一个旋转矩阵可以表示为一个3x3的矩阵,而一个四元数可以表示为一个包含四个分量的向量。

具体的转换公式和方法

从旋转矩阵转换到四元数的公式如下:

  1. 首先计算w的值:
    w = sqrt(1 + r00 + r11 + r22) / 2
    其中r00、r11、r22是旋转矩阵的对角线元素。
  2. 然后计算x、y、z的值:
    x = (r21 - r12) / (4w)
    y = (r02 - r20) / (4w)
    z = (r10 - r01) / (4w)
    需要注意的是,由于四元数具有多个等价的表示方式,上述公式中的分母部分使用了4w来确保单位化。

实际应用和注意事项

在实际应用中,旋转矩阵转四元数的转换在计算机图形学、机器人学等领域有着广泛的应用。例如,在计算机图形学中,四元数常用于表示三维空间的旋转,以避免万向锁问题。需要注意的是,由于四元数有多种表示方式,转换过程中需要注意选择合适的表示方式,并确保转换后的四元数是单位化的。

回到inv_rotation_to_quaternion函数中。在调用inv_icm20948_convert_matrix_to_quat_flt函数之后,接下来调用INVN_CONVERT_FLT_TO_FXP函数,代码片段如下:

static void inv_rotation_to_quaternion(float Rcb[9], long Qcb_fp[4])
{	
	float q[4]; 
	inv_icm20948_convert_matrix_to_quat_flt(Rcb, q); 
	INVN_CONVERT_FLT_TO_FXP(q, Qcb_fp, 4, 30); 
}

INVN_CONVERT_FLT_TO_FXP实际上是一个宏,在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.h中,定义如下:

//! \def INVN_FLT_TO_FXP
//! Convert the \a value from float to QN value. \ingroup invn_macro 
#define INVN_FLT_TO_FXP(value, shift)	( (int32_t)  ((float)(value)*(1ULL << (shift)) + ( (value>=0)-0.5f )) ) 
//!	Macro to convert float values from an address into QN values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_FLT_TO_FXP(fltptr, fixptr, length, shift)	{ int i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_FLT_TO_FXP((fltptr)[i], shift); }

将以上代码转换成容易理解的函数方式,如下:

//! \def INVN_FLT_TO_FXP
//! Convert the \a value from float to QN value. \ingroup invn_macro 
#define INVN_FLT_TO_FXP(value, shift)
{
    (int32_t) ((float)(value)*(1ULL << (shift)) + ( (value>=0)-0.5f ))
} 

//!	Macro to convert float values from an address into QN values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_FLT_TO_FXP(fltptr, fixptr, length, shift)
{
    int i;

    for (i=0; i<(length); ++i)
        (fixptr)[i] = INVN_FLT_TO_FXP((fltptr)[i], shift);
}

这一部分代码应该是浮点数转定点数,参考:定点转浮点的Qn定义及计算公式方法。

参考:https://www.zhihu.com/question/50534472/answer/2144622129。

这里,shift就是公式中的n。

最终,将计算得到的Qcb_fp形参的值返给inv_icm20948_set_chip_to_body_axis_quaternion函数的调用处,也就是生成了qcb的值。

    long qcb[4];

    //convert Chip to Body transformation matrix to quaternion
    //inv_icm20948_convert_matrix_to_quat_fxp(rot, qcb);
	inv_rotation_to_quaternion(rot, qcb);

综合以上内容,inv_rotation_to_quaternion函数的功能是:

1)先将旋转矩阵(数组rot[9])转换成浮点数形式的四元数;

2)再将浮点数形式的四元数转换为整数形式的四元数。

static void inv_rotation_to_quaternion(float Rcb[9], long Qcb_fp[4])
{	
	float q[4]; 
	inv_icm20948_convert_matrix_to_quat_flt(Rcb, q); 
	INVN_CONVERT_FLT_TO_FXP(q, Qcb_fp, 4, 30); 
}

回到inv_icm20948_set_chip_to_body_axis_quaternion函数中。对于该函数接下来代码的解析,请看下回。

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

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

相关文章

前端技术(七)——less 教程

一、less简介 1. less是什么&#xff1f; less是一种动态样式语言&#xff0c;属于css预处理器的范畴&#xff0c;它扩展了CSS语言&#xff0c;增加了变量、Mixin、函数等特性&#xff0c;使CSS 更易维护和扩展LESS 既可以在 客户端 上运行 &#xff0c;也可以借助Node.js在服…

关于武汉芯景科技有限公司的IIC缓冲器芯片XJ4307开发指南(兼容LTC4307)

一、芯片引脚介绍 1.芯片引脚 2.引脚描述 二、系统结构图 三、功能描述 1.总线超时&#xff0c;自动断开连接 当 SDAOUT 或 SCLOUT 为低电平时&#xff0c;将启动内部定时器。定时器仅在相应输入变为高电平时重置。如果在 30ms &#xff08;典型值&#xff09; 内没有变为高…

社交媒体的未来:Facebook如何通过AI技术引领潮流

在数字化时代的浪潮中&#xff0c;社交媒体平台不断演变&#xff0c;以适应用户需求和技术发展的变化。作为全球领先的社交媒体平台&#xff0c;Facebook在这一进程中扮演了重要角色。尤其是人工智能&#xff08;AI&#xff09;技术的应用&#xff0c;正在深刻地改变Facebook的…

Docker零基础入门

参考课程https://www.bilibili.com/video/BV1VC4y177re/?vd_source=b15169a302bee35f484245aecc69d4dd 参考书籍Docker 实践 - 面向 AI 开发人员的 Docker 实践 (dockerpractice.readthedocs.io) 1. 什么是Docker 1.1. Docker起源 随着计算机的发展,计算机上已经可以运行多…

Stable Diffusion绘画 | ControlNet应用-Inpaint(局部重绘):更完美的重绘

Inpaint(局部重绘) 相当于小号的AI版PS&#xff0c;不但可以进行局部画面的修改&#xff0c;还可以去除背景中多余的内容&#xff0c;或者是四周画面内容的扩充。 预处理器说明 Inpaint_Global_Harmonious&#xff1a;重绘-全局融合算法&#xff0c;会对整个图片的画面和色调…

【无标题】SAM(Segment Anything Model)

1.SAM是什么&#xff1f; SAM是基于NLP的一个基础模型&#xff0c;专注于提示分割任务&#xff0c;使用提升工程来适应不同的下游分割任务。 2.SAM有什么用&#xff1f; 1&#xff09;SAM 可以通过简单地单击或交互选择要包含或排除在对象中的点来分割对象。还可以通过使用多边…

成都爱尔综合眼病科李晓峰主任解析空调续命,干眼别忍!

高温酷暑&#xff0c;命都是空调给的。 凉风一直吹&#xff0c;根本不敢停。 热到大汗淋漓&#xff0c;身体缺水&#xff0c;眼睛也是。 屋外闷热湿度不低&#xff0c;屋内空调一开湿度“骤降”不够用。 房间被“除湿”&#xff0c;眼睛也不例外。 长时间吹空调&#xff0c…

基于C++实现(控制台)模拟网上购书订单管理系统

模拟网上购书订单管理系统&#xff08;大一小学期C大作业&#xff09; 一、任务 1. 基础任务 建立继承了Buyer类的三个子类作为顾客的三种类型&#xff0c;用于管理顾客对象&#xff1b;建立Book类&#xff0c;管理书本对象&#xff1b;根据不同类型的顾客&#xff0c;计算出…

全球主要指数年度收益率汇总

1 美国 1.1 道琼斯工业平均指数 DJIA 1.2 纳斯达克综合指数 IXIC 1.3 纳斯达克100指数 NDX 1.4 标准普尔500 INX 2 中国 2.1 国债指数 000021 2.2 上证综指 000001 2.3 深证成指 399001 2.4 创业板 399006 2.5 中小100 399005 2.6 上证50 000016 3 香港

智能可视耳勺怎么用?智能可视耳勺使用方法!

随着科技的进步&#xff0c;有很多人摒弃了传统挖耳勺&#xff0c;选择更加高效直观的智能可视耳勺&#xff0c;这是因为智能可视耳勺能更加直观地看到耳朵的内部&#xff0c;让掏耳过程清晰明了&#xff0c;精准掏出耳垢。 但市场有的智能可视耳勺鱼龙混杂&#xff0c;很多人在…

【解决】vue 弹窗后面页面可以滚动问题

做web端项目过程中&#xff0c;发现点击弹窗后&#xff0c;弹窗后面的页面还可以滚动。 复现如下&#xff1a; 【方法1】 step1&#xff1a;在弹框页面使用 mousewheel.prevent <divv-show"workShowMenu"mousewheel.prevent>// TO DO...弹框内容 </div&…

C盘清理 拯救你的C盘!C盘从此不再爆满~!

C盘清理&#xff0c;拯救你的C盘&#xff01;C盘从此不再爆满~&#xff01;C盘爆满是许多人经常遇到的问题&#xff0c;它可能导致系统运行缓慢甚至崩溃&#xff0c;对于这种情况&#xff0c;我们要从根源触发&#xff0c;彻底的清理干净C盘垃圾。 一般的C盘清理有下面几种方法…

AI跟踪报道第55期-新加坡内哥谈技术-本周AI新闻: GPT NEXT (x100倍)即将在2024推出

每周跟踪AI热点新闻动向和震撼发展 想要探索生成式人工智能的前沿进展吗&#xff1f;订阅我们的简报&#xff0c;深入解析最新的技术突破、实际应用案例和未来的趋势。与全球数同行一同&#xff0c;从行业内部的深度分析和实用指南中受益。不要错过这个机会&#xff0c;成为AI领…

[概率论] 随机变量的分布函数 (一)

文章目录 1.随机变量的分布函数2.离散型随机变量的分布函数3.连续性随机变量的分布函数 1.随机变量的分布函数 设X XX是一个随机变量&#xff0c;x xx是任意实数&#xff0c;则函数 几何表示 性质&#xff08;一个函数是分布函数的充要条件&#xff09; 2.离散型随机变量的分布…

区块链--代币之外的应用

数字货币是区块链技术的首次应用&#xff0c;但这可以说并没有真正发挥其潜力。比特币的发明首次引入了区块链的概念&#xff0c;但是直到 2013 年&#xff0c;区块链技术的真正潜力才得以展现&#xff0c;并在除加密货币之外的许多不同行业中得到应用。从那时起&#xff0c;人…

《机器学习》—— SVD奇异值分解方法对图像进行压缩

文章目录 一、SVD奇异值分解简单介绍二、代码实现—SVD奇异值分解方法对图像进行压缩 一、SVD奇异值分解简单介绍 SVD&#xff08;奇异值分解&#xff09;是一种在信号处理、统计学、线性代数、机器学习等多个领域广泛应用的矩阵分解方法。它将任何 mn 矩阵 A 分解为三个特定矩…

软考基础知识之性能指标

目录 前言 性能指标 计算机 1、时钟频率&#xff08;主频&#xff09; 2 、高速缓存 3、运算速度 4、运算精度 5、内存的存储容量 6、存储器的存取周期 7、数据处理速率 8、响应时间 9、RASIS 特性 10、平均故障响应时间 11、兼容性 网络 1、设备级性能指标 2、…

18067 字符统计

### 思路 1. **初始化计数器**&#xff1a;初始化字母计数器nL和数字计数器nN为0。 2. **遍历输入字符串**&#xff1a;逐个字符检查。 3. **判断字符类型**&#xff1a; - 如果是字母&#xff0c;增加nL。 - 如果是数字&#xff0c;增加nN。 - 如果是空格&#xff0c…

OpenAI 计划推出最高每月 2000 美元的 ChatGPT 订阅服务|TodayAI

OpenAI 正在计划推出更高价的 ChatGPT 订阅服务&#xff0c;以满足日益增长的市场需求。据《The Information》报道&#xff0c;OpenAI 已经在内部讨论了高级订阅的价格&#xff0c;最高可能达到每月 2000 美元。这些高级订阅将提供目前正在开发的高性能 AI 模型中的高级功能&a…

快充协议工作原理 XSP04快充协议芯片的简绍

快充协议‌是一种通过提高充电效率来缩短设备充电时间的电池充电技术。它是通过在充电器和设备之间建立一种沟通机制&#xff0c;使得充电器能够根据设备的需求和状态&#xff0c;调整输出的电压和电流。这种沟通机制由快充协议定义&#xff0c;它决定了设备和充电器如何互相识…