概述

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

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

视频教学

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

样品申请

[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=%dn",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.2ft%4.2ft%4.2frn",
//                    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.2ft%4.2ft%4.2frn",
//                                    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=%drn",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°

审核编辑 黄宇