陀螺仪LSM6DSV16X与AI集成(12)----SFLP获取四元数

嵌入式单片机MCU开发 2024-09-06 1096

嵌入式单片机MCU开发

300 文章 49.2w阅读 100粉丝

描述

概述

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

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

陀螺仪

视频教学

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

样品申请

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

源码下载

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

硬件准备

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

主控为STM32H503CB，陀螺仪为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配置

陀螺仪

LSM6DSV16X最大IIC通讯速率为1M。

陀螺仪

配置IIC速度为1M

陀螺仪

CS和SA0设置

陀螺仪

陀螺仪

由于还有一个磁力计，需要把该CS也使能。

陀螺仪

陀螺仪

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

初始化SFLP

开启嵌入式函数访问需要向 FUNC_CFG_ACCESS (01h)的EMB_FUNC_REG_ACCESS写入1进行开启。 陀螺仪

陀螺仪

/**
  * @brief  Change memory bank.[set]
  *
  * @param  ctx      read / write interface definitions
  * @param  val      MAIN_MEM_BANK, EMBED_FUNC_MEM_BANK,
  * @retval          interface status (MANDATORY: return 0 - > no Error)
  *
  */
int32_t lsm6dsv16x_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t val)
{
  lsm6dsv16x_func_cfg_access_t func_cfg_access;
  int32_t ret;

  ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1);
  if (ret != 0) { return ret; }

  func_cfg_access.shub_reg_access = ((uint8_t)val & 0x02U) > > 1;
  func_cfg_access.emb_func_reg_access = (uint8_t)val & 0x01U;
  ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1);

  return ret;
}

SFLP_GAME_FIFO_EN 是 LSM6DSV16X 传感器中 EMB_FUNC_FIFO_EN_A（44h）寄存器的一个设置位。这个特定的位用于控制是否启用将 SFLP（Sensor Fusion Low Power）算法计算出的游戏旋转向量（四元数）值存储到 FIFO（先进先出）缓冲区中的功能。当这个位被设置为 1 时，启用了这个功能，使得算法计算出的游戏旋转向量可以批量存储到 FIFO 缓冲区中。默认值为 0，表示该功能默认是禁用的。

陀螺仪

陀螺仪

LSM6DSV16X 传感器的 FIFO_CTRL4 (0Ah) 寄存器配置信息。这个寄存器控制着 FIFO（先进先出）缓冲区的各种操作和数据批处理（batching）的设置。 连续模式，如果 FIFO 已满，新采集的样本会覆盖旧样本。

陀螺仪

陀螺仪

在AN5763手册中，也说明了融合数据会输出在FIFO中，同时有如下的输出速率，我们可以配置默认的速率。

陀螺仪

陀螺仪

最后对EMB_FUNC_EN_A (04h) 寄存器的SFLP_GAME_EN设置为1。

陀螺仪

陀螺仪

读取四元数数据

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


  // Check the number of samples inside FIFO
  if (status != LSM6DSV16X_OK) {
    printf("LSM6DSV16X Sensor failed to get number of samples inside FIFO");
    while (1);
  }
    fifo_samples = fifo_status.fifo_level;

之后需要通过FIFO_DATA_OUT_TAG (78h)判断是什么数据准备好，当为SFLP game rotation vector(0X13)时候，为四元数准备完毕。

陀螺仪

之后读取FIFO_DATA_OUT_X_L (79h)到FIFO_DATA_OUT_Z_H (7Eh)共6个字节数据，进行四元数读取。

陀螺仪

最后转换为姿态角。

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

审核编辑 黄宇

打开APP阅读更多精彩内容
声明：本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人，不代表电子发烧友网立场。文章及其配图仅供工程师学习之用，如有内容侵权或者其他违规问题，请联系本站处理。 举报投诉

全部0条评论

快来发表一下你的评论吧 !

下载APP
|投诉反馈|电子发烧友网
© 2021 elecfans.com
湘ICP备2023018690号
×
20
完善资料，
赚取积分