0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看技术视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

陀螺仪LSM6DSV16X与AI集成(3)----读取融合算法输出的四元数

嵌入式单片机MCU开发 来源:嵌入式单片机MCU开发 作者:嵌入式单片机MCU开 2023-12-18 10:53 次阅读

概述

LSM6DSV16X 特性涉及到的是一种低功耗的传感器融合算法(Sensor Fusion Low Power, SFLP).
低功耗传感器融合(SFLP)算法:
该算法旨在以节能的方式结合加速度计和陀螺仪的数据。传感器融合算法通过结合不同传感器的优势,提供更准确、可靠的数据。
6轴游戏旋转向量:
SFLP算法能够生成游戏旋转向量。这种向量是一种表示设备在空间中方向的数据,特别适用于游戏和增强现实应用,这些应用中理解设备的方向和运动非常关键。
四元数表示法:
旋转向量以四元数的形式表示。四元数是一种编码3D旋转的方法,它避免了欧拉角等其他表示法的一些限制(如万向节锁)。一个四元数有四个分量(X, Y, Z 和 W),其中 X, Y, Z 代表向量部分,W 代表标量部分。
FIFO存储:
四元数的 X, Y, Z 分量存储在 LSM6DSV16X 的 FIFO(先进先出)缓冲区中。FIFO 缓冲区是一种数据存储方式,允许临时存储传感器数据。这对于有效管理数据流非常有用,特别是在数据处理可能不如数据收集那么快的系统中。

最近在弄ST和瑞萨RA的课程,需要样片的可以加群申请:615061293 。
在这里插入图片描述

图片包含了关于 LSM6DSV16X 传感器的低功耗传感器融合(Sensor Fusion Low Power, SFLP)功能的说明。这里是对图片内容的解释:
SFLP 功能:

  1. SFLP 单元用于生成基于加速度计和陀螺仪数据处理的以下数据:
  2. 游戏旋转向量:以四元数形式表示设备的姿态。
  3. 重力向量:提供一个三维向量,表示重力方向。
  4. 陀螺仪偏差:提供一个三维向量,表示陀螺仪的偏差。
    激活与重置:
  5. 通过在 EMB_FUNC_EN_A(04h)嵌入式功能寄存器中设置 SFLP_GAME_EN 位为 1 来激活 SFLP 单元。
  6. 通过在 EMB_FUNC_INIT_A(66h)嵌入式功能寄存器中设置 SFLP_GAME_INIT 位为 1 来重置 SFLP 单元。
    性能参数表:
    表格展示了 SFLP 功能在不同情况下的性能,包括静态精度、低动态精度和高动态精度,以及校准时间和方向稳定时间。这些参数反映了传感器在不同运动状态下的精确度和响应速度。
    在这里插入图片描述

视频教学

https://www.bilibili.com/video/BV1Jw41187c5/

样品申请

https://www.wjx.top/vm/OhcKxJk.aspx#

完整代码下载

https://download.csdn.net/download/qq_24312945/88613104

使用demo板

最近在弄ST和瑞萨RA的课程,需要样片的可以加群申请:615061293 。
在这里插入图片描述

生成STM32CUBEMX

用STM32CUBEMX生成例程,这里使用MCU为STM32WB55RG。
配置时钟树,配置时钟为32M。

在这里插入图片描述

串口配置

查看原理图,PB6和PB7设置为开发板的串口。

在这里插入图片描述
配置串口。

在这里插入图片描述

IIC配置

在这里插入图片描述
配置IIC为快速模式,速度为400k。
在这里插入图片描述

CS和SA0设置

在这里插入图片描述

串口重定向

打开魔术棒,勾选MicroLIB

在这里插入图片描述

在main.c中,添加头文件,若不添加会出现 identifier "FILE" is undefined报错。

/* USER CODE BEGIN Includes */
#include "stdio.h"
/* USER CODE END Includes */

函数声明和串口重定向:

/* USER CODE BEGIN PFP */
int fputc(int ch, FILE *f){
	HAL_UART_Transmit(&huart1 , (uint8_t *)&ch, 1, 0xFFFF);
	return ch;
}
/* USER CODE END PFP */

参考程序

https://github.com/stm32duino/LSM6DSV16X/blob/main/examples/LSM6DSV16X_Sensor_Fusion/LSM6DSV16X_Sensor_Fusion.ino

初始化SFLP步骤

启用 LSM6DSV16X 传感器中的旋转向量低功耗传感器融合(Rotation Vector SFLP)功能的步骤。旋转向量是一个四元数,它提供了一个精确的设备姿态估计。这通常用于游戏控制、增强现实和虚拟现实等应用。下面是函数各部分的作用:

  1. 函数定义:LSM6DSV16XSensor_Enable_Rotation_Vector 旨在启用旋转向量功能,并返回操作的结果。如果成功,返回 0;如果出现错误,则返回错误代码。
  2. 设置满量程:函数首先设置加速度计和陀螺仪的满量程,这是传感器能够测量的最大范围。这里分别设置为 4g 和 2000 度每秒(dps)。
  3. 获取 FIFO SFLP 设置:然后,它读取当前的 FIFO SFLP(传感器融合低功耗)配置。
  4. 启用旋转向量 SFLP 特性:通过将 fifo_sflp.game_rotation 设为 1 来启用游戏旋转向量功能。
  5. 设置 FIFO 模式:将 FIFO 设置为流模式(也称为连续模式),在此模式下,数据持续地流入 FIFO,如果 FIFO 满了,新数据会覆盖旧数据。
  6. 设置数据输出率:为加速度计和陀螺仪以及 SFLP 设置数据输出率(ODR),在这里都设置为每秒 120 次采样(120Hz)。
  7. 启用 SFLP 低功耗模式:最后,启用 SFLP 游戏旋转向量特性,确保以低功耗模式运行。

初始化SFLP

开启嵌入式函数访问需要向 FUNC_CFG_ACCESS (01h)的EMB_FUNC_REG_ACCESS写入1进行开启。
在这里插入图片描述
在这里插入图片描述

/**
  * @brief  Change memory bank.[set]
  *
  * @param  ctx      read / write interface definitions
  * @param  val      MAIN_MEM_BANK, EMBED_FUNC_MEM_BANK,
  * @retval          interface status (MANDATORY: return 0 - > no Error)
  *
  */
int32_t lsm6dsv16x_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t val)
{
  lsm6dsv16x_func_cfg_access_t func_cfg_access;
  int32_t ret;

  ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1);
  if (ret != 0) { return ret; }

  func_cfg_access.shub_reg_access = ((uint8_t)val & 0x02U) > > 1;
  func_cfg_access.emb_func_reg_access = (uint8_t)val & 0x01U;
  ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1);

  return ret;
}

SFLP_GAME_FIFO_EN 是 LSM6DSV16X 传感器中 EMB_FUNC_FIFO_EN_A(44h)寄存器的一个设置位。这个特定的位用于控制是否启用将 SFLP(Sensor Fusion Low Power)算法计算出的游戏旋转向量(四元数)值存储到 FIFO(先进先出)缓冲区中的功能。当这个位被设置为 1 时,启用了这个功能,使得算法计算出的游戏旋转向量可以批量存储到 FIFO 缓冲区中。默认值为 0,表示该功能默认是禁用的。

在这里插入图片描述

在这里插入图片描述

LSM6DSV16X 传感器的 FIFO_CTRL4 (0Ah) 寄存器配置信息。这个寄存器控制着 FIFO(先进先出)缓冲区的各种操作和数据批处理(batching)的设置。
连续模式,如果 FIFO 已满,新采集的样本会覆盖旧样本。

在这里插入图片描述

在这里插入图片描述

在AN5763手册中,也说明了融合数据会输出在FIFO中,同时有如下的输出速率,我们可以配置默认的速率。
在这里插入图片描述
在这里插入图片描述

最后对EMB_FUNC_EN_A (04h) 寄存器的SFLP_GAME_EN设置为1。

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

读取四元数数据

FIFO_STATUS1(1Bh)和 FIFO_STATUS2(1Ch)寄存器中的 DIFF_FIFO [8:0] 字段包含在 FIFO 中收集的字(1 字节标签 + 6 字节数据)的数量。
在这里插入图片描述
在这里插入图片描述

/* Read watermark flag */
    status=lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);


  // Check the number of samples inside FIFO
  if (status != LSM6DSV16X_OK) {
    printf("LSM6DSV16X Sensor failed to get number of samples inside FIFO");
    while (1);
  }
	fifo_samples = fifo_status.fifo_level;

之后需要通过FIFO_DATA_OUT_TAG (78h)判断是什么数据准备好,当为SFLP game rotation vector(0X13)时候,为四元数准备完毕。

在这里插入图片描述
之后读取FIFO_DATA_OUT_X_L (79h)到FIFO_DATA_OUT_Z_H (7Eh)共6个字节数据,进行四元数读取。

在这里插入图片描述

最后转换为姿态角。

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	uint16_t fifo_samples;
	lsm6dsv16x_fifo_out_raw_t f_data;
    /* Read watermark flag */
    status=lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
  // Check the number of samples inside FIFO
  if (status != LSM6DSV16X_OK) {
    printf("LSM6DSV16X Sensor failed to get number of samples inside FIFO");
    while (1);
  }
	fifo_samples = fifo_status.fifo_level;

  // Read the FIFO if there is one stored sample
  if (fifo_samples > 0) {
		for (int i = 0; i < fifo_samples; i++) {
			
			lsm6dsv16x_fifo_out_raw_get(&dev_ctx,&f_data);
			if(f_data.tag==LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG){
				LSM6DSV16XSensor_FIFO_Get_Rotation_Vector(&dev_ctx,&quaternions[0]);
				// Print Quaternion data
//        printf("Quaternion: %.4lf,%.4lf,%.4lf,%.4lfn",quaternions[3],quaternions[0],quaternions[1],quaternions[2]);	
				q0=quaternions[3];
				q1=quaternions[0];
				q2=quaternions[1];
				q3=quaternions[2];
        Pitch  = asin(2 * q2 * q3 + 2 * q0* q1)* 57.3; // pitch ,转换为度数
        Roll = atan2(-2 * q1 * q3 + 2 * q0 * q2, q0*q0-q1*q1-q2*q2+q3*q3)* 57.3; // rollv
        Yaw = atan2(2*(q1*q2 - q0*q3),q0*q0-q1*q1+q2*q2-q3*q3) * 57.3;   //偏移太大,			
				Roll=Roll*100;
				Pitch=Pitch*100;
				Yaw=Yaw*100;
				data_angular_rate_raw[8]=(int16_t)Roll >>8;//roll
				data_angular_rate_raw[7]=(int16_t)Roll;
				data_angular_rate_raw[10]=(int16_t)Pitch >>8;//pitch
				data_angular_rate_raw[9]=(int16_t)Pitch;
				data_angular_rate_raw[12]=(int16_t)Yaw >>8;//yaw
				data_angular_rate_raw[11]=(int16_t)Yaw;
				sumcheck = 0;
				addcheck = 0;
				for(uint16_t i=0; i < 14; i++)
				{
				sumcheck += data_angular_rate_raw[i]; //从帧头开始,对每一字节进行求和,直到 DATA 区结束
				addcheck += sumcheck; //每一字节的求和操作,进行一次 sumcheck 的累加
				}
				data_angular_rate_raw[14]=sumcheck;
				data_angular_rate_raw[15]=addcheck;

				HAL_UART_Transmit(&huart1 , (uint8_t *)&data_angular_rate_raw, 16, 0xFFFF);	
//				printf("Roll=%.2f,Pitch=%.2f,Yaw=%.2fn",Roll,Pitch,Yaw);				
			}
		}
	}
      		HAL_Delay(10);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */

演示

在这里插入图片描述

审核编辑:汤梓红

声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • 传感器
    +关注

    关注

    2534

    文章

    48611

    浏览量

    742669
  • mems
    +关注

    关注

    128

    文章

    3769

    浏览量

    189292
  • 陀螺仪
    +关注

    关注

    44

    文章

    741

    浏览量

    97844
  • AI
    AI
    +关注

    关注

    87

    文章

    27176

    浏览量

    264916
收藏 人收藏

    评论

    相关推荐

    陀螺仪LSM6DSV16XAI集成(1)----轮询获取陀螺仪数据

    本文将介绍如何使用 LSM6DSV16X 传感器来读取数据。主要步骤包括初始化传感器接口、验证设备ID、配置传感器的数据输出率和滤波器,以及通过轮询方式持续读取加速度、角速率和温度数据
    的头像 发表于 12-18 10:49 824次阅读
    <b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6DSV16X</b>与<b class='flag-5'>AI</b><b class='flag-5'>集成</b>(1)----轮询获取<b class='flag-5'>陀螺仪</b>数据

    陀螺仪LSM6DSV16XAI集成(2)----姿态解算

    LSM6DSV16X包含三轴陀螺仪与三轴加速度计。
    的头像 发表于 12-18 10:51 894次阅读
    <b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6DSV16X</b>与<b class='flag-5'>AI</b><b class='flag-5'>集成</b>(2)----姿态解算

    【CANNON试用体验】6DOF姿态融合算法

    /jishu_550022_1_1.html(出处: 中国电子技术论坛)上一篇介绍了加速度计陀螺仪LSM6DS3这次介绍姿态融合算法在介绍算法时,先了解几个概念Yaw为航向角,表示机头
    发表于 02-29 21:31

    三轴陀螺仪和六轴陀螺仪的区别

    陀螺仪叫六轴动作感应器比较合适 是三轴陀螺仪和加速计的合称 如果有三轴陀螺仪也有加速计那就具有六轴动作感应  而九轴感测组件是:三轴加速度计、三轴陀螺仪、三轴磁强计,然后欧拉角加
    发表于 11-08 16:12

    ST LxG3IS系列全新陀螺仪

    3x3.5x1 mm封装  此外,为保证最高的可靠性,新的2轴(L2G3IS)和3轴(L3G3IS)陀螺仪可在谐振频率大约20kHz下工作
    发表于 11-13 16:03

    【uFun试用体验】测试Lis3dh陀螺仪

    也是一样的。图1图1是X轴寄存器的数据格式,前5位是符号位所以在进行数据使用的时候需要判断正负号//三轴陀螺仪原始数据void LIS3DH_Get_AccRaw(int16_t* p
    发表于 06-08 16:26

    姿态融合算法是什么

    作者:Joy Yang1.什么是姿态融合算法简单来说,姿态融合算法就是融合多种运动传感器数据(一般需要3轴加速度, 3
    发表于 07-19 06:47

    供应LSM6DSLTR LSM6DSL 传感器 LSM6DSMTR LSM6DSM

    DSM规格传感器类型:加速计,陀螺仪,温度,6输出类型:I²C,SPI工作温度:-40°C ~ 85°C(TA)封装/外壳:14-VFLGA 模块供应商器件封装:14-LGA(2.5x3
    发表于 07-15 11:46

    加速度计和陀螺仪的数学模型和基本算法是什么?如何进行融合

    加速度计和陀螺仪的数学模型和基本算法是什么?如何进行融合
    发表于 11-12 07:15

    什么是陀螺仪

    0是世界上第一款集成 6 轴MotionTracking设备。它集成3轴MEMS陀螺仪3轴M
    发表于 02-10 07:24

    请问LSM6DSV32XLSM6DSO32X是同一个封装吗?

    LSM6DSV32XLSM6DSO32X是同一个封装吗?
    发表于 12-14 07:19

    求助,是否有在LSM6DSV16X中使用传感器融合低功耗算法的示例

    你好我想知道是否有在 LSM6DSV16X 中使用传感器融合低功耗算法的示例。我想检索当前的 Heading、Pitch 和 Roll 信息。或者我也很高兴有一个例子解释如何从 SFLP 中获取
    发表于 02-03 09:12

    新品情报|STMicroelectronics全新LSM6DSV16BX运动+骨传导传感器

    。以下是本周新品情报,请及时查收: 运动/骨传导二合一 STMicroelectronics LSM6DSV16BX iNEMO三轴加速度计和陀螺仪 贸泽电子即日起
    的头像 发表于 05-17 10:35 815次阅读
    新品情报|STMicroelectronics全新<b class='flag-5'>LSM6DSV16</b>BX运动+骨传导传感器

    ST LSM6DSV16X iNEMO惯性模块相关的使用信息和应用提示

    AN5763,LSM6DSV16X是一款ST的3 轴加速度计和 3 轴陀螺仪LSM6DSV16X 是系统级封装的 3 轴数字加速度计和 3 轴数字陀螺仪,具有数字 I²C、SPI
    发表于 08-31 11:12 0次下载

    陀螺仪LSM6DSV16XAI集成(5)----6D方向检测功能

    陀螺仪通常可以读取三个方向上的旋转,即绕X轴、Y轴和Z轴的旋转。每个方向上的旋转包括正向旋转和反向旋转,因此一共有六个位置。这六个位置分别是:1.X轴正向旋转、2.X轴反向旋转、3.Y轴正向旋转
    的头像 发表于 01-09 16:14 482次阅读
    <b class='flag-5'>陀螺仪</b><b class='flag-5'>LSM6DSV16X</b>与<b class='flag-5'>AI</b><b class='flag-5'>集成</b>(5)----6D方向检测功能