陀螺仪LSM6DSOW开发(5)----MotionFX库解析空间坐标

已于 2024-07-31 19:19:36 修改
阅读量1w 收藏 20
点赞数 15
分类专栏： stm32cube 传感器 单片机 文章标签： 欧拉角 MotionFX STM32H503CB X-CUBE-MEMS1 LSM6DSOW 四元数
于 2024-07-31 01:08:05 首次发布
本文链接：https://blog.csdn.net/qq_24312945/article/details/140810349
版权
单片机 同时被 3 个专栏收录
282 篇文章 167 订阅
订阅专栏
stm32cube
162 篇文章 63 订阅
订阅专栏
传感器
112 篇文章 5 订阅
订阅专栏

陀螺仪LSM6DSOW开发.5--MotionFX库解析空间坐标

概述

本文将探讨如何使用MotionFX库解析空间坐标。MotionFX库是一种用于传感器融合的强大工具，可以将加速度计、陀螺仪和磁力计的数据融合在一起，实现精确的姿态和位置估计。本文将介绍如何初始化和配置MotionFX库，使用FIFO读取传感器数据，FIFO可以作为数据缓冲区，存储传感器的临时数据。这样可以防止数据丢失，特别是在处理器忙于其他任务时，并利用这些数据进行空间坐标的解析。本章案例使用上节的demo进行修改。

最近在弄ST和瑞萨RA的课程，需要样片的可以加群申请：615061293 。

在这里插入图片描述

视频教学

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

陀螺仪LSM6DSOW开发(5)----MotionFX库解析空间坐标

样品申请

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

源码下载

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

硬件准备

首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。
主控为STM32H503CB，陀螺仪为LSM6DSOW，磁力计为LIS2MDL。

在这里插入图片描述

开启CRC

在这里插入图片描述

串口设置

设置串口速率为2000000。
在这里插入图片描述

开启X-CUBE-MEMS1

在这里插入图片描述

速率选择

加速度和角速度的速率尽量大于100Hz。
在这里插入图片描述

XL_HM_MODE 和 G_HM_MODE 是LSM6DSOW传感器中的配置位，用于设置加速度计和陀螺仪的高性能模式或低功耗模式。这些模式可以根据应用需求选择，以优化功耗或提高性能。
在这里插入图片描述

XL_HM_MODE 位于 CTRL6_C (15h) 寄存器中，用于控制加速度计的高性能模式。在高性能模式下，加速度计提供更高的采样率和精度，但功耗也会增加。
XL_HM_MODE 位描述
● 位位置: 4
● 默认值: 0（高性能模式启用）
● 描述: 控制加速度计的高性能模式。
○ 0: 启用高性能操作模式（默认）。
○ 1: 禁用高性能操作模式，启用低功耗模式。
在这里插入图片描述

G_HM_MODE 位于 CTRL7_G (16h) 寄存器中，用于控制陀螺仪的高性能模式。在高性能模式下，陀螺仪提供更高的采样率和精度，但功耗也会增加。
G_HM_MODE 位描述
● 位位置: 7
● 默认值: 0（高性能模式启用）
● 描述: 控制陀螺仪的高性能模式。
○ 0: 启用高性能操作模式（默认）。
○ 1: 禁用高性能操作模式，启用低功耗模式。

在这里插入图片描述

添加到初始化位置。

	lsm6dso_xl_power_mode_set(&dev_ctx, LSM6DSO_HIGH_PERFORMANCE_MD);
	lsm6dso_gy_power_mode_set(&dev_ctx, LSM6DSO_GY_HIGH_PERFORMANCE);

在这里插入图片描述

设置FIFO触发阈值为30.

  lsm6dso_fifo_watermark_set(&dev_ctx, 30);

修改定义FIFO存储数组的长度为阈值的2倍。

uint8_t fifo_data[30*2][7];

初始化定义

  /* USER CODE BEGIN 2 */
	printf("HELLO!\n");
  HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET);
	HAL_Delay(100);
	
	

  /* Uncomment to configure INT 1 */
  lsm6dso_pin_int1_route_t int1_route;	
  /* Initialize mems driver interface */
  dev_ctx.write_reg = platform_write;
  dev_ctx.read_reg = platform_read;
  dev_ctx.mdelay = platform_delay;
  dev_ctx.handle = &SENSOR_BUS;
  /* Init test platform */
//  platform_init();
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);
  /* Check device ID */
  lsm6dso_device_id_get(&dev_ctx, &whoamI);
	printf("LSM6DSO_ID=0x%x,whoamI=0x%x",LSM6DSO_ID,whoamI);
  if (whoamI != LSM6DSO_ID)
    while (1);

  /* Restore default configuration */
  lsm6dso_reset_set(&dev_ctx, PROPERTY_ENABLE);

  do {
    lsm6dso_reset_get(&dev_ctx, &rst);
  } while (rst);

  /* Disable I3C interface */
  lsm6dso_i3c_disable_set(&dev_ctx, LSM6DSO_I3C_DISABLE);
  /* Enable Block Data Update */
  lsm6dso_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
  /* Set full scale */
  lsm6dso_xl_full_scale_set(&dev_ctx, LSM6DSO_2g);
  lsm6dso_gy_full_scale_set(&dev_ctx, LSM6DSO_2000dps);
  /* Set FIFO watermark (number of unread sensor data TAG + 6 bytes
   * stored in FIFO) to 10 samples
   */
  lsm6dso_fifo_watermark_set(&dev_ctx, 30);
  /* Set FIFO batch XL/Gyro ODR to 12.5Hz */
  lsm6dso_fifo_xl_batch_set(&dev_ctx, LSM6DSO_XL_BATCHED_AT_417Hz);
  lsm6dso_fifo_gy_batch_set(&dev_ctx, LSM6DSO_GY_BATCHED_AT_417Hz);
  /* Set FIFO mode to Stream mode (aka Continuous Mode) */
  lsm6dso_fifo_mode_set(&dev_ctx, LSM6DSO_STREAM_MODE);
  /* Enable drdy 75 μs pulse: uncomment if interrupt must be pulsed */
  lsm6dso_data_ready_mode_set(&dev_ctx, LSM6DSO_DRDY_PULSED);
  /* Uncomment if interrupt generation on Free Fall INT1 pin */
  lsm6dso_pin_int1_route_get(&dev_ctx, &int1_route);
  int1_route.fifo_th = PROPERTY_ENABLE;
  lsm6dso_pin_int1_route_set(&dev_ctx, int1_route);
  /* Uncomment if interrupt generation on Free Fall INT2 pin */
  //lsm6dso_pin_int2_route_get(&dev_ctx, &int2_route);
  //int2_route.reg.int2_ctrl.int2_fifo_th = PROPERTY_ENABLE;
  //lsm6dso_pin_int2_route_set(&dev_ctx, &int2_route);
  /* Set Output Data Rate */
  lsm6dso_xl_data_rate_set(&dev_ctx, LSM6DSO_XL_ODR_417Hz);
  lsm6dso_gy_data_rate_set(&dev_ctx, LSM6DSO_GY_ODR_417Hz);



	lsm6dso_fifo_timestamp_decimation_set(&dev_ctx, LSM6DSO_DEC_1);
  /* Enable timestamp */
  lsm6dso_timestamp_set(&dev_ctx, PROPERTY_ENABLE);

	lsm6dso_xl_power_mode_set(&dev_ctx, LSM6DSO_HIGH_PERFORMANCE_MD);
	lsm6dso_gy_power_mode_set(&dev_ctx, LSM6DSO_GY_HIGH_PERFORMANCE);
	
	
	lsm6dso_init();
  /* USER CODE END 2 */

MotionFX文件

主要包含lsm6dso_app.c和lsm6dso_app.h，这两个文件主要负责初始化和管理LSM6DSOW传感器的交互。它们提供了配置传感器、初始化通信接口以及读取传感器数据的功能。
该文件包含与LSM6DSOW传感器交互所需函数的实现。它提供了配置传感器、初始化通信接口以及读取传感器数据的功能。
lsm6dso_init(): 初始化MotionFX算法。
lsm6dso_motion_fx_determin(): 该函数主要用于读取传感器数据并使用MotionFX库进行数据融合处理。

卡尔曼滤波算法

运行卡尔曼滤波传播算法MotionFX_propagate。
根据需要更新卡尔曼滤波器MotionFX_update。
需要注意的是这2各算法非常吃资源，需要注意MCU算力分配。

在这里插入图片描述

函数结构如下所示。

在这里插入图片描述
对应的demo在2.2.9有提供。

在这里插入图片描述

主程序执行流程

  1. 读取FIFO水印标志：
    ○ 使用 中断以及lsm6dso_fifo_wtm_flag_get() 函数读取FIFO水印标志，判断FIFO中的数据是否达到设定的阈值。
  2. 处理FIFO数据：
    ○ 如果FIFO水印标志被设置，读取FIFO中的数据数量。
    ○ 使用 lsm6dso_fifo_out_raw_get() 函数逐项读取FIFO中的传感器数据。
  3. 调用姿态估计算法：
    ○ 当加速度计、陀螺仪和时间戳数据都已读取时，调用 lsm6dso_motion_fx_determin() 函数进行姿态估计。
    ○ 重置标志位并更新时间戳。
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
	if(fifo_flag)// 如果 FIFO 中断标志被设置
	{		
		uint8_t acc_flag=0,gyr_flag=0;//加速度角速度标志位
		uint8_t deltatime_flag=0;//时间标志位		
//		printf("fifo_num=%d\n",fifo_num);
		for(int i=0;i<fifo_num;i++)// 遍历 FIFO 数据数组
		{
			// 获取数据指针
			datax = (int16_t *)&fifo_data[i][1];
			datay = (int16_t *)&fifo_data[i][3];
			dataz = (int16_t *)&fifo_data[i][5];
				
			// 根据数据标签处理不同类型的数据
			switch (fifo_data[i][0]) {
        case LSM6DSO_XL_NC_TAG:// 加速度数据
					acc_flag=1;				
					acc_x=lsm6dso_from_fs2_to_mg(*datax);
					acc_y=lsm6dso_from_fs2_to_mg(*datay);
					acc_z=lsm6dso_from_fs2_to_mg(*dataz);		
//					printf("Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
//                    acc_x, acc_y, acc_z);				
          break;				
        case LSM6DSO_GYRO_NC_TAG:// 角速度数据
					gyr_flag=1;
					gyr_x=lsm6dso_from_fs2000_to_mdps(*datax);
					gyr_y=lsm6dso_from_fs2000_to_mdps(*datay);
					gyr_z=lsm6dso_from_fs2000_to_mdps(*dataz);	
//					printf("Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
//									gyr_x,gyr_y,gyr_z);			
          break;				
        case LSM6DSO_TIMESTAMP_TAG:// 时间戳数据
					deltatime_flag=1;
			 /* 读取时间戳数据 */
				uint32_t timestamp=0;
						timestamp+= fifo_data[i][1];
						timestamp+= fifo_data[i][2]<<8;					
						timestamp+= fifo_data[i][3]<<16;						
						timestamp+= fifo_data[i][4]<<24;	
					if(deltatime_first==0)//第一次
					{
						deltatime_1=timestamp;
						deltatime_2=deltatime_1;
						deltatime_first=1;
					}
					else
					{
						deltatime_2=timestamp;
					}
//					printf("timestamp=%d\r\n",timestamp);							
          break;	
        default:
          break;				
				}
			// 如果加速度、角速度和时间戳数据都已获取
			if(acc_flag&&gyr_flag&&deltatime_flag)
			{
				lsm6dso_motion_fx_determin();// 调用 MotionFX 处理函数
				acc_flag=0;
				gyr_flag=0;
				deltatime_flag=0;
				deltatime_1=deltatime_2;	// 更新时间戳		
			}						
		}

		// 清除 FIFO 标志和数据量
		fifo_flag=0;
		fifo_num=0;
		
		}

		
    /* USER CODE END WHILE */

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

欧拉角简介

欧拉角（Euler Angles）是一种表示三维旋转的方式，通过三个角度来描述物体在三维空间中的姿态。这三个角度通常称为滚转角（Roll）、俯仰角（Pitch）和偏航角（Yaw），它们分别表示绕物体的自身坐标系的三个轴的旋转。

横滚roll，俯仰pitch，偏航yaw的实际含义如下图：

在这里插入图片描述

● 优点
表示简单直观，易于理解。
适用于描述固定顺序的旋转操作。
● 缺点
存在万向节死锁问题（Gimbal Lock），即当俯仰角接近±90度时，会失去一个自由度，导致系统无法确定物体的姿态。
旋转顺序不同会导致不同的最终姿态，需要特别注意旋转顺序。

演示

初始位置和数据输出如下所示。
在这里插入图片描述

在这里插入图片描述

逆时针旋转90°
在这里插入图片描述

在这里插入图片描述
逆时针旋转180°
在这里插入图片描述

在这里插入图片描述

逆时针旋转270°

在这里插入图片描述

在这里插入图片描述

陀螺仪LSM6DSOW开发(5)-MotionFX库解析空间坐标
07-31
本文将探讨如何使用MotionFX库解析空间坐标。MotionFX库是一种用于传感器融合的强大工具，可以将加速度计、陀螺仪和磁力计的数据融合在一起，实现精确的姿态和位置估计。本文将介绍如何初始化和配置MotionFX库，使用FIFO读取传感器数据，FIFO可以作为数据缓冲区，存储传感器的临时数据。这样可以防止数据丢失，特别是在处理器忙于其他任务时，并利用这些数据进行空间坐标的解析。本章案例使用上节的demo进行修改。 首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。 主控为STM32H503CB，陀螺仪为LSM6DSOW，磁力计为LIS2MDL。 MotionFX文件 主要包含lsm6dso_app.c和lsm6dso_app.h，这两个文件主要负责初始化和管理LSM6DSOW传感器的交互。它们提供了配置传感器、初始化通信接口以及读取传感器数据的功能。 该文件包含与LSM6DSOW传感器交互所需函数的实现。它提供了配置传感器、初始化通信接口以及读取传感器数据的功能。 lsm6dso_init(): 初始化MotionFX算法。 lsm6dso_motion_
motionFX ST意法半导体X-MEMS运动融合库
xbw3336661的博客
04-10 582
校准流程：MotionFX_MagCal_init、MotionFX_MagCal_run，直到当isCalibration=True时，可以存下来。欧拉角是NED坐标系（可以在motionFX库中设置坐标系output_type: 0:NED 1:ENU）初始化、磁力校准( osx_MotionFX_compass_Init )、设置各旋钮（knobs）start_automatic_gbias_cal: 自动计算阈值，需保持静止。xxxx_th_sc_6X:静止检测阈值 默认就好。
三轴陀螺仪的四元数解算方法——详细分析
m0_68116022的博客
06-18 2486
为了解决欧拉角在解算角度时出现的“万向锁”现象，本文分析四元数在三轴陀螺仪解算方面的应用。
陀螺仪LSM6DSOW开发(9)-加速度校准
08-11
陀螺仪LSM6DSOW开发(9)----加速度校准 CSDN文字教程：https://blog.csdn.net/qq_24312945/article/details/141004489 B站教学视频：https://www.bilibili.com/video/BV1Yw4m1k7tR/ MotionAC 是 STMicroelectronics ...
陀螺仪LSM6DSOW开发(10)-陀螺仪校准
最新发布
08-27
陀螺仪LSM6DSOW开发(10)----陀螺仪校准 CSDN文字教程：https://blog.csdn.net/qq_24312945/article/details/141337310 B站教学视频：https://www.bilibili.com/video/BV12aW4euEvB/ 在现代电子设备中，陀螺仪传感器...
陀螺仪LSM6DSOW开发(6)-获取磁力计数据
08-03
陀螺仪LSM6DSOW开发(6)----获取磁力计数据 CSDN文字教程：https://blog.csdn.net/qq_24312945/article/details/140870687 B站教学视频：https://www.bilibili.com/video/BV1X4421Z73A/ 本文将介绍如何使用 LIS2MDL ...
陀螺仪LSM6DSOW开发(3)-FIFO数据读取与配置
07-29
陀螺仪LSM6DSOW开发(3)----FIFO数据读取与配置 CSDN文字教程：https://blog.csdn.net/qq_24312945/article/details/140758710 B站教学视频：https://www.bilibili.com/video/BV1Dw4m1r712/ 本文档旨在详细介绍如何...
CAN协议解析
热门推荐
xh1479的博客
10-13 5万+
CAN能够使用多种物理介质作为通信媒介，最常用的是双绞线，还可以使用光纤等材料。CAN总线信号通过两条差分电压信号线CAN_ H和CAN_ L传输，当CAN_ H和CANL信号线上的电压值处于2. 5V左右时表明此时总线处于我们所谓的逻辑电平“1”的状态，也可以称做隐性状态;当CAN_ H比CAN_ L上电压值高时表示逻辑“0”，通常该状态下两条信号线上的电压值为CAN_ H=3. 5V和CAN_ L=1. 5V，此时是显性状态。下图是双绞线CAN总线电平标称值，图中显示了逻辑高电平和逻辑低电平所对应的大
STM32CUBEIDE（14）----外部中断EXTI
xh1479的博客
10-01 5万+
外部中断和事件控制器 (EXTI) 管理外部和内部异步事件 / 中断，并生成相应的事件请求到CPU/ 中断控制器和到电源管理的唤醒请求。本例程主要讲解如何使用外部中断触发LED。
GD32F303固件库开发（15）----外部中断EXTI
xh1479的博客
10-07 5万+
本章配置GD32F303使用EXTI进行驱动LED。
GD32F303固件库开发（16）----移植兆易创新SPI Nor Flash之GD25Q64Flash
xh1479的博客
10-09 5万+
移植兆易创新SPI Nor Flash之GD25Q64Flash
STM32CUBEMX开发GD32F303（15）----外部中断EXTI
xh1479的博客
10-07 5万+
本章STM32CUBEMX配置STM32F103，并且在GD32F303中进行开发，同时通过开发板内进行验证。使用外部中断EXTI进行驱动LED。
STM32CUBEIDE（15）----移植兆易创新SPI Nor Flash之GD25Q64Flash
xh1479的博客
10-07 4万+
SPI是串行外设接口（Serial Peripheral Interface）的缩写，是一种高速的，全双工，同步的通信总线，并且在芯片的管脚上只占用四根线，节约了芯片的管脚，同时为PCB的布局上节省空间，提供方便，正是出于这种简单易用的特性，越来越多的芯片集成了这种通信协议，比如 EEPROM，FLASH，实时时钟，AD转换器。 W25Q64 是一款SPI接口的Flash芯片，其存储空间为 64Mbit，相当于8M字节。W25Q64可以支持 SPI 的模式 0 和模式 3，也就是 CPOL=0/CPHA=0
STM32CUBEMX开发GD32F303（16）----移植兆易创新SPI Nor Flash之GD25Q64Flash
xh1479的博客
10-07 4万+
STM32CUBEMX开发GD32F303（16）----移植兆易创新SPI Nor Flash之GD25Q64Flash
GD32F303固件库开发
xh1479的博客
11-02 4万+
GD32F303固件库开发
STM32CUBEIDE----配置输出串口中文和文本大小
xh1479的博客
10-22 4万+
关于STM32CUBEIDE无法通过串口打印中文字符，需要更改编码配置。点击proiect->Properties编码格式输入GMK。此时就可以正确输出中文字符。
arduino(2)--通过ESP8266模块实现串口接收数据，使用TCP协议进行局域网通信
xh1479的博客
10-16 4万+
arduino--通过ESP8266模块实现串口接收数据，使用TCP协议进行局域网通信硬件准备端口连接网络调试助手代码功能说明最后 由于调试机器需要打印数据来查看程序是否正常运行，将MCU的串口线接入到电脑需要很长的延长线，而且机器的移动会变的很不方便，故使用8266进行局域网通信，通过wifi将数据直接发送到电脑，这样调试起来更加的灵活。 硬件准备 首先需要准备一个开发板，这里我准备的是淘宝购买的兼容arduino的ESP8266模块。 具体的配置在我博客上章有讲，链接如下：ESP8266配置 端口连接
STM32CUBEMX(3)--ADC通过DMA方式采样，USART打印
xh1479的博客
06-04 4万+
STM32CUBEME--3ADC通过DMA方式采样，USART打印概述硬件准备选择芯片型号配置时钟源配置时钟树串口配置ADC配置生成工程设置生成代码配置keil代码演示效果最后 概述 直接存储器访问（DMA)用于在外设与存储器之间以及存储器与存储器之间提供高速数据传输。可以在无需任何CPU操作的情况下通过DMA快速传输传输。这样节省的CPU资源可供其它操作使用。 DMA允许在后台执行数据传输，无需Cortex-MO处理器干预。在此操作过程中，主处理器可以执行其它任务，仅当整个数据块需要处理时，才会中断主处
STM32CUBEMX生成freeRTOS代码的时候出现警告
xh1479的博客
06-19 3万+
STM32CUBEME生成freeRTOS代码的时候出现警告概述 概述 最近做新项目使用了FREERTOS.1，执行生成代码时提示： WARNINGS: When FreeRTOS is used, it is strongly recommanded to use HAL timebase source other than the Systick. HAL函数如果是阻塞型呼叫，內部会用到HAL_Delay()，FreeRTOS应该还是使用SystTick。如果使用的时基操作來源一样，怕有不可预期问题
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

记帖

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值