驱动LSM6DS3TR-C实现高效运动检测与数据采集(7)----MotionFX库解析空间坐标

记帖

已于 2024-07-13 01:51:08 修改

阅读量7.8k

点赞数 15

分类专栏：传感器 stm32cube 单片机文章标签： LSM6DS3TR-C MotionFX STM32CUBEMX MEMS 欧拉角四元数 stm32h503

于 2024-07-02 00:12:31 首次发布

本文链接：https://blog.csdn.net/qq_24312945/article/details/140112859

版权

单片机同时被 3 个专栏收录

260 篇文章 157 订阅

订阅专栏

stm32cube

140 篇文章 60 订阅

订阅专栏

传感器

96 篇文章 3 订阅

订阅专栏

驱动LSM6DS3TR-C实现高效运动检测与数据采集.7--MotionFX库解析空间坐标

概述
视频教学
样品申请
源码下载
开启CRC
串口设置
开启X-CUBE-MEMS1
设置加速度和角速度量程
速率选择
设置FIFO速率
初始化定义
MotionFX文件
卡尔曼滤波算法
主程序执行流程
lsm6ds3tr_c_motion_fx_determin
欧拉角简介
演示

概述

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

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

在这里插入图片描述

视频教学

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

驱动LSM6DS3TR-C实现高效运动检测与数据采集(7)----MotionFX库解析空间坐标

样品申请

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

源码下载

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

开启CRC

在这里插入图片描述

串口设置

设置串口速率为2000000。

在这里插入图片描述

开启X-CUBE-MEMS1

在这里插入图片描述

设置加速度和角速度量程

这里设置加速度量程为4g和角速度为4000dps。

  /* 设置加速度计和陀螺仪的满量程范围 */
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_4g);
  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps);

速率选择

加速度和角速度的速率尽量大于100Hz。
在这里插入图片描述
XL_HM_MODE 位定义了加速度计的电源模式，可以在高性能模式和低功耗模式之间切换。这有助于在不同应用场景下优化传感器的功耗和性能。对于高速率，需要配置为高性能模式。

	lsm6ds3tr_c_xl_power_mode_set(&dev_ctx, LSM6DS3TR_C_XL_HIGH_PERFORMANCE);	
  /* 设置加速度计和陀螺仪的输出数据速率：
   * 在本例中，我们将加速度计和陀螺仪的速率设置为26 Hz
   */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_208Hz);
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_208Hz);

设置FIFO速率

LSM6DS3TR-C传感器的FIFO控制寄存器3（FIFO_CTRL3）的内容，该寄存器用于选择陀螺仪和加速度计数据写入FIFO的批处理数据速率（BDR，Batch Data Rate）。以下是详细描述：
FIFO_CTRL3寄存器（地址09h）,该寄存器包含两个主要字段：
● DEC_FIFO_GYRO [2:0]：选择陀螺仪数据的批处理速率。
● DEC_FIFO_XL [2:0]：选择加速度计数据的批处理速率。

在这里插入图片描述

  /* 设置FIFO传感器的降采样因子 */
  lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_XL_NO_DEC);
  lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_GY_NO_DEC);

  /* 设置FIFO的输出数据速率 */
	//FIFO_CTRL5 (0Ah)
  lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, LSM6DS3TR_C_FIFO_208Hz);

初始化定义

  /* 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);

	
  /* Initialize mems driver interface */
  stmdev_ctx_t dev_ctx;
  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 */
  whoamI = 0;
  lsm6ds3tr_c_device_id_get(&dev_ctx, &whoamI);
	printf("LSM6DS3TR-C_ID=0x%x,whoamI=0x%x",LSM6DS3TR_C_ID,whoamI);
  if ( whoamI != LSM6DS3TR_C_ID )
    while (1); /*manage here device not found */

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

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



  /* 设置加速度计和陀螺仪的满量程范围 */
  lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_4g);
  lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps);
  /* 启用块数据更新（BDU），当FIFO支持时 */
  lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

	lsm6ds3tr_c_xl_power_mode_set(&dev_ctx, LSM6DS3TR_C_XL_HIGH_PERFORMANCE);	
  /* 设置加速度计和陀螺仪的输出数据速率：
   * 在本例中，我们将加速度计和陀螺仪的速率设置为26 Hz
   */
  lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_208Hz);
  lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_208Hz);
	
	
  /* 设置FIFO水印为模式的倍数
   * 在本例中，我们将水印设置为10个模式
   * 这意味着10个序列：
   * (陀螺仪 + 加速度计) = 12字节
   * (外部传感器 + 时间戳) = 12字节
   */
	lsm6ds3tr_c_int1_route_t int_1_reg;
  uint16_t pattern_len = 24;  // 每个数据集由6个字节组成,4*6=24
  lsm6ds3tr_c_fifo_watermark_set(&dev_ctx, 10 * pattern_len);

	
  /* 将FIFO模式设置为流模式 */
	//FIFO_CTRL5(0x0A)->STREAM_MODE
  lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_STREAM_MODE);

  /* 启用时间戳并将其添加到FIFO */
	//CTRL10_C (19h)->TIMER_EN
  lsm6ds3tr_c_timestamp_set(&dev_ctx, PROPERTY_ENABLE);
	//CTRL10_C (19h)->PEDO_EN	
	lsm6ds3tr_c_pedo_sens_set(&dev_ctx, PROPERTY_ENABLE); // 根据需求配置步数计数


  /* 将时间戳分辨率设置为25 μs (WAKE_UP_DUR寄存器中的TIMER_HR位) */
	//WAKE_UP_DUR (5Ch)->TIMER_HR
	lsm6ds3tr_c_timestamp_res_set(&dev_ctx, LSM6DS3TR_C_LSB_25us);

	//设置第3数据集（Dataset 3）的降采样因子
		lsm6ds3tr_c_fifo_dataset_3_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_DS3_NO_DEC);	
	//设置第4数据集（Dataset 4）的降采样因子
	//FIFO_CTRL4 (09h)->DEC_DS4_FIFO[2:0]
		lsm6ds3tr_c_fifo_dataset_4_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_DS4_NO_DEC);
		
		
	// 启用时间戳写入FIFO第四数据集
	//FIFO_CTRL2 (07h)->TIMER_PEDO_FIFO_EN
  lsm6ds3tr_c_fifo_pedo_and_timestamp_batch_set(&dev_ctx, PROPERTY_ENABLE);			
		
	
  /* 设置FIFO传感器的降采样因子 */
  lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_XL_NO_DEC);
  lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, LSM6DS3TR_C_FIFO_GY_NO_DEC);

  /* 设置FIFO的输出数据速率 */
	//FIFO_CTRL5 (0Ah)
  lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, LSM6DS3TR_C_FIFO_208Hz);

	lsm6ds3tr_c_init();
  /* USER CODE END 2 */

MotionFX文件

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

卡尔曼滤波算法

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

在这里插入图片描述

对应的demo在2.2.9有提供。

在这里插入图片描述

主程序执行流程

读取FIFO水印标志：
○ 使用 lsm6ds3tr_c_fifo_wtm_flag_get() 函数读取FIFO水印标志，判断FIFO中的数据是否达到设定的阈值。
处理FIFO数据：
○ 如果FIFO水印标志被设置，读取FIFO中的数据数量。
○ 使用 lsm6ds3tr_c_fifo_raw_data_get() 函数逐项读取FIFO中的传感器数据。
调用姿态估计算法：
○ 当加速度计、陀螺仪和时间戳数据都已读取时，调用 lsm6ds3tr_c_motion_fx_determin() 函数进行姿态估计。
○ 重置标志位并更新时间戳。

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    uint16_t num = 0,num1=0;
    uint16_t num_pattern = 0;
    uint8_t waterm = 0;
    /* 读取LSM6DS3TR-C的水印标志 */
    lsm6ds3tr_c_fifo_wtm_flag_get(&dev_ctx, &waterm);		
    if (waterm) {
      /* 读取FIFO中的字数 */
      lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &num);
      num_pattern = num / 24*2;
			printf("num=%d\n",num);
      while (num_pattern-- > 0) {
				printf ("num1=%d\n",num1);
				num1++;
        /* 根据传感器的ODR配置，FIFO模式由以下样本序列组成：GYRO, XL 外部传感器 时间戳*/
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  data_raw_angular_rate.u8bit,
                                  3 * sizeof(int16_t));
        gyr_x =
          lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[0]);
        gyr_y =
          lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[1]);
        gyr_z=
          lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[2]);
//        printf(
//                "Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
//                gyr_x, gyr_y, gyr_z);
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  data_raw_acceleration.u8bit,
                                  3 * sizeof(int16_t));
        acc_x =
          lsm6ds3tr_c_from_fs4g_to_mg(data_raw_acceleration.i16bit[0]);
        acc_y =
          lsm6ds3tr_c_from_fs4g_to_mg(data_raw_acceleration.i16bit[1]);
        acc_z =
          lsm6ds3tr_c_from_fs4g_to_mg(data_raw_acceleration.i16bit[2]);
//        printf("Acc [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
//                acc_x, acc_y, acc_z);
				//外部传感器数据				
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                  data_raw_none.u8bit,
                                  3 * sizeof(int16_t));		
        // 打印外部传感器数据
//        printf("External sensor data: %02x %02x %02x %02x %02x %02x\r\n",
//               data_raw_none.u8bit[0], data_raw_none.u8bit[1], data_raw_none.u8bit[2],
//               data_raw_none.u8bit[3], data_raw_none.u8bit[4], data_raw_none.u8bit[5]);


        /* 读取时间戳数据 */
        uint32_t timestamp=0;
        lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx,
                                      data_raw_Timestamp.u8bit,
                                      3*sizeof(int16_t));//
					timestamp=(data_raw_Timestamp.u8bit[1]<<16)|(data_raw_Timestamp.u8bit[0]<<8)
						|(data_raw_Timestamp.u8bit[3]);
//        printf("Timestamp: %u\r\n", timestamp);		
				if(deltatime_first==0)//第一次
					{
						deltatime_1=timestamp;
						deltatime_2=timestamp;
						deltatime_first=1;
					}
					else
					{
						deltatime_2=timestamp;
					}

					lsm6ds3tr_c_motion_fx_determin();
//					acc_flag=0;
//					gyr_flag=0;
//					deltatime_flag=0;
					deltatime_1=deltatime_2;	

								
      }
    }		
		
		
    /* USER CODE END WHILE */

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

lsm6ds3tr_c_motion_fx_determin

● 外部变量声明：
○ acc_x, acc_y, acc_z: 加速度计数据。
○ gyr_x, gyr_y, gyr_z: 陀螺仪数据。
○ deltatime_1, deltatime_2: 时间戳数据。
○ out_num: 输出计数器。
● 读取并存储传感器数据：
○ 将全局变量中的加速度计和陀螺仪数据存储到 sensor_hub_data 结构体中。
● 准备 MotionFX 输入数据：
○ 将读取到的加速度计和陀螺仪数据转换为 MotionFX 库所需的单位（g 和 dps），并存储在 mfx_data_in 结构体中。
○ 初始化磁力计数据为 0。
● 计算时间差：
○ 计算两个时间戳之间的差值（单位：秒），并存储在 delta_time 数组中。
● 卡尔曼滤波算法：
○ 使用 MotionFX_propagate 函数运行卡尔曼滤波传播算法。
○ 使用 MotionFX_update 函数更新卡尔曼滤波器。

extern	float acc_x,acc_y,acc_z;
extern	float gyr_x,gyr_y,gyr_z;
extern uint32_t deltatime_1,deltatime_2;



void lsm6ds3tr_c_motion_fx_determin(void){
	


	sensor_hub_data.acceleration[0]=acc_x;
	sensor_hub_data.acceleration[1]=acc_y;
	sensor_hub_data.acceleration[2]=acc_z;

	sensor_hub_data.angular_rate[0]=gyr_x;
	sensor_hub_data.angular_rate[1]=gyr_y;
	sensor_hub_data.angular_rate[2]=gyr_z;	
	
	/*----------------------------------------------------------------------------------
	  
													  fx motion 移动算法（卡尔曼滤波）
	
		----------------------------------------------------------------------------------*/
	MFX_input_t mfx_data_in;

	/* MotionFX 算法库，计算四元数，参考自 AlgoBuilded 生成代码 */
	mfx_data_in.acc[0] = sensor_hub_data.acceleration[0] * FROM_MG_TO_G;
	mfx_data_in.acc[1] = sensor_hub_data.acceleration[1] * FROM_MG_TO_G;
	mfx_data_in.acc[2] = sensor_hub_data.acceleration[2] * FROM_MG_TO_G;

	mfx_data_in.gyro[0] = sensor_hub_data.angular_rate[0] * FROM_MDPS_TO_DPS;
	mfx_data_in.gyro[1] = sensor_hub_data.angular_rate[1] * FROM_MDPS_TO_DPS;
	mfx_data_in.gyro[2] = sensor_hub_data.angular_rate[2] * FROM_MDPS_TO_DPS;

	mfx_data_in.mag[0] = 0;
	mfx_data_in.mag[1] = 0;
	mfx_data_in.mag[2] = 0; 
	
//	printf("Acceleration [mg]:\t%4.2f \t%4.2f \t%4.2f\r\n",mfx_data_in.acc[0], 
//																		mfx_data_in.acc[1], mfx_data_in.acc[2]);
																		
	

	/* 跟传感器输出速率ODR相关，△time 要比采样周期小 */
//		float delta_time = DELATE_TIME;
	float delta_time[1];
	if(deltatime_2>deltatime_1)
	{
		delta_time[0]=(float)((double)(deltatime_2-deltatime_1)*25.0f/1000000);
//		printf("d=%f\n",delta_time[0]);
		/* 运行卡尔曼滤波传播算法 */
		MotionFX_propagate(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, delta_time);
		/* 更新卡尔曼滤波器 */
		MotionFX_update(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, delta_time, NULL);
	}
	else if(deltatime_1>deltatime_2)
	{
		delta_time[0]=(float)((double)(0xffffff-deltatime_2+deltatime_1)*25.0f/1000000);	
		/* 运行卡尔曼滤波传播算法 */
		MotionFX_propagate(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, delta_time);
		/* 更新卡尔曼滤波器 */
		MotionFX_update(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, delta_time, NULL);		
	}
	else if(deltatime_1==deltatime_2)
	{
		delta_time[0]=0.0f;	
	}
//	/* 运行卡尔曼滤波传播算法 */
//	MotionFX_propagate(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, &delta_time);
//	/* 更新卡尔曼滤波器 */
//	MotionFX_update(mfxstate_6x, &sensor_hub_data.mfx_6x, &mfx_data_in, &delta_time, NULL);

	/* 将四元数存储到数组，方便后续操作 */
//	Quaternions_data[0] = sensor_hub_data.mfx_6x.quaternion[0];
//	Quaternions_data[1] = sensor_hub_data.mfx_6x.quaternion[1];
//	Quaternions_data[2] = sensor_hub_data.mfx_6x.quaternion[2];
//	Quaternions_data[3] = sensor_hub_data.mfx_6x.quaternion[3];

	/* 按照 VOFA+ 的 FireWater 数据协议格式，输出四元数数据 */
	/* 斜视图 右前上视角：scalar | x | y | z */
//	printf("%f, %f, %f, %f \n",Quaternions_data[3],\
						Quaternions_data[1],Quaternions_data[2],Quaternions_data[0]);
	
	printf("%f, %f, %f\n",sensor_hub_data.mfx_6x.rotation[0],
					sensor_hub_data.mfx_6x.rotation[1],sensor_hub_data.mfx_6x.rotation[2]);
}

欧拉角简介

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

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

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

演示

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

在这里插入图片描述

逆时针旋转90°

在这里插入图片描述

逆时针旋转180°

在这里插入图片描述

逆时针旋转270°

在这里插入图片描述

记帖

关注

15
点赞
踩
22

收藏

觉得还不错? 一键收藏
打赏
0
评论
驱动LSM6DS3TR-C实现高效运动检测与数据采集(7)----MotionFX库解析空间坐标

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

扫一扫