陀螺仪LSM6DSV16X与AI集成(8)----MotionFX库解析空间坐标

概述

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

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

视频教学

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

样品申请

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

源码下载

[https://download.csdn.net/download/qq_24312945/89475748]

开启CRC

串口设置

设置串口速率为2000000。

开启X-CUBE-MEMS1

设置加速度和角速度量程

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

/* Set full scale */
  lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_4g);
  lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_4000dps);

速率选择

加速度和角速度的速率尽量大于100Hz。

设置FIFO速率

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

将加速度计的数据速率（Output Data Rate, ODR）设置为60Hz。这意味着加速度计的数据将以每秒60次的频率批量写入FIFO。
将陀螺仪的数据速率设置为15Hz。这意味着陀螺仪的数据将以每秒15次的频率批量写入FIFO。

/* Set FIFO batch XL/Gyro ODR to 12.5Hz */
  lsm6dsv16x_fifo_xl_batch_set(&dev_ctx, LSM6DSV16X_XL_BATCHED_AT_480Hz);
  lsm6dsv16x_fifo_gy_batch_set(&dev_ctx, LSM6DSV16X_GY_BATCHED_AT_480Hz);

设置FIFO时间戳批处理速率

LSM6DSV16X传感器的时间戳批处理速率、温度数据批处理速率、增强的EIS陀螺仪输出批处理，以及FIFO的工作模式。这些配置确保传感器数据能够以适当的速率和模式进行批处理和存储，以满足不同的应用需求。

/* Set Output Data Rate */
  lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_480Hz);
  lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_480Hz);
  lsm6dsv16x_fifo_timestamp_batch_set(&dev_ctx, LSM6DSV16X_TMSTMP_DEC_1);

配置过滤链

lsm6dsv16x_filt_gy_lp1_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSV16X_GY_ULTRA_LIGHT);
  lsm6dsv16x_filt_xl_lp2_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSV16X_XL_STRONG);

初始化定义

/* USER CODE BEGIN 2 */
    printf("HELLO!n");
  HAL_GPIO_WritePin(CS_GPIO_Port, CS_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(SA0_GPIO_Port, SA0_Pin, GPIO_PIN_RESET);
    HAL_Delay(100);
//lsm6dsdtr_init();

  lsm6dsv16x_fifo_status_t fifo_status;
  stmdev_ctx_t dev_ctx;
  lsm6dsv16x_reset_t rst;

  /* 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(dev_ctx.handle);
  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);

  /* Check device ID */
  lsm6dsv16x_device_id_get(&dev_ctx, &whoamI);
    printf("LSM6DSV16X_ID=0x%x,whoamI=0x%x",LSM6DSV16X_ID,whoamI);
  if (whoamI != LSM6DSV16X_ID)
    while (1);

  /* Restore default configuration */
  lsm6dsv16x_reset_set(&dev_ctx, LSM6DSV16X_RESTORE_CTRL_REGS);
  do {
    lsm6dsv16x_reset_get(&dev_ctx, &rst);
  } while (rst != LSM6DSV16X_READY);

  /* Enable Block Data Update */
  lsm6dsv16x_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

  /* Set full scale */
  lsm6dsv16x_xl_full_scale_set(&dev_ctx, LSM6DSV16X_4g);
  lsm6dsv16x_gy_full_scale_set(&dev_ctx, LSM6DSV16X_4000dps);

  /*
   * Set FIFO watermark (number of unread sensor data TAG + 6 bytes
   * stored in FIFO) to FIFO_WATERMARK samples
   */
  lsm6dsv16x_fifo_watermark_set(&dev_ctx, FIFO_WATERMARK);
  /* Set FIFO batch XL/Gyro ODR to 12.5Hz */
  lsm6dsv16x_fifo_xl_batch_set(&dev_ctx, LSM6DSV16X_XL_BATCHED_AT_480Hz);
  lsm6dsv16x_fifo_gy_batch_set(&dev_ctx, LSM6DSV16X_GY_BATCHED_AT_480Hz);

  /* Set FIFO mode to Stream mode (aka Continuous Mode) */
  lsm6dsv16x_fifo_mode_set(&dev_ctx, LSM6DSV16X_STREAM_MODE);

  /* Set Output Data Rate */
  lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_480Hz);
  lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_480Hz);
  lsm6dsv16x_fifo_timestamp_batch_set(&dev_ctx, LSM6DSV16X_TMSTMP_DEC_1);
  lsm6dsv16x_timestamp_set(&dev_ctx, PROPERTY_ENABLE);


  lsm6dsv16x_filt_gy_lp1_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSV16X_GY_ULTRA_LIGHT);
  lsm6dsv16x_filt_xl_lp2_set(&dev_ctx, PROPERTY_ENABLE);
  lsm6dsv16x_filt_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSV16X_XL_STRONG);    


//  lsm6dsv16x_pin_int_route_t pin_int;
//  pin_int.fifo_th = PROPERTY_ENABLE;
//  lsm6dsv16x_pin_int1_route_set(&dev_ctx, &pin_int);


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

MotionFX文件

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

卡尔曼滤波算法

运行卡尔曼滤波传播算法MotionFX_propagate。

根据需要更新卡尔曼滤波器MotionFX_update。

需要注意的是这2各算法非常吃资源，需要注意MCU算力分配。

对应的demo在2.2.9有提供。

主程序执行流程

读取FIFO水印标志：
○ 使用 lsm6dsv16x_fifo_status_get() 函数读取FIFO水印标志，判断FIFO中的数据是否达到设定的阈值。
处理FIFO数据：
○ 如果FIFO水印标志被设置，读取FIFO中的数据数量。
○ 切换LED状态，用于指示数据读取状态。
○ 使用 lsm6dsv16x_fifo_out_raw_get() 函数逐项读取FIFO中的传感器数据。
○ 根据数据标签（tag）识别数据类型并处理：
■ 加速度计数据：设置 acc_flag 标志位，并转换数据单位。
■ 陀螺仪数据：设置 gyr_flag 标志位，并转换数据单位。
■ 时间戳数据：设置 deltatime_flag 标志位，并计算时间差。
调用姿态估计算法：
○ 当加速度计、陀螺仪和时间戳数据都已读取时，调用 lsm6dsv16x_motion_fx_determin() 函数进行姿态估计。
○ 重置标志位并更新时间戳。

/* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    uint16_t num = 0;
    /* Read watermark flag */
    lsm6dsv16x_fifo_status_get(&dev_ctx, &fifo_status);
        uint8_t acc_flag=0,gyr_flag=0;//加速度角速度标志位
        uint8_t deltatime_flag=0;//时间标志位
    if (fifo_status.fifo_th == 1) {
      num = fifo_status.fifo_level;
//      printf( "-- FIFO num %d rn", num);

      while (num--) {
        lsm6dsv16x_fifo_out_raw_t f_data;

        /* Read FIFO sensor value */
        lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);
        datax = (int16_t *)&f_data.data[0];
        datay = (int16_t *)&f_data.data[2];
        dataz = (int16_t *)&f_data.data[4];
        ts = (int32_t *)&f_data.data[0];

        switch (f_data.tag) {
        case LSM6DSV16X_XL_NC_TAG:
                    acc_flag=1;
                    acc_x=lsm6dsv16x_from_fs4_to_mg(*datax);
                    acc_y=lsm6dsv16x_from_fs4_to_mg(*datay);
                    acc_z=lsm6dsv16x_from_fs4_to_mg(*dataz);
//          printf( "ACC [mg]:t%4.2ft%4.2ft%4.2frn",
//                  lsm6dsv16x_from_fs4_to_mg(*datax),
//                  lsm6dsv16x_from_fs4_to_mg(*datay),
//                  lsm6dsv16x_from_fs4_to_mg(*dataz));
          break;
        case LSM6DSV16X_GY_NC_TAG:
                    gyr_flag=1;
                    gyr_x=lsm6dsv16x_from_fs4000_to_mdps(*datax);
                    gyr_y=lsm6dsv16x_from_fs4000_to_mdps(*datay);
                    gyr_z=lsm6dsv16x_from_fs4000_to_mdps(*dataz);            
//          printf("GYR [mdps]:t%4.2ft%4.2ft%4.2frn",
//                  lsm6dsv16x_from_fs4000_to_mdps(*datax),
//                  lsm6dsv16x_from_fs4000_to_mdps(*datay),
//                  lsm6dsv16x_from_fs4000_to_mdps(*dataz));
          break;
        case LSM6DSV16X_TIMESTAMP_TAG:
                    deltatime_flag=1;
                    if(deltatime_first==0)//第一次
                    {
                        deltatime_1=*ts;
                        deltatime_2=deltatime_1;
                        deltatime_first=1;
                    }
                    else
                    {
                        deltatime_2=*ts;
                    }

//          printf( "TIMESTAMP [ms] %drn", *ts);

          break;
        default:
          break;
        }
                if(acc_flag&&gyr_flag&&deltatime_flag)
                {
                    lsm6dsv16x_motion_fx_determin();
                    acc_flag=0;
                    gyr_flag=0;
                    deltatime_flag=0;
                    deltatime_1=deltatime_2;            
                }    
      }
//      printf("------ rnrn");
    }
//            HAL_Delay(10);

    /* USER CODE END WHILE */

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

lsm6dsv16x_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;
extern int out_num;
void lsm6dsv16x_motion_fx_determin(void){

//    lsm6dsv16x_data_ready_t drdy;
//    
//    /* Read output only if new xl value is available */
//  lsm6dsv16x_flag_data_ready_get(&dev_ctx, &drdy);
//    
//    if (drdy.drdy_xl){
//        /* Read acceleration field data */
//        memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
//        
//        lsm6dsv16x_acceleration_raw_get(&dev_ctx, 
//                                                        data_raw_acceleration);
//        
//        sensor_hub_data.acceleration[0] =    lsm6dsv16x_from_fs2_to_mg(
//                                                        data_raw_acceleration[0]);
//        sensor_hub_data.acceleration[1] =    lsm6dsv16x_from_fs2_to_mg(
//                                                        data_raw_acceleration[1]);
//        sensor_hub_data.acceleration[2] =    lsm6dsv16x_from_fs2_to_mg(
//                                                        data_raw_acceleration[2]);
//    }
//    
//    if (drdy.drdy_gy){
//        memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));

//        /* 读取角速度数据,并将 角速度 转换为 dps */
//        lsm6dsv16x_angular_rate_raw_get(&dev_ctx,
//                                                                         data_raw_angular_rate);
//        sensor_hub_data.angular_rate[0] = lsm6dsv16x_from_fs2000_to_mdps(
//                                                         data_raw_angular_rate[0]);
//        sensor_hub_data.angular_rate[1] = lsm6dsv16x_from_fs2000_to_mdps(
//                                                         data_raw_angular_rate[1]);
//        sensor_hub_data.angular_rate[2] = lsm6dsv16x_from_fs2000_to_mdps(
//                                                         data_raw_angular_rate[2]);
//    }
//    
    // 将全局变量中的加速度计和陀螺仪数据存储在 sensor_hub_data 结构体中
    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.2frn",mfx_data_in.acc[0], 
//                                                                        mfx_data_in.acc[1], mfx_data_in.acc[2]);



    /* 跟传感器输出速率ODR相关，delta_time为2次数据的间隔 */
//        float delta_time = DELATE_TIME;
    float delta_time[1];
    if(deltatime_2 >deltatime_1)
    {
        delta_time[0]=(float)((double)(deltatime_2-deltatime_1)*21.75f/1000000);
//        printf("d=%fn",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)(0xffffffff-deltatime_2+deltatime_1)*21.75f/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]);
    if(out_num< 10)// 每10次输出一次旋转数据
        out_num++;
    else
    {
    printf("%f, %f, %fn",sensor_hub_data.mfx_6x.rotation[0],
                    sensor_hub_data.mfx_6x.rotation[1],sensor_hub_data.mfx_6x.rotation[2]);
        out_num=0;
    }
}

欧拉角简介

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

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

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

演示

初始位置和数据输出如下所示。

逆时针旋转90°

逆时针旋转180°

逆时针旋转270°

审核编辑 黄宇