STM32H5开发陀螺仪LSM6DSV16X(3)----SFLP获取四元数

概述

在现代的运动跟踪和姿态检测应用中，低功耗、高精度的传感器数据融合处理变得越来越重要。LSM6DSV16X传感器集成了SFLP（Sensor Fusion Low Power）算法模块，可以在低功耗模式下实现六轴传感器数据的高效融合。SFLP模块通过处理加速度计和陀螺仪的数据，生成一个表示设备姿态的四元数，这为游戏、增强现实（AR）、虚拟现实（VR）等应用中的精准运动追踪提供了技术支持。在本文中，我们将深入探讨如何利用SFLP模块获取四元数数据，并分析其在实际应用中的优势和实现方法。

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

视频教学

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

样品申请

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

源码下载

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

硬件准备

首先需要准备一个开发板，这里我准备的是自己绘制的开发板，需要的可以进行申请。

主控为STM32H503CB，陀螺仪为LSM6DSV16X，磁力计为LIS2MDL。

参考程序

https://github.com/CoreMaker-lab/STM32H503_LSM6DSV16X_LIS2MDL

https://gitee.com/CoreMaker/STM32H503_LSM6DSV16X_LIS2MDL

SFLP

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 缓冲区是一种数据存储方式，允许临时存储传感器数据。这对于有效管理数据流非常有用，特别是在数据处理可能不如数据收集那么快的系统中。

图片包含了关于 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 功能在不同情况下的性能，包括静态精度、低动态精度和高动态精度，以及校准时间和方向稳定时间。这些参数反映了传感器在不同运动状态下的精确度和响应速度。

生成STM32CUBEMX

用STM32CUBEMX生成例程，这里使用MCU为STM32H503CB。
配置时钟树，配置时钟为250M。

串口配置

查看原理图，PA9和PA10设置为开发板的串口。

配置串口，速率为2000000。

IIC配置

配置IIC为快速模式，速度为400k。

CS和SA0设置

ICASHE

修改堆栈

串口重定向

打开魔术棒，勾选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/STMicroelectronics/lsm6dsv16x-pid

初始换管脚

由于需要向LSM6DSV16X_I2C_ADD_L写入以及为IIC模式。

所以使能CS为高电平，配置为IIC模式。 配置SA0为高电平。

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


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

获取ID

可以向WHO_AM_I (0Fh)获取固定值，判断是否为0x70。

lsm6dsv16x_device_id_get为获取函数。

对应的获取ID驱动程序,如下所示。

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

复位操作

可以向CTRL3 (12h)的SW_RESET寄存器写入1进行复位。

lsm6dsv16x_reset_set为重置函数。

对应的驱动程序,如下所示。

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

BDU设置

在很多传感器中，数据通常被存储在输出寄存器中，这些寄存器分为两部分：MSB和LSB。这两部分共同表示一个完整的数据值。例如，在一个加速度计中，MSB和LSB可能共同表示一个加速度的测量值。
连续更新模式（BDU = ‘0’）：在默认模式下，输出寄存器的值会持续不断地被更新。这意味着在你读取MSB和LSB的时候，寄存器中的数据可能会因为新的测量数据而更新。这可能导致一个问题：当你读取MSB时，如果寄存器更新了，接下来读取的LSB可能就是新的测量值的一部分，而不是与MSB相对应的值。这样，你得到的就是一个“拼凑”的数据，它可能无法准确代表任何实际的测量时刻。
块数据更新（BDU）模式（BDU = ‘1’）：当激活BDU功能时，输出寄存器中的内容不会在读取MSB和LSB之间更新。这就意味着一旦开始读取数据（无论是先读MSB还是LSB），寄存器中的那一组数据就被“锁定”，直到两部分都被读取完毕。这样可以确保你读取的MSB和LSB是同一测量时刻的数据，避免了读取到代表不同采样时刻的数据。
简而言之，BDU位的作用是确保在读取数据时，输出寄存器的内容保持稳定，从而避免读取到拼凑或错误的数据。这对于需要高精度和稳定性的应用尤为重要。
可以向CTRL3 (12h)的BDU寄存器写入1进行开启。

对应的驱动程序,如下所示。

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

设置量程

速率可以通过CTRL1 (10h)设置加速度速率和CTRL2 (11h)进行设置角速度速率。

设置加速度量程可以通过CTRL8 (17h)进行设置。 设置角速度量程可以通过CTRL6 (15h)进行设置。

设置加速度和角速度的量程和速率可以使用如下函数。

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

初始化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 游戏旋转向量特性，确保以低功耗模式运行。

FIFO 阈值

设置 FIFO 的“水位阈值”，即 当 FIFO 中存储的数据量 ≥ 该阈值时，传感器会触发 Watermark 中断（如果配置了中断）。
表示当 FIFO 中累积了FIFO_WATERMARK(32)时，就会满足“水位条件”。

LSM6DSV16X 的 FIFO_CTRL1（0x07）寄存器，是设置 FIFO 水位阈值（watermark threshold） 的关键寄存器。

1 LSB 表示 1个样本，每个样本由：

● 1 byte TAG（标识是哪类数据，如 GYRO/ACC）

● 6 byte 数据（例如三轴陀螺仪 X/Y/Z，每轴2字节）

设置了 FIFO 的“容量警戒线”为 32 个样本，等于 224 字节，一旦数据满到这个程度，FIFO_WTM_IA 标志就会被置位，从而引发中断或被 MCU 轮询识别到，随后就可以读取 FIFO 里的数据了。

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

配置 FIFO 模式

设置 SFLP 模块中要写入 FIFO 的三类输出：

那么 FIFO 中的每个 SFLP 样本，可能是：

● 四元数：QX, QY, QZ, QW

● 重力向量：3 轴重力向量

● 陀螺仪偏置：3 轴陀螺仪偏置

/* Set FIFO batch of sflp data */
  fifo_sflp.game_rotation = 1;
  fifo_sflp.gravity = 1;
  fifo_sflp.gbias = 1;
  lsm6dsv16x_fifo_sflp_batch_set(&dev_ctx, fifo_sflp);

设置FIFO模式为Stream（连续）模式

Stream Mode 是最常用于实时流数据采集的方式 —— 即使 FIFO 满了，也继续采集，不丢失最新数据（会覆盖旧数据）。

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

设置 加速度计 ODR

设置 加速度计 ODR（输出数据率）为 30Hz。

/* Set Output Data Rate */
  lsm6dsv16x_xl_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz);

设置 陀螺仪 ODR

设置 陀螺仪 ODR 为 30Hz。

lsm6dsv16x_gy_data_rate_set(&dev_ctx, LSM6DSV16X_ODR_AT_30Hz);

设定 SFLP 输出速率

设定 SFLP 模块自身的输出速率，控制四元数/重力/陀螺仪偏置等数据输出频率。

lsm6dsv16x_sflp_data_rate_set(&dev_ctx, LSM6DSV16X_SFLP_30Hz);

启用 SFLP 四元数输出

启用 SFLP 的四元数输出。

lsm6dsv16x_sflp_game_rotation_set(&dev_ctx, PROPERTY_ENABLE);

设置陀螺仪偏置（Gyro Bias）值

初始化时设置陀螺仪偏置（Gyro Bias）值，目的是为了提升姿态融合（四元数）算法的准确性，特别适用于你使用的 SFLP（Sensor Fusion Low Power）模块。

/*
   * here application may initialize offset with latest values
   * calculated from previous run and saved to non volatile memory.
   */
  gbias.gbias_x = 0.0f;
  gbias.gbias_y = 0.0f;
  gbias.gbias_z = 0.0f;
  lsm6dsv16x_sflp_game_gbias_set(&dev_ctx, &gbias);

读取四元数数据

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

之后需要通过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 num = 0;

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

    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;
        uint8_t *axis;
        float_t quat[4];
        float_t gravity_mg[3];
        float_t gbias_mdps[3];

        /* Read FIFO sensor value */
        lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);

        switch (f_data.tag) {
        case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
          axis = &f_data.data[0];
          gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0] | (axis[1] < < 8));
          gbias_mdps[1] = lsm6dsv16x_from_fs125_to_mdps(axis[2] | (axis[3] < < 8));
          gbias_mdps[2] = lsm6dsv16x_from_fs125_to_mdps(axis[4] | (axis[5] < < 8));
          printf("GBIAS [mdps]:%4.2ft%4.2ft%4.2frn",
                         (double_t)gbias_mdps[0], (double_t)gbias_mdps[1], (double_t)gbias_mdps[2]);

          break;
        case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
          axis = &f_data.data[0];
          gravity_mg[0] = lsm6dsv16x_from_sflp_to_mg(axis[0] | (axis[1] < < 8));
          gravity_mg[1] = lsm6dsv16x_from_sflp_to_mg(axis[2] | (axis[3] < < 8));
          gravity_mg[2] = lsm6dsv16x_from_sflp_to_mg(axis[4] | (axis[5] < < 8));
          printf("Gravity [mg]:%4.2ft%4.2ft%4.2frn",
                         (double_t)gravity_mg[0], (double_t)gravity_mg[1], (double_t)gravity_mg[2]);

          break;
        case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
          sflp2q(quat, (uint16_t *)&f_data.data[0]);
          printf("[%02x %02x %02x %02x %02x %02x] Game Rotation tX: %2.3ftY: %2.3ftZ: %2.3ftW: %2.3frn",
                   f_data.data[0], f_data.data[1],f_data.data[2],f_data.data[3],f_data.data[4],f_data.data[5],
                   (double_t)quat[0], (double_t)quat[1], (double_t)quat[2], (double_t)quat[3]);


          break;
        default:
         break;
        }
      }

      printf("------ rnrn");

    }        


    /* USER CODE END WHILE */

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

最后转换为姿态角。

/* USER CODE BEGIN WHILE */
  while (1)
  {

    uint16_t num = 0;

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

    if (fifo_status.fifo_th == 1) {
      num = fifo_status.fifo_level;

      sprintf((char *)tx_buffer, "-- FIFO num %d rn", num);

      while (num--) {
        lsm6dsv16x_fifo_out_raw_t f_data;
        int16_t *axis;
        float quat[4];
        float gravity_mg[3];
        float gbias_mdps[3];

        /* Read FIFO sensor value */
        lsm6dsv16x_fifo_out_raw_get(&dev_ctx, &f_data);

        switch (f_data.tag) {
//        case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG:
//          axis = (int16_t *)&f_data.data[0];
//          gbias_mdps[0] = lsm6dsv16x_from_fs125_to_mdps(axis[0]);
//          gbias_mdps[1] = lsm6dsv16x_from_fs125_to_mdps(axis[1]);
//          gbias_mdps[2] = lsm6dsv16x_from_fs125_to_mdps(axis[2]);
//          printf("GBIAS [mdps]:%4.2ft%4.2ft%4.2frn",
//                         (double_t)gbias_mdps[0], (double_t)gbias_mdps[1], (double_t)gbias_mdps[2]);

//          break;
//        case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG:
//          axis = (int16_t *)&f_data.data[0];
//          gravity_mg[0] = lsm6dsv16x_from_sflp_to_mg(axis[0]);
//          gravity_mg[1] = lsm6dsv16x_from_sflp_to_mg(axis[1]);
//          gravity_mg[2] = lsm6dsv16x_from_sflp_to_mg(axis[2]);
//          printf("Gravity [mg]:%4.2ft%4.2ft%4.2frn",
//                         (double_t)gravity_mg[0], (double_t)gravity_mg[1], (double_t)gravity_mg[2]);

//          break;
        case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG:
          sflp2q(quat, (uint16_t *)&f_data.data[0]);
//          printf("Game Rotation tX: %2.3ftY: %2.3ftZ: %2.3ftW: %2.3frn",
//                  (double_t)quat[0], (double_t)quat[1], (double_t)quat[2], (double_t)quat[3]);

                    float sx=quat[1];  
                    float sy=quat[2];  
                    float sz=quat[0];  
                    float sw=quat[3];

                    if (sw< 0.0f) 
                    {
                        sx*=-1.0f;
                        sy*=-1.0f;
                        sz*=-1.0f;
                        sw*=-1.0f;
                    }

                    float sqx = sx * sx;
                    float sqy = sy * sy;
                    float sqz = sz * sz;
                    float euler[3];
                    euler[0] = -atan2f(2.0f* (sy*sw+sx*sz), 1.0f-2.0f*(sqy+sqx));
                    euler[1] = -atan2f(2.0f * (sx*sy+sz*sw),1.0f-2.0f*(sqx+sqz));
                    euler[2] = -asinf(2.0f* (sx*sw-sy*sz));

                    if (euler[0] < 0.0f)
                        euler[0] +=2.0f*3.1415926;

                    for(uint8_t i=0; i< 3; i++){
                            euler[i] = 57.29578 * (euler[i]);
                    }

                    printf("euler[0]=%f,euler[1]=%f,euler[2]=%fn",euler[0],euler[1],euler[2]);

          break;
        default:
         break;
        }
      }

    }        


    /* USER CODE END WHILE */

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

演示

审核编辑 黄宇